tracker/aruco: back out changes since rc7

However, include some crash fixes and minor changes.

Fixes #481

Reported-by: @Emton
Testing-by: @Emton

6 files changed, 200 insertions, 128 deletions

diff --git a/tracker-aruco/CMakeLists.txt b/tracker-aruco/CMakeLists.txt
index c0b50bee..d68b8f59 100644
--- a/tracker-aruco/CMakeLists.txt
+++ b/tracker-aruco/CMakeLists.txt
@@ -1,7 +1,9 @@
-find_package(OpenCV 3.0 QUIET COMPONENTS ${opencv-modules})
-set(SDK_ARUCO_LIBPATH "" CACHE FILEPATH "Aruco paper marker tracker static library path")
-if(SDK_ARUCO_LIBPATH AND OpenCV_FOUND)
-    opentrack_boilerplate(opentrack-tracker-aruco)
-    target_link_libraries(opentrack-tracker-aruco opentrack-cv ${SDK_ARUCO_LIBPATH} ${OpenCV_LIBS})
-    target_include_directories(opentrack-tracker-aruco SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
+find_package(OpenCV 3.0 QUIET)
+if(OpenCV_FOUND)
+    set(SDK_ARUCO_LIBPATH "" CACHE FILEPATH "Aruco paper marker tracker static library path")
+    if(SDK_ARUCO_LIBPATH)
+        opentrack_boilerplate(opentrack-tracker-aruco)
+        target_link_libraries(opentrack-tracker-aruco opentrack-cv ${SDK_ARUCO_LIBPATH} ${OpenCV_LIBS})
+        target_include_directories(opentrack-tracker-aruco SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
+    endif()
 endif()
diff --git a/tracker-aruco/aruco-trackercontrols.ui b/tracker-aruco/aruco-trackercontrols.ui
index 1479b2e7..5f043098 100644
--- a/tracker-aruco/aruco-trackercontrols.ui
+++ b/tracker-aruco/aruco-trackercontrols.ui
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <ui version="4.0">
- <class>dialog_aruco</class>
- <widget class="QWidget" name="dialog_aruco">
+ <class>Form</class>
+ <widget class="QWidget" name="Form">
   <property name="windowModality">
    <enum>Qt::NonModal</enum>
   </property>
@@ -145,10 +145,10 @@
             <locale language="English" country="UnitedStates"/>
            </property>
            <property name="minimum">
-            <number>10</number>
+            <number>35</number>
            </property>
            <property name="maximum">
-            <number>180</number>
+            <number>90</number>
            </property>
           </widget>
          </item>
@@ -172,11 +172,6 @@
            </item>
            <item>
             <property name="text">
-             <string>320x200</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
              <string>Default (not recommended!)</string>
             </property>
            </item>
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp
index 8edb8cf5..0df89fcb 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.cpp
+++ b/tracker-aruco/ftnoir_tracker_aruco.cpp
@@ -5,13 +5,13 @@
  * copyright notice and this permission notice appear in all copies.
  */
 
+#include "cv/video-widget.hpp"
 #include "ftnoir_tracker_aruco.h"
-#include "api/plugin-api.hpp"
-#include "compat/camera-names.hpp"
 #include "cv/video-property-page.hpp"
+#include "compat/camera-names.hpp"
+#include "compat/sleep.hpp"
 
 #include <opencv2/core.hpp>
-#include <opencv2/videoio.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/calib3d.hpp>
 
@@ -30,34 +30,35 @@ struct resolution_tuple
     int height;
 };
 
-static resolution_tuple resolution_choices[] =
+static constexpr const resolution_tuple resolution_choices[] =
 {
     { 640, 480 },
     { 320, 240 },
-    { 320, 200 },
     { 0, 0 }
 };
 
-tracker_aruco::tracker_aruco() :
+constexpr const double aruco_tracker::RC;
+constexpr const double aruco_tracker::size_min;
+constexpr const double aruco_tracker::size_max;
+
+aruco_tracker::aruco_tracker() :
     stop(false),
     layout(nullptr),
     videoWidget(nullptr),
+    fps(0),
     obj_points(4),
-    intrinsics(decltype(intrinsics)::eye()),
-    dist_coeffs(decltype(dist_coeffs)::zeros()),
-    rmat(decltype(rmat)::eye()),
+    intrinsics(cv::Matx33d::eye()),
+    rmat(cv::Matx33d::eye()),
     roi_points(4),
-    last_roi(65535, 65535, 0, 0),
-    freq(cv::getTickFrequency()), // XXX change to timer.hpp
-    cur_fps(0)
+    last_roi(65535, 65535, 0, 0)
 {
     // param 2 ignored for Otsu thresholding. it's required to use our fork of Aruco.
-    detector.setThresholdParams(5, -1);
+    detector.setThresholdParams(7, -1);
     detector.setDesiredSpeed(3);
     detector._thresMethod = aruco::MarkerDetector::FIXED_THRES;
 }
 
-tracker_aruco::~tracker_aruco()
+aruco_tracker::~aruco_tracker()
 {
     stop = true;
     wait();
@@ -65,10 +66,12 @@ tracker_aruco::~tracker_aruco()
         delete videoWidget;
     if(layout)
         delete layout;
+    // fast start/stop causes breakage
+    portable::sleep(1000);
     camera.release();
 }
 
-void tracker_aruco::start_tracker(QFrame* videoframe)
+void aruco_tracker::start_tracker(QFrame* videoframe)
 {
     videoframe->show();
     videoWidget = new cv_video_widget(videoframe);
@@ -85,9 +88,7 @@ void tracker_aruco::start_tracker(QFrame* videoframe)
     layout = layout_;
 }
 
-#define HT_PI M_PI
-
-void tracker_aruco::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
+void aruco_tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
 {
     QMutexLocker l(&mtx);
 
@@ -95,17 +96,14 @@ void tracker_aruco::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
     t_ = t;
 }
 
-bool tracker_aruco::detect_with_roi()
+bool aruco_tracker::detect_with_roi()
 {
     if (last_roi.width > 1 && last_roi.height > 1)
     {
-        float min = std::min(1.f, std::max(.01f, size_min * grayscale.cols / last_roi.width));
-        float max = std::max(.01f, std::min(1.f, size_max * grayscale.cols / last_roi.width));
-        detector.setMinMaxSize(min, max);
+        detector.setMinMaxSize(std::min(1., std::max(.01, size_min * grayscale.cols / last_roi.width)),
+                               std::max(.01, std::min(1., size_max * grayscale.cols / last_roi.width)));
 
-        cv::Mat grayscale_ = grayscale(last_roi);
-
-        detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false);
+        detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false);
 
         if (markers.size() == 1 && markers[0].size() == 4)
         {
@@ -124,14 +122,14 @@ bool tracker_aruco::detect_with_roi()
     return false;
 }
 
-bool tracker_aruco::detect_without_roi()
+bool aruco_tracker::detect_without_roi()
 {
     detector.setMinMaxSize(size_min, size_max);
     detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
     return markers.size() == 1 && markers[0].size() == 4;
 }
 
-bool tracker_aruco::open_camera()
+bool aruco_tracker::open_camera()
 {
     int rint = s.resolution;
     if (rint < 0 || rint >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple)))
@@ -180,7 +178,7 @@ bool tracker_aruco::open_camera()
     return true;
 }
 
-void tracker_aruco::set_intrinsics()
+void aruco_tracker::set_intrinsics()
 {
     const int w = grayscale.cols, h = grayscale.rows;
     const double diag_fov = static_cast<int>(s.fov) * M_PI / 180.;
@@ -195,16 +193,20 @@ void tracker_aruco::set_intrinsics()
     intrinsics(1, 2) = grayscale.rows/2;
 }
 
-void tracker_aruco::update_fps(double alpha)
+void aruco_tracker::update_fps()
 {
-    std::uint64_t time = std::uint64_t(cv::getTickCount());
-    const double dt = std::max(0., (time - last_time) / freq);
-    last_time = time;
+    const double dt = fps_timer.elapsed_seconds();
+    fps_timer.start();
+    const double alpha = dt/(dt + RC);
 
-    cur_fps = cur_fps * alpha + (1-alpha) * (fabs(dt) < 1e-6 ? 0 : 1./dt);
+    if (dt > 1e-3)
+    {
+        fps *= 1 - alpha;
+        fps += alpha * (1./dt + .8);
+    }
 }
 
-void tracker_aruco::draw_ar(bool ok)
+void aruco_tracker::draw_ar(bool ok)
 {
     if (ok)
     {
@@ -213,13 +215,13 @@ void tracker_aruco::draw_ar(bool ok)
             cv::line(frame, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 2, 8);
     }
 
-    char buf[32];
-    ::snprintf(buf, sizeof(buf)-1, "Hz: %d", (int)(unsigned short)cur_fps);
+    char buf[9];
+    ::snprintf(buf, sizeof(buf)-1, "Hz: %d", clamp(int(fps), 0, 9999));
     buf[sizeof(buf)-1] = '\0';
     cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1);
 }
 
-void tracker_aruco::clamp_last_roi()
+void aruco_tracker::clamp_last_roi()
 {
     if (last_roi.x < 0)
         last_roi.x = 0;
@@ -242,7 +244,7 @@ void tracker_aruco::clamp_last_roi()
     last_roi.height -= last_roi.y;
 }
 
-void tracker_aruco::set_points()
+void aruco_tracker::set_points()
 {
     using f = float;
     const f hx = f(s.headpos_x), hy = f(s.headpos_y), hz = f(s.headpos_z);
@@ -257,19 +259,18 @@ void tracker_aruco::set_points()
     obj_points[x4] = cv::Point3f(-size + hx, size + hy, 0 + hz);
 }
 
-void tracker_aruco::draw_centroid()
+void aruco_tracker::draw_centroid()
 {
     repr2.clear();
 
     static const std::vector<cv::Point3f> centroid { cv::Point3f(0, 0, 0) };
 
-    cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2);
+    cv::projectPoints(centroid, rvec, tvec, intrinsics, cv::noArray(), repr2);
 
-    auto s = cv::Scalar(255, 0, 255);
-    cv::circle(frame, repr2[0], 4, s, -1);
+    cv::circle(frame, repr2[0], 4, cv::Scalar(255, 0, 255), -1);
 }
 
-void tracker_aruco::set_last_roi()
+void aruco_tracker::set_last_roi()
 {
     roi_projection.clear();
 
@@ -281,12 +282,12 @@ void tracker_aruco::set_last_roi()
         roi_points[i] = pt * c_search_window;
     }
 
-    cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
+    cv::projectPoints(roi_points, rvec, tvec, intrinsics, cv::noArray(), roi_projection);
 
     set_roi_from_projection();
 }
 
-void tracker_aruco::set_rmat()
+void aruco_tracker::set_rmat()
 {
     cv::Rodrigues(rvec, rmat);
 
@@ -305,7 +306,7 @@ void tracker_aruco::set_rmat()
     t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]);
 }
 
-void tracker_aruco::set_roi_from_projection()
+void aruco_tracker::set_roi_from_projection()
 {
     last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0);
 
@@ -328,7 +329,7 @@ void tracker_aruco::set_roi_from_projection()
     clamp_last_roi();
 }
 
-void tracker_aruco::run()
+void aruco_tracker::run()
 {
     cv::setNumThreads(0);
 
@@ -340,7 +341,7 @@ void tracker_aruco::run()
     if (!open_camera())
         return;
 
-    last_time = std::uint64_t(cv::getTickCount());
+    fps_timer.start();
 
     while (!stop)
     {
@@ -357,7 +358,7 @@ void tracker_aruco::run()
 
         set_intrinsics();
 
-        update_fps(alpha_);
+        update_fps();
 
         markers.clear();
 
@@ -367,10 +368,7 @@ void tracker_aruco::run()
         {
             set_points();
 
-            if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_DLS))
-                goto fail;
-
-            if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, true, cv::SOLVEPNP_ITERATIVE))
+            if (!cv::solvePnP(obj_points, markers[0], intrinsics, cv::noArray(), rvec, tvec, false, cv::SOLVEPNP_ITERATIVE))
                 goto fail;
 
             set_last_roi();
@@ -389,19 +387,19 @@ fail:
     }
 }
 
-void tracker_aruco::data(double *data)
+void aruco_tracker::data(double *data)
 {
     QMutexLocker lck(&mtx);
 
     data[Yaw] = pose[Yaw];
     data[Pitch] = pose[Pitch];
     data[Roll] = pose[Roll];
-    data[TX] = pose[TX] * .5;
-    data[TY] = pose[TY] * .5;
+    data[TX] = pose[TX];
+    data[TY] = pose[TY];
     data[TZ] = pose[TZ];
 }
 
-dialog_aruco::dialog_aruco()
+aruco_dialog::aruco_dialog()
 {
     tracker = nullptr;
     calib_timer.setInterval(250);
@@ -419,34 +417,15 @@ dialog_aruco::dialog_aruco()
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
     connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate()));
     connect(this, SIGNAL(destroyed()), this, SLOT(cleanupCalib()));
-
     connect(&calib_timer, SIGNAL(timeout()), this, SLOT(update_tracker_calibration()));
-    connect(ui.cameraName, &QComboBox::currentTextChanged, this, &dialog_aruco::set_camera_settings_available);
-    set_camera_settings_available(ui.cameraName->currentText());
-    connect(ui.camera_settings, &QPushButton::clicked, this, &dialog_aruco::show_camera_settings);
-}
+    connect(ui.camera_settings, SIGNAL(clicked()), this, SLOT(camera_settings()));
 
+    connect(&s.camera_name, SIGNAL(valueChanged(const QString&)), this, SLOT(update_camera_settings_state(const QString&)));
 
-void dialog_aruco::set_camera_settings_available(const QString& camera_name)
-{
-    const bool avail = video_property_page::should_show_dialog(camera_name);
-    ui.camera_settings->setEnabled(avail);
+    update_camera_settings_state(s.camera_name);
 }
 
-void dialog_aruco::show_camera_settings()
-{
-    const int idx = ui.cameraName->currentIndex();
-    if (tracker)
-    {
-        cv::VideoCapture& cap = tracker->camera;
-        if (cap.isOpened())
-            video_property_page::show_from_capture(cap, idx);
-    }
-    else
-        video_property_page::show(idx);
-}
-
-void dialog_aruco::toggleCalibrate()
+void aruco_dialog::toggleCalibrate()
 {
     if (!calib_timer.isActive())
     {
@@ -467,13 +446,13 @@ void dialog_aruco::toggleCalibrate()
     }
 }
 
-void dialog_aruco::cleanupCalib()
+void aruco_dialog::cleanupCalib()
 {
     if (calib_timer.isActive())
         calib_timer.stop();
 }
 
-void dialog_aruco::update_tracker_calibration()
+void aruco_dialog::update_tracker_calibration()
 {
     if (calib_timer.isActive() && tracker)
     {
@@ -484,15 +463,31 @@ void dialog_aruco::update_tracker_calibration()
     }
 }
 
-void dialog_aruco::doOK()
+void aruco_dialog::doOK()
 {
     s.b->save();
     close();
 }
 
-void dialog_aruco::doCancel()
+void aruco_dialog::doCancel()
 {
     close();
 }
 
-OPENTRACK_DECLARE_TRACKER(tracker_aruco, dialog_aruco, aruco_metadata)
+void aruco_dialog::camera_settings()
+{
+    if (tracker)
+    {
+        QMutexLocker l(&tracker->camera_mtx);
+        video_property_page::show_from_capture(tracker->camera, camera_name_to_index(s.camera_name));
+    }
+    else
+        video_property_page::show(camera_name_to_index(s.camera_name));
+}
+
+void aruco_dialog::update_camera_settings_state(const QString& name)
+{
+    ui.camera_settings->setEnabled(video_property_page::should_show_dialog(name));
+}
+
+OPENTRACK_DECLARE_TRACKER(aruco_tracker, aruco_dialog, aruco_metadata)
diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h
index ccb1ad21..54940d28 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.h
+++ b/tracker-aruco/ftnoir_tracker_aruco.h
@@ -8,14 +8,13 @@
 #pragma once
 
 #include "ui_aruco-trackercontrols.h"
+#include "options/options.hpp"
+#include "trans_calib.h"
 #include "api/plugin-api.hpp"
-#include "include/markerdetector.h"
-
 #include "cv/video-widget.hpp"
-#include "cv/translation-calibrator.hpp"
+#include "compat/timer.hpp"
 
-#include <opencv2/core.hpp>
-#include <opencv2/videoio.hpp>
+#include "include/markerdetector.h"
 
 #include <QObject>
 #include <QThread>
@@ -26,7 +25,9 @@
 
 #include <cinttypes>
 
-#include "options/options.hpp"
+#include <opencv2/core.hpp>
+#include <opencv2/videoio.hpp>
+
 using namespace options;
 
 struct settings : opts {
@@ -46,16 +47,16 @@ struct settings : opts {
     {}
 };
 
-class dialog_aruco;
+class aruco_dialog;
 
-class tracker_aruco : protected QThread, public ITracker
+class aruco_tracker : protected QThread, public ITracker
 {
     Q_OBJECT
-    friend class dialog_aruco;
+    friend class aruco_dialog;
     static constexpr float c_search_window = 1.3f;
 public:
-    tracker_aruco();
-    ~tracker_aruco() override;
+    aruco_tracker();
+    ~aruco_tracker() override;
     void start_tracker(QFrame* frame) override;
     void data(double *data) override;
     void run() override;
@@ -65,7 +66,7 @@ private:
     bool detect_without_roi();
     bool open_camera();
     void set_intrinsics();
-    void update_fps(double dt);
+    void update_fps();
     void draw_ar(bool ok);
     void clamp_last_roi();
     void set_points();
@@ -77,16 +78,14 @@ private:
     cv::VideoCapture camera;
     QMutex camera_mtx;
     QMutex mtx;
-    volatile bool stop;
     QHBoxLayout* layout;
     cv_video_widget* videoWidget;
     settings s;
-    double pose[6];
+    double pose[6], fps;
     cv::Mat frame, grayscale, color;
     cv::Matx33d r;
     std::vector<cv::Point3f> obj_points;
     cv::Matx33d intrinsics;
-    cv::Matx14f dist_coeffs;
     aruco::MarkerDetector detector;
     std::vector<aruco::Marker> markers;
     cv::Vec3d t;
@@ -97,25 +96,26 @@ private:
     cv::Vec3d euler;
     std::vector<cv::Point3f> roi_points;
     cv::Rect last_roi;
-    double freq, cur_fps;
-    std::uint64_t last_time;
+    Timer fps_timer;
+
+    volatile bool stop;
 
-    static constexpr float size_min = 0.05f;
-    static constexpr float size_max = 0.3f;
+    static constexpr const double size_min = 0.05f;
+    static constexpr const double size_max = 0.3f;
 
-    static constexpr double alpha_ = .95;
+    static constexpr const double RC = .25;
 };
 
-class dialog_aruco : public ITrackerDialog
+class aruco_dialog : public ITrackerDialog
 {
     Q_OBJECT
 public:
-    dialog_aruco();
-    void register_tracker(ITracker * x) override { tracker = static_cast<tracker_aruco*>(x); }
+    aruco_dialog();
+    void register_tracker(ITracker * x) override { tracker = static_cast<aruco_tracker*>(x); }
     void unregister_tracker() override { tracker = nullptr; }
 private:
-    Ui::dialog_aruco ui;
-    tracker_aruco* tracker;
+    Ui::Form ui;
+    aruco_tracker* tracker;
     settings s;
     TranslationCalibrator calibrator;
     QTimer calib_timer;
@@ -125,12 +125,12 @@ private slots:
     void toggleCalibrate();
     void cleanupCalib();
     void update_tracker_calibration();
-    void set_camera_settings_available(const QString& camera_name);
-    void show_camera_settings();
+    void camera_settings();
+    void update_camera_settings_state(const QString& name);
 };
 
 class aruco_metadata : public Metadata
 {
-    QString name() { return QString(QCoreApplication::translate("aruco_metadata", "aruco -- paper marker tracker")); }
+    QString name() { return QString("aruco -- paper marker tracker"); }
     QIcon icon() { return QIcon(":/images/aruco.png"); }
 };
diff --git a/tracker-aruco/trans_calib.cpp b/tracker-aruco/trans_calib.cpp
new file mode 100644
index 00000000..b5148efd
--- /dev/null
+++ b/tracker-aruco/trans_calib.cpp
@@ -0,0 +1,41 @@
+/* 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.
+ */
+
+#include "trans_calib.h"
+
+TranslationCalibrator::TranslationCalibrator()
+{
+    reset();
+}
+
+void TranslationCalibrator::reset()
+{
+    P = cv::Matx66f::zeros();
+    y = cv::Vec6f(0,0,0, 0,0,0);
+}
+
+void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k)
+{
+    cv::Matx<double, 6,3> H_k_T = cv::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;
+}
+
+cv::Vec3f TranslationCalibrator::get_estimate()
+{
+    cv::Vec6f x = P.inv() * y;
+    return cv::Vec3f(x[0], x[1], x[2]);
+}
diff --git a/tracker-aruco/trans_calib.h b/tracker-aruco/trans_calib.h
new file mode 100644
index 00000000..3d1e56fa
--- /dev/null
+++ b/tracker-aruco/trans_calib.h
@@ -0,0 +1,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/core/core.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::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k);
+
+    // get the current estimate for t_MH
+    cv::Vec3f get_estimate();
+
+private:
+    cv::Matx66f P;  // normalized precision matrix = inverse covariance
+    cv::Vec6f y;    // P*(-t_MH, t_CH)
+};
+
+#endif //TRANSCALIB_H