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
|
/* Copyright (c) 2012 Patrick Ruoff
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*/
#pragma once
#include <opencv2/core.hpp>
#include <vector>
//-----------------------------------------------------------------------------
// Calibrates the translation from head to model = t_MH
// by recursive least squares /
// kalman filter in information form with identity noise covariance
// measurement equation when head position = t_CH is fixed:
// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k
class TranslationCalibrator
{
bool check_bucket(const cv::Matx33d& R_CM_k);
cv::Matx66f P; // normalized precision matrix = inverse covariance
cv::Vec6f y; // P*(-t_MH, t_CH)
using vec_i = std::vector<unsigned>;
vec_i used_yaw_poses {};
vec_i used_pitch_poses {};
unsigned yaw_rdof, pitch_rdof, nsamples = 0;
public:
TranslationCalibrator(unsigned yaw_rdof, unsigned pitch_rdof);
void reset();
// update the current estimate
void update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k);
// we're bringing in 3DOF samples but the calibrator only
// checks yaw and pitch
static constexpr inline unsigned num_cal_axis = 3;
static constexpr inline unsigned num_nsample_axis = 2;
using cv_cal_vec = cv::Vec<float, num_cal_axis>;
using cv_nsample_vec = cv::Vec<unsigned, num_nsample_axis>;
using tt = std::tuple<cv_cal_vec, cv_nsample_vec>;
// get the current estimate for t_MH
tt get_estimate();
static constexpr inline double yaw_spacing_in_degrees = 2;
static constexpr inline double pitch_spacing_in_degrees = 1.5;
};
|
