summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--ftnoir_tracker_pt/FTNoIR_PT_Controls.ui31
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp9
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.h3
-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.cpp7
-rw-r--r--ftnoir_tracker_pt/point_tracker.h6
7 files changed, 53 insertions, 10 deletions
diff --git a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
index 63ed7f86..109e50cb 100644
--- a/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
+++ b/ftnoir_tracker_pt/FTNoIR_PT_Controls.ui
@@ -9,7 +9,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>453</width>
+ <width>460</width>
<height>621</height>
</rect>
</property>
@@ -273,6 +273,35 @@
</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">
+ <string>°</string>
+ </property>
+ <property name="prefix">
+ <string/>
+ </property>
+ <property name="minimum">
+ <number>10</number>
+ </property>
+ <property name="maximum">
+ <number>90</number>
+ </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 05e7b067..3abaa35e 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -48,6 +48,13 @@ void Tracker::reset_command(Command command)
commands &= ~command;
}
+float Tracker::get_focal_length()
+{
+ static constexpr float pi = 3.1415926f;
+ const float fov = static_cast<int>(s.fov) * pi / 180.f;
+ return 0.5f / tan(0.5f * fov);
+}
+
void Tracker::run()
{
#ifdef PT_PERF_LOG
@@ -85,7 +92,7 @@ void Tracker::run()
4);
}
if (points.size() == PointModel::N_POINTS)
- point_tracker.track(points, model);
+ point_tracker.track(points, model, get_focal_length());
video_widget->update_image(frame);
}
#ifdef PT_PERF_LOG
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
index 2f922e17..3dd15618 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -57,6 +57,9 @@ private:
};
void set_command(Command command);
void reset_command(Command command);
+
+ float get_focal_length();
+
volatile int commands;
CVCamera camera;
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
index 2bf8ba75..8f6edc2f 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_dialog.cpp
@@ -65,6 +65,8 @@ TrackerDialog::TrackerDialog()
tie_setting(s.t_MH_x, ui.tx_spin);
tie_setting(s.t_MH_y, ui.ty_spin);
tie_setting(s.t_MH_z, ui.tz_spin);
+
+ tie_setting(s.fov, ui.fov);
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 c95dc3e5..804de22e 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt_settings.h
@@ -33,6 +33,8 @@ struct settings
value<int> clip_ty, clip_tz, clip_by, clip_bz;
value<int> active_model_panel, cap_x, cap_y, cap_z;
+
+ value<int> fov;
settings() :
b(bundle("tracker-pt")),
@@ -60,7 +62,8 @@ struct settings
active_model_panel(b, "active-model-panel", 0),
cap_x(b, "cap-x", 0),
cap_y(b, "cap-y", 0),
- cap_z(b, "cap-z", 0)
+ cap_z(b, "cap-z", 0),
+ fov(b, "camera-fov", 56)
{}
};
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index 3b1a49e2..9f04af30 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -83,16 +83,15 @@ void PointModel::get_d_order(const std::vector<vec>& points, int d_order[], vec
}
-// ----------------------------------------------------------------------------
PointTracker::PointTracker()
{
X_CM.t[2] = 1000; // default position: 1 m away from cam;
}
-void PointTracker::track(const vector<Vec2f>& points, const PointModel& model)
+void PointTracker::track(const vector<Vec2f>& points, const PointModel& model, float f)
{
const PointOrder& order = find_correspondences(points, model);
- POSIT(model, order);
+ POSIT(model, order, f);
}
PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv::Vec2f>& points, const PointModel& model)
@@ -117,7 +116,7 @@ PointTracker::PointOrder PointTracker::find_correspondences(const std::vector<cv
return p;
}
-int PointTracker::POSIT(const PointModel& model, const PointOrder& order_)
+int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float focal_length)
{
// POSIT algorithm for coplanar points as presented in
// [Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis: "Iterative Pose Estimation Using Coplanar Feature Points"]
diff --git a/ftnoir_tracker_pt/point_tracker.h b/ftnoir_tracker_pt/point_tracker.h
index f9bba311..d37fb726 100644
--- a/ftnoir_tracker_pt/point_tracker.h
+++ b/ftnoir_tracker_pt/point_tracker.h
@@ -88,15 +88,15 @@ 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);
+ void track(const std::vector<cv::Vec2f>& projected_points, const PointModel& model, float f);
Affine pose() const { return X_CM; }
+
private:
// the points in model order
typedef struct { cv::Vec2f points[PointModel::N_POINTS]; } PointOrder;
- static constexpr float focal_length = 1.0f;
PointOrder find_correspondences(const std::vector<cv::Vec2f>& projected_points, const PointModel &model);
- int POSIT(const PointModel& point_model, const PointOrder& order); // The POSIT algorithm, returns the number of iterations
+ 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
};