1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
|
#pragma once
#include "simple-mat.hpp"
#include <array>
#include <vector>
#include <tuple>
#include "export.hpp"
namespace correlation_calibrator_impl {
static constexpr inline double min[6] = {
-50,
-50,
250,
-180,
-180,
-180,
};
static constexpr inline double max[6] = {
50,
50,
250,
180,
180,
180,
};
static constexpr inline double yaw_spacing_in_degrees = 1.5;
static constexpr inline double pitch_spacing_in_degrees = 1;
static constexpr inline double roll_spacing_in_degrees = 1;
static constexpr inline unsigned yaw_nbuckets = 1+ 360./yaw_spacing_in_degrees;
static constexpr inline unsigned pitch_nbuckets = 1+ 360./pitch_spacing_in_degrees;
static constexpr inline unsigned roll_nbuckets = 1+ 360./roll_spacing_in_degrees;
static constexpr inline double translation_spacing = .25;
static constexpr inline unsigned x_nbuckets = 1+ (max[0]-min[0])/translation_spacing;
static constexpr inline unsigned y_nbuckets = 1+ (max[1]-min[1])/translation_spacing;
static constexpr inline unsigned z_nbuckets = 1+ (max[2]-min[2])/translation_spacing;
using vec6 = Mat<double, 6, 1>;
using mat66 = Mat<double, 6, 6>;
class OTR_COMPAT_EXPORT correlation_calibrator final
{
// careful to avoid vector copies
std::array<std::vector<bool>, 6> buckets =
{
std::vector<bool>(x_nbuckets, false),
std::vector<bool>(y_nbuckets, false),
std::vector<bool>(z_nbuckets, false),
std::vector<bool>(yaw_nbuckets, false),
std::vector<bool>(pitch_nbuckets, false),
std::vector<bool>(roll_nbuckets, false),
};
std::vector<vec6> data;
bool check_buckets(const vec6& data);
public:
correlation_calibrator();
void input(const vec6& data);
mat66 get_coefficients() const;
unsigned sample_count() const;
static constexpr inline unsigned min_samples = 25;
};
} // ns correlation_calibrator_impl
using correlation_calibrator = correlation_calibrator_impl::correlation_calibrator;
|
