diff options
Diffstat (limited to 'ftnoir_tracker_aruco/trans_calib.cpp')
| -rw-r--r-- | ftnoir_tracker_aruco/trans_calib.cpp | 32 |
1 files changed, 16 insertions, 16 deletions
diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp index 369de449..a562763a 100644 --- a/ftnoir_tracker_aruco/trans_calib.cpp +++ b/ftnoir_tracker_aruco/trans_calib.cpp @@ -12,33 +12,33 @@ using namespace cv; //----------------------------------------------------------------------------- TranslationCalibrator::TranslationCalibrator() { - reset(); + reset(); } void TranslationCalibrator::reset() { - P = Matx66f::zeros(); - y = Vec6f(0,0,0, 0,0,0); + P = Matx66f::zeros(); + y = Vec6f(0,0,0, 0,0,0); } void TranslationCalibrator::update(const Matx33d& R_CM_k, const Vec3d& t_CM_k) { - Matx<double, 6,3> H_k_T = Matx<double, 6,3>::zeros(); - for (int i=0; i<3; ++i) { - for (int j=0; j<3; ++j) { - H_k_T(i,j) = R_CM_k(j,i); - } - } - for (int i=0; i<3; ++i) - { - H_k_T(3+i,i) = 1.0; - } - P += H_k_T * H_k_T.t(); - y += H_k_T * t_CM_k; + Matx<double, 6,3> H_k_T = Matx<double, 6,3>::zeros(); + for (int i=0; i<3; ++i) { + for (int j=0; j<3; ++j) { + H_k_T(i,j) = R_CM_k(j,i); + } + } + for (int i=0; i<3; ++i) + { + H_k_T(3+i,i) = 1.0; + } + P += H_k_T * H_k_T.t(); + y += H_k_T * t_CM_k; } Vec3f TranslationCalibrator::get_estimate() { - Vec6f x = P.inv() * y; + Vec6f x = P.inv() * y; return Vec3f(x[0], x[1], x[2]); } |