summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_pt
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_pt')
-rw-r--r--ftnoir_tracker_pt/FTNoIR_PT_Controls.ui86
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp2
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp2
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h5
-rw-r--r--ftnoir_tracker_pt/point_tracker.cpp54
-rw-r--r--ftnoir_tracker_pt/point_tracker.h15
6 files changed, 124 insertions, 40 deletions
diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
index 4342cfac..0e6048c3 100644
--- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
+++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
@@ -83,21 +83,30 @@
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QComboBox" name="camdevice_combo">
+ <item row="1" column="1">
+ <widget class="QSpinBox" name="res_x_spin">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="minimumContentsLength">
+ <property name="toolTip">
+ <string>Desired capture width</string>
+ </property>
+ <property name="suffix">
+ <string> px</string>
+ </property>
+ <property name="maximum">
+ <number>2000</number>
+ </property>
+ <property name="singleStep">
<number>10</number>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_36">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_41">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Preferred">
<horstretch>0</horstretch>
@@ -105,34 +114,38 @@
</sizepolicy>
</property>
<property name="text">
- <string>Width</string>
+ <string>Height</string>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QSpinBox" name="res_x_spin">
+ <item row="0" column="1">
+ <widget class="QComboBox" name="camdevice_combo">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="toolTip">
- <string>Desired capture width</string>
- </property>
- <property name="suffix">
- <string> px</string>
+ <property name="minimumContentsLength">
+ <number>10</number>
</property>
- <property name="maximum">
- <number>2000</number>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_36">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
- <property name="singleStep">
- <number>10</number>
+ <property name="text">
+ <string>Width</string>
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_41">
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_4">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Preferred">
<horstretch>0</horstretch>
@@ -140,7 +153,7 @@
</sizepolicy>
</property>
<property name="text">
- <string>Height</string>
+ <string>Field of view</string>
</property>
</widget>
</item>
@@ -201,19 +214,6 @@
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_4">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Field of view</string>
- </property>
- </widget>
- </item>
<item row="4" column="1">
<widget class="QSpinBox" name="fov">
<property name="suffix">
@@ -230,6 +230,26 @@
</property>
</widget>
</item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Dynamic pose resolution</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QCheckBox" name="dynamic_pose">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index fc5e5b99..23cc72c7 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -77,7 +77,7 @@ void Tracker::run()
std::vector<cv::Vec2f> points = point_extractor.extract_points(frame);
if (points.size() == PointModel::N_POINTS)
- point_tracker.track(points, PointModel(s), get_focal_length());
+ point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose);
{
Affine CM = pose();
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index 2c1ec5d8..7be05bb7 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -69,6 +69,8 @@ TrackerDialog::TrackerDialog()
tie_setting(s.fov, ui.fov);
tie_setting(s.active_model_panel, ui.model_tabs);
+
+ tie_setting(s.dynamic_pose, ui.dynamic_pose);
connect( ui.tcalib_button,SIGNAL(toggled(bool)), this,SLOT(startstop_trans_calib(bool)) );
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
index bce89058..57752ed6 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -34,6 +34,8 @@ struct settings
value<int> active_model_panel, cap_x, cap_y, cap_z;
value<int> fov;
+
+ value<bool> dynamic_pose;
settings() :
b(bundle("tracker-pt")),
@@ -62,7 +64,8 @@ struct settings
cap_x(b, "cap-x", 0),
cap_y(b, "cap-y", 0),
cap_z(b, "cap-z", 0),
- fov(b, "camera-fov", 56)
+ fov(b, "camera-fov", 56),
+ dynamic_pose(b, "dynamic-pose-resolution", false)
{}
};
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index 0f996316..324119a9 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -67,9 +67,53 @@ PointTracker::PointTracker()
X_CM.t[2] = 1000; // default position: 1 m away from cam;
}
-void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f)
+PointTracker::PointOrder PointTracker::find_correspondences_previous(const vector<Vec2f>& points, const PointModel& model, float f)
{
- const PointOrder& order = find_correspondences(points, model);
+ PointTracker::PointOrder p;
+ p.points[0] = project(Vec3f(0,0,0), f);
+ p.points[1] = project(model.M01, f);
+ p.points[2] = project(model.M02, f);
+
+ // set correspondences by minimum distance to projected model point
+ bool point_taken[PointModel::N_POINTS];
+ for (int i=0; i<PointModel::N_POINTS; ++i)
+ point_taken[i] = false;
+
+ for (int i=0; i<PointModel::N_POINTS; ++i)
+ {
+ float min_sdist = 0;
+ int min_idx = 0;
+ // find closest point to projected model point i
+ for (int j=0; j<PointModel::N_POINTS; ++j)
+ {
+ Vec2f d = p.points[i]-points[j];
+ float sdist = d.dot(d);
+ if (sdist < min_sdist || j==0)
+ {
+ min_idx = j;
+ min_sdist = sdist;
+ }
+ }
+ // if one point is closest to more than one model point, fallback
+ if (point_taken[min_idx])
+ {
+ return find_correspondences(points, model);
+ }
+ point_taken[min_idx] = true;
+ p.points[i] = points[min_idx];
+ }
+ return p;
+}
+
+void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f, bool dynamic_pose)
+{
+ PointOrder order;
+
+ if (!dynamic_pose)
+ order = find_correspondences(points, model);
+ else
+ order = find_correspondences_previous(points, model, f);
+
POSIT(model, order, f);
}
@@ -222,3 +266,9 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float
//
//qDebug()<<"r: "<<r[0]<<' '<<r[1]<<' '<<r[2]<<'\n';
}
+
+cv::Vec2f PointTracker::project(const cv::Vec3f& v_M, float f)
+{
+ cv::Vec3f v_C = X_CM * v_M;
+ return cv::Vec2f(f*v_C[0]/v_C[2], f*v_C[1]/v_C[2]);
+}
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index 61d2a826..10bd2cef 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -119,14 +119,23 @@ public:
// track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5)
// f : (focal length)/(sensor width)
// dt : time since last call
- void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f);
+ void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f, bool dynamic_pose);
Affine pose() const { return X_CM; }
-
+ cv::Vec2f project(const cv::Vec3f& v_M, float f);
private:
// the points in model order
- typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder;
+ struct PointOrder
+ {
+ cv::Vec2f points[PointModel::N_POINTS];
+ PointOrder()
+ {
+ for (int i = 0; i < PointModel::N_POINTS; i++)
+ points[i] = cv::Vec2f(0, 0);
+ }
+ };
PointOrder find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel &model);
+ PointOrder find_correspondences_previous(const std::vector<cv::Vec2f>& points, const PointModel &model, float f);
int POSIT(const PointModel& point_model, const PointOrder& order, float focal_length); // The POSIT algorithm, returns the number of iterations
Affine X_CM; // trafo from model to camera
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-26 06:27:26 +0000