Adds CPP implementatinon
parent
f6e691d3db
commit
c72e925ac2
@ -1,6 +1,10 @@
|
||||
|
||||
set(assignment_SOURCES
|
||||
main.cpp)
|
||||
main.cpp
|
||||
calc.cpp
|
||||
json_output.cpp
|
||||
proto_loader.cpp)
|
||||
|
||||
add_executable(main ${assignment_SOURCES})
|
||||
target_link_libraries(main proto ${PROTOBUF_LIBRARY})
|
||||
target_link_libraries(main PRIVATE proto ${PROTOBUF_LIBRARY})
|
||||
target_link_libraries(main PRIVATE nlohmann_json::nlohmann_json)
|
||||
|
@ -0,0 +1,70 @@
|
||||
#include "calc.hpp"
|
||||
#include <algorithm>
|
||||
|
||||
|
||||
namespace calc {
|
||||
|
||||
|
||||
std::pair<Position, Position>
|
||||
position_segment(std::vector<Position> const& position, double time)
|
||||
{
|
||||
auto second = std::lower_bound(position.begin(), position.end(), time,
|
||||
[](Position const& pos, double t) {
|
||||
return pos.time < t;
|
||||
});
|
||||
if (second == position.begin()) { // corner case
|
||||
second++;
|
||||
}
|
||||
return std::make_pair(*(second-1), *second);
|
||||
}
|
||||
|
||||
|
||||
void interpolate_positions(std::vector<Magnetic>& magnetics, std::vector<Position> const& truth)
|
||||
{
|
||||
for (auto& mag : magnetics) {
|
||||
auto const truth_seg = position_segment(truth, mag.time);
|
||||
for (size_t i = 0; i < mag.position.size(); i++) {
|
||||
mag.position[i] =
|
||||
(truth_seg.second.position[i] - truth_seg.first.position[i]) /
|
||||
(truth_seg.second.time - truth_seg.first.time) *
|
||||
(mag.time - truth_seg.first.time) + truth_seg.first.position[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void crop_temporal_inersection(std::vector<Magnetic>& magnetics, std::vector<Position> const& truth)
|
||||
{
|
||||
auto const mag_first = std::lower_bound(
|
||||
magnetics.begin(), magnetics.end(), truth[0].time,
|
||||
[](Magnetic const& mag, double t) {
|
||||
return mag.time < t;
|
||||
});
|
||||
magnetics.erase(magnetics.begin(), mag_first);
|
||||
auto const mag_last = std::upper_bound(
|
||||
magnetics.begin(), magnetics.end(), truth.back().time,
|
||||
[](double t, Magnetic const& mag) {
|
||||
return t < mag.time;
|
||||
});
|
||||
magnetics.erase(mag_last, magnetics.end());
|
||||
}
|
||||
|
||||
|
||||
std::tuple<double, double, double, double>
|
||||
spatial_extent(std::vector<Magnetic> const& mag)
|
||||
{
|
||||
auto [x_min, x_max] = std::minmax_element(
|
||||
std::cbegin(mag), std::cend(mag),
|
||||
[](Magnetic const& a, Magnetic const& b) {
|
||||
return a.position[0] < b.position[0];
|
||||
});
|
||||
auto [y_min, y_max] = std::minmax_element(
|
||||
std::cbegin(mag), std::cend(mag),
|
||||
[](Magnetic const& a, Magnetic const& b) {
|
||||
return a.position[1] < b.position[1];
|
||||
});
|
||||
return std::make_tuple(x_min->position[0], y_min->position[1], x_max->position[0], y_max->position[1]);
|
||||
}
|
||||
|
||||
|
||||
}
|
@ -0,0 +1,47 @@
|
||||
#ifndef CALC_HPP_
|
||||
#define CALC_HPP_
|
||||
|
||||
#include "data.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <numeric>
|
||||
#include <vector>
|
||||
|
||||
|
||||
namespace calc {
|
||||
|
||||
|
||||
template <typename iter_t>
|
||||
double norm(iter_t first, iter_t last)
|
||||
{
|
||||
return std::sqrt(std::inner_product(first, last, first, 0.0));
|
||||
}
|
||||
|
||||
|
||||
std::pair<Position, Position>
|
||||
position_segment(std::vector<Position> const& position, double time);
|
||||
|
||||
void interpolate_positions(std::vector<Magnetic>& magnetics, std::vector<Position> const& truth);
|
||||
void crop_temporal_inersection(std::vector<Magnetic>& magnetics, std::vector<Position> const& truth);
|
||||
|
||||
std::tuple<double, double, double, double>
|
||||
spatial_extent(std::vector<Magnetic> const& mag);
|
||||
|
||||
template<typename grid_t, typename avg_grid_t>
|
||||
void average_grid(grid_t& grid, avg_grid_t& average_grid)
|
||||
{
|
||||
for (size_t x = 0; x < grid.size_x(); x++) {
|
||||
for (size_t y = 0; y < grid.size_y(); y++) {
|
||||
auto const& cell = grid[x][y];
|
||||
if (cell.size() > 0) {
|
||||
double const sum = std::accumulate(cell.cbegin(), cell.cend(), 0.0);
|
||||
size_t const n = cell.size();
|
||||
average_grid[x][y] = sum / n;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace calc
|
||||
|
||||
#endif // CALC_HPP_
|
@ -0,0 +1,31 @@
|
||||
#ifndef DATA_HPP_
|
||||
#define DATA_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
using Coordinate = std::array<double, 2>;
|
||||
|
||||
|
||||
struct Magnetic {
|
||||
Magnetic(double t, double x, double y, double z) :
|
||||
time(t), field({x, y, z}) {}
|
||||
Magnetic(double t, double x, double y) :
|
||||
time(t), position({x, y}) {}
|
||||
double time;
|
||||
std::array<double, 3> field;
|
||||
Coordinate position;
|
||||
};
|
||||
|
||||
|
||||
struct Position {
|
||||
Position(double t, double x, double y) :
|
||||
time(t), position({x, y}) {}
|
||||
double time;
|
||||
Coordinate position;
|
||||
};
|
||||
|
||||
using Magnetics = std::vector<Magnetic>;
|
||||
using Positions = std::vector<Position>;
|
||||
|
||||
#endif /* DATA_HPP_ */
|
@ -0,0 +1,22 @@
|
||||
|
||||
#include "json_output.hpp"
|
||||
|
||||
#include <iomanip>
|
||||
#include <fstream>
|
||||
|
||||
|
||||
JsonOutput::JsonOutput(char const* file_path) :
|
||||
filePath_(file_path)
|
||||
{}
|
||||
|
||||
|
||||
bool JsonOutput::write()
|
||||
{
|
||||
std::ofstream outfile(filePath_);
|
||||
if (!outfile) {
|
||||
return false;
|
||||
}
|
||||
outfile << std::setw(2);
|
||||
outfile << jsonData_ << std::endl;
|
||||
return true;
|
||||
}
|
@ -0,0 +1,24 @@
|
||||
#ifndef JSONOUTPUT_HPP_
|
||||
#define JSONOUTPUT_HPP_
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
|
||||
class JsonOutput {
|
||||
public:
|
||||
explicit JsonOutput(char const* file_path);
|
||||
~JsonOutput() = default;
|
||||
|
||||
template<typename T>
|
||||
void add(char const* key, T const& obj)
|
||||
{
|
||||
jsonData_[key] = obj;
|
||||
}
|
||||
bool write();
|
||||
|
||||
private:
|
||||
char const* filePath_;
|
||||
::nlohmann::json jsonData_;
|
||||
};
|
||||
|
||||
#endif // JSONOUTPUT_HPP_
|
@ -0,0 +1,61 @@
|
||||
#include "proto_loader.hpp"
|
||||
#include "protocol_buffers_definitions/recordings.pb.h"
|
||||
|
||||
#include <fstream>
|
||||
#include <stdexcept>
|
||||
|
||||
|
||||
|
||||
ProtoLoader::ProtoLoader(char const* f) :
|
||||
filePath_(f)
|
||||
{
|
||||
// Verify that the version of the library that we linked against is
|
||||
// compatible with the version of the headers we compiled against.
|
||||
GOOGLE_PROTOBUF_VERIFY_VERSION;
|
||||
|
||||
std::fstream input(filePath_, std::ios::in | std::ios::binary);
|
||||
if (!recording_.ParseFromIstream(&input)) {
|
||||
throw std::runtime_error("Failed to parse input file");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
ProtoLoader::~ProtoLoader()
|
||||
{
|
||||
// Optional: Delete all global objects allocated by libprotobuf.
|
||||
::google::protobuf::ShutdownProtobufLibrary();
|
||||
}
|
||||
|
||||
|
||||
std::vector<Magnetic> ProtoLoader::magnetics() const
|
||||
{
|
||||
double t_prev = 0;
|
||||
std::vector<Magnetic> magnetics;
|
||||
for (int i = 0; i < recording_.magnetics_size(); i++) {
|
||||
auto const& s = recording_.magnetics(i);
|
||||
double const t = s.t();
|
||||
if (t_prev > t) {
|
||||
throw std::runtime_error("Magnetics data not sorted by time");
|
||||
}
|
||||
magnetics.emplace_back(t, s.x(), s.y(), s.z());
|
||||
t_prev = t;
|
||||
}
|
||||
return magnetics;
|
||||
}
|
||||
|
||||
|
||||
std::vector<Position> ProtoLoader::groundtruth() const
|
||||
{
|
||||
double t_prev = 0;
|
||||
std::vector<Position> groundtruth;
|
||||
for (int i=0; i < recording_.positions_size(); i++) {
|
||||
auto const& s = recording_.positions(i);
|
||||
double const t = s.t();
|
||||
if ((t - t_prev) < dt_min) {
|
||||
throw std::runtime_error("Groundtruth data invalid");
|
||||
}
|
||||
groundtruth.emplace_back(t, s.x(), s.y());
|
||||
t_prev = t;
|
||||
}
|
||||
return groundtruth;
|
||||
}
|
@ -0,0 +1,24 @@
|
||||
#ifndef PROTOLOADER_HPP_
|
||||
#define PROTOLOADER_HPP_
|
||||
|
||||
#include "data.hpp"
|
||||
#include "protocol_buffers_definitions/recordings.pb.h"
|
||||
#include <vector>
|
||||
|
||||
|
||||
class ProtoLoader {
|
||||
public:
|
||||
explicit ProtoLoader(char const* file_path);
|
||||
~ProtoLoader();
|
||||
|
||||
std::vector<Magnetic> magnetics() const;
|
||||
std::vector<Position> groundtruth() const;
|
||||
|
||||
static constexpr double dt_min{1e-3};
|
||||
|
||||
private:
|
||||
char const* filePath_;
|
||||
::indoors::proto::Recording recording_;
|
||||
};
|
||||
|
||||
#endif /* PROTOLOADER_HPP_ */
|
@ -0,0 +1,69 @@
|
||||
#ifndef SPATIALGRID_HPP_
|
||||
#define SPATIALGRID_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
|
||||
template<typename value_t>
|
||||
class SpatialGrid {
|
||||
public:
|
||||
SpatialGrid() = delete;
|
||||
explicit SpatialGrid(double x_start, double y_start, double x_end, double y_end) :
|
||||
offset_({static_cast<size_t>(x_start / spacing), static_cast<size_t>(y_start / spacing)}),
|
||||
size_x_(static_cast<size_t>(std::ceil(x_end / spacing)) - offset_[0]),
|
||||
size_y_(static_cast<size_t>(std::ceil(y_end / spacing)) - offset_[1]),
|
||||
grid_(size_x_, std::vector<value_t>(size_y_))
|
||||
{}
|
||||
~SpatialGrid() = default;
|
||||
|
||||
value_t& at(double x, double y)
|
||||
{
|
||||
size_t const id_x{static_cast<size_t>(x / spacing) - offset_[0]};
|
||||
size_t const id_y{static_cast<size_t>(y / spacing) - offset_[1]};
|
||||
return grid_[id_x][id_y];
|
||||
}
|
||||
value_t& at(std::array<double, 2> pos)
|
||||
{
|
||||
return at(pos[0], pos[1]);
|
||||
}
|
||||
|
||||
std::vector<value_t>& operator[](size_t idx)
|
||||
{
|
||||
return grid_[idx];
|
||||
}
|
||||
size_t size_x() const
|
||||
{
|
||||
return grid_.size();
|
||||
}
|
||||
size_t size_y() const
|
||||
{
|
||||
return grid_[0].size();
|
||||
}
|
||||
size_t start_x() const noexcept
|
||||
{
|
||||
return offset_[0] * spacing;
|
||||
}
|
||||
size_t start_y() const noexcept
|
||||
{
|
||||
return offset_[1] * spacing;
|
||||
}
|
||||
|
||||
std::vector<std::vector<value_t>>& data() {
|
||||
return grid_;
|
||||
}
|
||||
|
||||
static constexpr size_t spacing{5};
|
||||
|
||||
private:
|
||||
size_t const offset_[2];
|
||||
size_t const size_x_;
|
||||
size_t const size_y_;
|
||||
|
||||
std::vector<std::vector<value_t>> grid_;
|
||||
};
|
||||
|
||||
|
||||
#endif /* SPATIALGRID_HPP_ */
|
Loading…
Reference in New Issue