corrected vc9 project file debug settings

Tracker_PT: implemented centering
Tracker_PT: implemented new point extraction
Tracker_PT: various bugfixes

git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@148 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb

10 files changed, 140 insertions, 30 deletions

diff --git a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui
index ca2d9898..38909013 100644
--- a/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui
+++ b/FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui
@@ -760,6 +760,13 @@
    <item>

     <layout class="QHBoxLayout" name="horizontalLayout_4">

      <item>

+      <widget class="QPushButton" name="center_button">

+       <property name="text">

+        <string>Center</string>

+       </property>

+      </widget>

+     </item>

+     <item>

       <spacer name="horizontalSpacer_6">

        <property name="orientation">

         <enum>Qt::Horizontal</enum>

diff --git a/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj b/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj
index a7989a5e..b938173d 100644
--- a/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj
+++ b/FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj
@@ -85,7 +85,7 @@
 		</Configuration>

 		<Configuration

 			Name="Debug|Win32"

-			OutputDirectory="$(SolutionDir)\bin"

+			OutputDirectory="$(SolutionDir)/bin_dbg"

 			IntermediateDirectory="$(SolutionDir)\$(ConfigurationName)"

 			ConfigurationType="2"

 			>

@@ -124,7 +124,7 @@
 			/>

 			<Tool

 				Name="VCLinkerTool"

-				AdditionalDependencies="qtmain.lib QtCore4.lib QtGui4.lib QtOpenGL4.lib opengl32.lib glu32.lib opencv_core242d.lib opencv_highgui242d.lib opencv_imgproc242d.lib opencv_calib3d242d.lib"

+				AdditionalDependencies="qtmaind.lib QtCored4.lib QtGuid4.lib QtOpenGLd4.lib opengl32.lib glu32.lib opencv_core242d.lib opencv_highgui242d.lib opencv_imgproc242d.lib opencv_calib3d242d.lib"

 				OutputFile="$(OutDir)\$(ProjectName).dll"

 				AdditionalLibraryDirectories="&quot;$(QTDIR)\lib&quot;;&quot;$(OPENCV_DIR)\x86\vc9\lib&quot;"

 				GenerateDebugInformation="true"

diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp
index 2e8430bc..6301ddd0 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp
@@ -152,29 +152,49 @@ bool Tracker::GiveHeadPoseData(THeadPoseData *data)
 	const float rad2deg = 180.0/3.14159265;

 	{

 		QMutexLocker lock(&mutex);

-		FrameTrafo pose = point_tracker.get_pose();

+

+		FrameTrafo X_CM = point_tracker.get_pose();

+		FrameTrafo X_MH(Matx33f::eye(), t_MH);

+		FrameTrafo X_CH = X_CM * X_MH;

+

+		Matx33f R = X_CH.R * X_CH_0.R.t(); 

+		Vec3f t   = X_CH.t - X_CH_0.t;

 

 		// get translation

-		Vec3f p = pose.R * t_MH + pose.t;

-		data->x = p[0] / 10.0;	// convert to cm

-		data->y = p[1] / 10.0;

-		data->z = p[2] / 10.0;

-

-		// extract rotation angles from rotation matrix

-		const Matx33f& R = pose.R;

-		data->yaw = atan2( -R(2,0), sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) );

-		float cos_beta = cos(data->yaw);

-		if (cos_beta != 0)

-		{

-			data->pitch = rad2deg * atan2( R(2,1)/cos_beta, R(2,2)/cos_beta);

-			data->roll  = rad2deg * atan2( R(1,0)/cos_beta, R(0,0)/cos_beta);

-		}

-		data->yaw *= rad2deg;

+		data->x = t[0] / 10.0;	// convert to cm

+		data->y = t[1] / 10.0;

+		data->z = t[2] / 10.0;

+

+		// translate rotatation matrix from opengl (G) to roll-pitch-yaw (R) frame

+		// -z -> x, y -> z, x -> -y

+		Matx33f R_RG( 0, 0,-1,

+			         -1, 0, 0,

+					  0, 1, 0);

+		R = R_RG * R * R_RG.t();

+

+		// extract rotation angles

+		float alpha, beta, gamma;

+		//beta = atan2( -R(2,0), sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0)) );

+		beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );

+		alpha = atan2( R(1,0), R(0,0));

+		gamma = atan2( R(2,1), R(2,2));		

+

+		data->yaw   = rad2deg * alpha;

+		data->pitch = rad2deg * beta;

+		data->roll  = rad2deg * gamma;

 	}

-	refreshVideo();

 	return true;

 }

 

+void Tracker::CenterTracker()

+{

+	QMutexLocker lock(&mutex);

+	FrameTrafo X_CM_0 = point_tracker.get_pose();

+	FrameTrafo X_MH(Matx33f::eye(), t_MH);

+	X_CH_0 = X_CM_0 * X_MH;

+

+}

+

 //-----------------------------------------------------------------------------

 #pragma comment(linker, "/export:GetTracker=_GetTracker@0")

 

diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h
index a3390cbf..24ecb6f1 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt.h
@@ -27,6 +27,9 @@ public:
 	Tracker();

 	~Tracker();

 

+	void CenterTracker();

+	

+

 	// ITracker interface

 	void Initialize(QFrame *videoframe);

 	void StartTracker(HWND parent_window);

@@ -42,7 +45,9 @@ public:
 	int get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); }

 	void get_cam_info(CamInfo* info) { QMutexLocker lock(&mutex); *info = camera.get_info(); }

 

-protected:

+protected:	

+	FrameTrafo X_CH_0; // for centering

+

 	QMutex mutex;

 	cv::Mat frame;	// the output frame for display

 

@@ -53,14 +58,14 @@ protected:
 	void set_command(Command command);

 	void reset_command(Command command);

 	int commands;

-

+	

 	Camera camera;

 	PointExtractor point_extractor;

 	PointTracker point_tracker;

 	cv::Vec3f t_MH;

 	bool draw_frame;

 	int sleep_time;

-

+	

 	long frame_count;

 

 	VideoWidget* video_widget;

diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp
index 4b79b892..e13ed94a 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp
@@ -62,6 +62,7 @@ TrackerDialog::TrackerDialog()
 

 	connect(ui.ok_button, SIGNAL(clicked()), this, SLOT(doOK()));

 	connect(ui.cancel_button, SIGNAL(clicked()), this, SLOT(doCancel()));

+	connect(ui.center_button, SIGNAL(clicked()), this, SLOT(doCenter()));

 

     connect(&timer,SIGNAL(timeout()), this,SLOT(poll_tracker_info()));

     timer.start(100);

@@ -114,6 +115,12 @@ void TrackerDialog::settings_changed()
 		tracker->apply_without_camindex(settings);

 }

 

+void TrackerDialog::doCenter()

+{

+	if (tracker)

+		tracker->CenterTracker();

+}

+

 void TrackerDialog::doOK()

 {

 	settings.save_ini();

@@ -196,7 +203,7 @@ void TrackerDialog::unRegisterTracker()
 {

 	qDebug()<<"tracker un-registerd";

 	tracker = NULL;

-	ui.tcalib_button->setEnabled(true); 

+	ui.tcalib_button->setEnabled(false); 

 }

 

 //-----------------------------------------------------------------------------

diff --git a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h
index 95736a8d..09bbb839 100644
--- a/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h
+++ b/FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h
@@ -33,6 +33,7 @@ public:
 protected slots:

 	void doOK();

 	void doCancel();

+	void doCenter();

 

 	// ugly qt stuff

 	void set_video_widget(bool val)  { settings.video_widget = val;   settings_changed(); }

diff --git a/FTNoIR_Tracker_PT/point_extractor.cpp b/FTNoIR_Tracker_PT/point_extractor.cpp
index 956a2834..a415f24e 100644
--- a/FTNoIR_Tracker_PT/point_extractor.cpp
+++ b/FTNoIR_Tracker_PT/point_extractor.cpp
@@ -15,14 +15,32 @@ using namespace std;
 // ----------------------------------------------------------------------------

 const vector<Vec2f>& PointExtractor::extract_points(Mat frame, float dt, bool draw_output)

 {

+	/*

+	// sensitivity test for tracker

+	static int n = 0;

+	if (points.size() == 3)

+	{

+		for (int i=0; i<3; ++i)

+		{

+			points[i][0] -= 1e-4*sin(n/100.0);

+			points[i][1] -= 1e-4*cos(n/100.0);

+		}

+		++n;

+		return points;

+	}

+	*/

+

 	// convert to grayscale

 	Mat frame_bw;

 	cvtColor(frame, frame_bw, CV_RGB2GRAY);

 

 	// convert to binary

 	threshold(frame_bw, frame_bw, threshold_val, 255, THRESH_BINARY);

-	erode(frame_bw, frame_bw, Mat(), Point(-1,-1), min_size);

+	//erode(frame_bw, frame_bw, Mat(), Point(-1,-1), min_size); //destroys information -> bad for subpixel accurarcy

 

+	// find connected components...

+	// Method 1: contours

+	//*

 	// find contours

 	vector< vector<Point> > contours;

 	findContours(frame_bw.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

@@ -38,14 +56,66 @@ const vector<Vec2f>& PointExtractor::extract_points(Mat frame, float dt, bool dr
 		 ++iter)

 	{

 		minEnclosingCircle(*iter, dummy, r);

-		if (r > max_size - min_size) break;

+		if (r > max_size) continue;

 		Moments m = moments(*iter);

-		if (m.m00 == 0) break;

+		if (m.m00 < 3.14*min_size*min_size) continue;

 		// convert to centered camera coordinate system with y axis upwards

 		c[0] =  (m.m10/m.m00 - frame.cols/2)/frame.cols;

 		c[1] = -(m.m01/m.m00 - frame.rows/2)/frame.cols;

 		points.push_back(c);

 	}

+	//*/

+

+	// Method 2: floodfill
+	/*
+	// extract blobs
+	struct BlobInfo
+	{
+		BlobInfo() : m00(0), m10(0), m01(0) {}
+		long m00;
+		long m10;
+		long m01;
+	};
+	vector<BlobInfo> blobs;
+    int blob_count = 1;
+
+    for (int y=0; y < frame_bw.rows; y++) {
+        for (int x=0; x < frame_bw.cols; x++) {
+			if (frame_bw.at<unsigned char>(y,x) != 255) continue;
+            Rect rect;
+            floodFill(frame_bw, Point(x,y), Scalar(blob_count), &rect, Scalar(0), Scalar(0), 4);
+			BlobInfo blob;
+            for (int i=rect.y; i < (rect.y+rect.height); i++) {
+                for (int j=rect.x; j < (rect.x+rect.width); j++) {
+                    if (frame_bw.at<unsigned char>(i,j) != blob_count) continue;
+                    blob.m00++;
+					blob.m01+=i;
+					blob.m10+=j;
+                }
+            }
+            blobs.push_back(blob);
+            blob_count++;
+			if (blob_count >= 255) break;
+        }
+    }
+
+	// extract points
+	Vec2f c;
+	points.clear();
+	float m00_min = 3.14*min_size*min_size;
+	float m00_max = 3.14*max_size*max_size;
+	for (vector<BlobInfo>::iterator iter = blobs.begin();

+		 iter!= blobs.end();

+		 ++iter)

+	{

+		const BlobInfo& m = *iter;

+		if (m.m00 < m00_min || m.m00 > m00_max) continue;

+		// convert to centered camera coordinate system with y axis upwards

+		c[0] =  (m.m10/float(m.m00) - frame.cols/2)/frame.cols;

+		c[1] = -(m.m01/float(m.m00) - frame.rows/2)/frame.cols;

+		points.push_back(c);

+	}
+	*/
 	

 	// draw output image

 	if (draw_output)

diff --git a/FTNoIR_Tracker_PT/point_tracker.cpp b/FTNoIR_Tracker_PT/point_tracker.cpp
index 8a32326d..90093032 100644
--- a/FTNoIR_Tracker_PT/point_tracker.cpp
+++ b/FTNoIR_Tracker_PT/point_tracker.cpp
@@ -138,7 +138,7 @@ void PointTracker::POSIT(float f)
 	//Matx33f R_expected = X_CM.R;

 

 	const int MAX_ITER = 100;

-	const float EPS_THRESHOLD = 1e-3;

+	const float EPS_THRESHOLD = 1e-4;

 

 	int i=1;

 	for (; i<MAX_ITER; ++i)

diff --git a/FTNoIR_Tracker_PT/trans_calib.cpp b/FTNoIR_Tracker_PT/trans_calib.cpp
index 260d24b1..9b75a1b6 100644
--- a/FTNoIR_Tracker_PT/trans_calib.cpp
+++ b/FTNoIR_Tracker_PT/trans_calib.cpp
@@ -40,5 +40,5 @@ void TranslationCalibrator::update(const Matx33f& R_CM_k, const Vec3f& t_CM_k)
 Vec3f TranslationCalibrator::get_estimate()

 {

 	Vec6f x = P.inv() * y;

-	return Vec3f(x[0], x[1], x[2]);

+	return Vec3f(-x[0], -x[1], -x[2]);

 }
\ No newline at end of file
diff --git a/FTNoIR_Tracker_PT/trans_calib.h b/FTNoIR_Tracker_PT/trans_calib.h
index 57b9496c..4024d011 100644
--- a/FTNoIR_Tracker_PT/trans_calib.h
+++ b/FTNoIR_Tracker_PT/trans_calib.h
@@ -15,7 +15,7 @@
 // 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

+// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k

 

 class TranslationCalibrator

 {

@@ -33,7 +33,7 @@ public:
 

 protected:

 	cv::Matx66f P;	// normalized precision matrix = inverse covariance

-	cv::Vec6f y;	// P*(t_MH, -c)

+	cv::Vec6f y;	// P*(-t_MH, t_CH)

 };

 

 #endif //TRANSCALIB_H
\ No newline at end of file