From 5da3b245b35cdd2bb4b1490aae81f8d0ff0bde82 Mon Sep 17 00:00:00 2001 From: Patrick Ruoff Date: Thu, 20 Sep 2012 15:08:10 +0000 Subject: 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 --- .../FTNoIR_Filter_accela_vc9.vcproj | 6 +- FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2_vc9.vcproj | 2 +- FTNoIR_Protocol_FG/FTNoIR_Protocol_FG_vc9.vcproj | 2 +- .../FTNoIR_Protocol_FSUIPC_vc9.vcproj | 2 +- FTNoIR_Protocol_FT/FTNoIR_Protocol_FT_vc9.vcproj | 2 +- .../FTNoIR_Protocol_FTIR_vc9.vcproj | 2 +- FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN_vc9.vcproj | 2 +- .../FTNoIR_Protocol_MOUSE_vc9.vcproj | 2 +- .../FTNoIR_Protocol_PPJOY_vc9.vcproj | 2 +- FTNoIR_Protocol_SC/FTNoIR_Protocol_SC_vc9.vcproj | 2 +- FTNoIR_Tracker_FD/FTNoIR_Tracker_FD_vc9.vcproj | 17 ++++- FTNoIR_Tracker_PT/FTNoIR_PT_Controls.ui | 7 ++ FTNoIR_Tracker_PT/FTNoIR_Tracker_PT_vc9.vcproj | 4 +- FTNoIR_Tracker_PT/ftnoir_tracker_pt.cpp | 54 ++++++++++----- FTNoIR_Tracker_PT/ftnoir_tracker_pt.h | 11 +++- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.cpp | 9 ++- FTNoIR_Tracker_PT/ftnoir_tracker_pt_dialog.h | 1 + FTNoIR_Tracker_PT/point_extractor.cpp | 76 +++++++++++++++++++++- FTNoIR_Tracker_PT/point_tracker.cpp | 2 +- FTNoIR_Tracker_PT/trans_calib.cpp | 2 +- FTNoIR_Tracker_PT/trans_calib.h | 4 +- FTNoIR_Tracker_SM/FTNoIR_Tracker_SM_vc9.vcproj | 2 +- FTNoIR_Tracker_UDP/FTNoIR_Tracker_UDP_vc9.vcproj | 2 +- FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj | 4 +- FaceTrackNoIR_vc9.sln | 4 -- 25 files changed, 170 insertions(+), 53 deletions(-) diff --git a/FTNoIR_Filter_Accela/FTNoIR_Filter_accela_vc9.vcproj b/FTNoIR_Filter_Accela/FTNoIR_Filter_accela_vc9.vcproj index 0e08c9f1..3999ca41 100644 --- a/FTNoIR_Filter_Accela/FTNoIR_Filter_accela_vc9.vcproj +++ b/FTNoIR_Filter_Accela/FTNoIR_Filter_accela_vc9.vcproj @@ -88,7 +88,7 @@ @@ -127,9 +127,9 @@ /> diff --git a/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG_vc9.vcproj b/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG_vc9.vcproj index ba982efb..d6863402 100644 --- a/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG_vc9.vcproj +++ b/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC_vc9.vcproj b/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC_vc9.vcproj index 8e233686..8c534c27 100644 --- a/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC_vc9.vcproj +++ b/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC_vc9.vcproj @@ -89,7 +89,7 @@ diff --git a/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT_vc9.vcproj b/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT_vc9.vcproj index 1dd6de7f..7c709f5e 100644 --- a/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT_vc9.vcproj +++ b/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR_vc9.vcproj b/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR_vc9.vcproj index 6e63fc36..644625b2 100644 --- a/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR_vc9.vcproj +++ b/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN_vc9.vcproj b/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN_vc9.vcproj index 2ce0b7c4..9d7e0d5d 100644 --- a/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN_vc9.vcproj +++ b/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE_vc9.vcproj b/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE_vc9.vcproj index 70d0915f..bf2edea9 100644 --- a/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE_vc9.vcproj +++ b/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY_vc9.vcproj b/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY_vc9.vcproj index 90881ae9..f6af5545 100644 --- a/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY_vc9.vcproj +++ b/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC_vc9.vcproj b/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC_vc9.vcproj index 8bae9dac..3ebca1a9 100644 --- a/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC_vc9.vcproj +++ b/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC_vc9.vcproj @@ -89,7 +89,7 @@ diff --git a/FTNoIR_Tracker_FD/FTNoIR_Tracker_FD_vc9.vcproj b/FTNoIR_Tracker_FD/FTNoIR_Tracker_FD_vc9.vcproj index 784999c1..a37729e1 100644 --- a/FTNoIR_Tracker_FD/FTNoIR_Tracker_FD_vc9.vcproj +++ b/FTNoIR_Tracker_FD/FTNoIR_Tracker_FD_vc9.vcproj @@ -88,7 +88,7 @@ @@ -201,7 +201,18 @@ + + + @@ -322,7 +333,7 @@ SourceControlFiles="false" > + + + + Center + + + 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 @@ @@ -124,7 +124,7 @@ /> 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& 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 > contours; findContours(frame_bw.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); @@ -38,14 +56,66 @@ const vector& 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 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(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(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::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 diff --git a/FTNoIR_Tracker_UDP/FTNoIR_Tracker_UDP_vc9.vcproj b/FTNoIR_Tracker_UDP/FTNoIR_Tracker_UDP_vc9.vcproj index 88a6f16f..fa465ec9 100644 --- a/FTNoIR_Tracker_UDP/FTNoIR_Tracker_UDP_vc9.vcproj +++ b/FTNoIR_Tracker_UDP/FTNoIR_Tracker_UDP_vc9.vcproj @@ -88,7 +88,7 @@ diff --git a/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj b/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj index e7e9ce81..1d27b1e8 100644 --- a/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj +++ b/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj @@ -150,9 +150,9 @@ />