summaryrefslogtreecommitdiffhomepage
path: root/FTNoIR_Tracker_PT/trans_calib.h
blob: 57b9496cd2cabd029dcbc70fc0402df5c32324c1 (plain)
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
/* 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.
 */

#ifndef TRANSCALIB_H
#define TRANSCALIB_H

#include <opencv2/opencv.hpp>

//-----------------------------------------------------------------------------
// 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
{
public:
	TranslationCalibrator();

	// reset the calibration process
	void reset();

	// update the current estimate
	void update(const cv::Matx33f& R_CM_k, const cv::Vec3f& t_CM_k);

	// get the current estimate for t_MH
	cv::Vec3f get_estimate();

protected:
	cv::Matx66f P;	// normalized precision matrix = inverse covariance
	cv::Vec6f y;	// P*(t_MH, -c)
};

#endif //TRANSCALIB_H