summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2010-06-22 18:51:07 -0400
committerGuillaume Seguin <guillaume@segu.in>2010-06-22 18:51:07 -0400
commit4d8601146ee5067a1aae2a9af6cc62b55a7a94f3 (patch)
tree70bb82fe946d2826ff2289d94369af2fb6c5385e
parent50358ba88693bf7fd6fa767b6a5e8800fe7cbe62 (diff)
downloadcmu-4d8601146ee5067a1aae2a9af6cc62b55a7a94f3.tar.gz
cmu-4d8601146ee5067a1aae2a9af6cc62b55a7a94f3.tar.bz2
Add tracking
-rw-r--r--libmoped/src/config.hpp7
-rw-r--r--libmoped/src/sfm/SFM_TRACKING.hpp117
-rw-r--r--libmoped/src/sfm/SFM_TRACKING_DISPLAY.hpp116
-rw-r--r--libmoped/src/util.hpp9
4 files changed, 248 insertions, 1 deletions
diff --git a/libmoped/src/config.hpp b/libmoped/src/config.hpp
index b42df9d..cb7b854 100644
--- a/libmoped/src/config.hpp
+++ b/libmoped/src/config.hpp
@@ -37,6 +37,10 @@
#include <filter/FILTER_PROJECTION_CPU.hpp>
//#include <filter/FILTER_DISPLAY.hpp>
+#include <sfm/SFM_TRACKING.hpp>
+#include <sfm/SFM_TRACKING_DISPLAY.hpp>
+//#include <sfm/SFM_RECONSTRUCTION.hpp> /* FIXME : TODO */
+
#include <STATUS_DISPLAY.hpp>
#include <GLOBAL_DISPLAY.hpp>
@@ -121,6 +125,9 @@ namespace MopedNS {
pipeline.addAlg( "FILTER2", new FILTER_PROJECTION_CPU( 8, 4096.) );
+ pipeline.addAlg( "TRACKING", new SFM_TRACKING() );
+ pipeline.addAlg( "TRACKING_DISPLAY", new SFM_TRACKING_DISPLAY(DEFAULT_DISPLAY_LEVEL) );
+
//pipeline.addAlg( "POSE2_DISPLAY", new POSE_DISPLAY( DEFAULT_DISPLAY_LEVEL ) );
diff --git a/libmoped/src/sfm/SFM_TRACKING.hpp b/libmoped/src/sfm/SFM_TRACKING.hpp
new file mode 100644
index 0000000..8bde249
--- /dev/null
+++ b/libmoped/src/sfm/SFM_TRACKING.hpp
@@ -0,0 +1,117 @@
+#pragma once
+
+#include <cv.h>
+#include <highgui.h>
+#include <iostream>
+#include <cstdio>
+#include <string>
+#include <sstream>
+#include <list>
+#include <vector>
+
+using namespace std;
+using namespace cv;
+
+#include "misc.h"
+#include "feature_detector.h"
+#include "descriptor_match.h"
+
+namespace MopedNS {
+
+ class SFM_TRACKING:public MopedAlg {
+
+ ShiTomashiFeatureDetector feature_detector;
+ KLTTracker feature_matcher;
+
+ IplImage *prevprev;
+ IplImage *prev;
+
+ vector<KeyPoint> key_points;
+
+ public:
+
+ SFM_TRACKING ()
+ : prevprev (NULL),
+ prev (NULL)
+ {}
+
+ void getConfig (map<string,string> &config) const {}
+
+ void setConfig (map<string,string> &config) {}
+
+ void processOne (FrameData &frameData, int i) {
+ Image *image = frameData.images[i].get ();
+ IplImage* img0 = cvCreateImage (cvSize (image->width,
+ image->height),
+ IPL_DEPTH_8U, 3);
+
+ for (int y = 0; y < image->height; y++) {
+ for (int x = 0; x < image->width; x++) {
+ img0->imageData[y*img0->widthStep+3*x + 0] = image->data[y*image->width + x];
+ img0->imageData[y*img0->widthStep+3*x + 1] = image->data[y*image->width + x];
+ img0->imageData[y*img0->widthStep+3*x + 2] = image->data[y*image->width + x];
+ }
+ }
+ IplImage* img2 = cvCreateImage (cvSize (image->width,
+ image->height),
+ IPL_DEPTH_8U, 1);
+ cvCvtColor (img0, img2, CV_BGR2GRAY);
+ if (!prev)
+ {
+ feature_detector.detect (img2, key_points);
+ feature_matcher.add (img2, key_points);
+ }
+ else
+ {
+ vector<Point2f> & new_points = frameData.trackingNewPoints[i];
+ vector<int> & old_points_indices = frameData.trackingPrevIndices[i];
+ frameData.trackedFeatures[i] = key_points;
+ feature_matcher.search (img2, new_points, old_points_indices);
+ }
+ if (prevprev)
+ cvReleaseImage (&prevprev);
+ prevprev = prev;
+ prev = img2;
+ feature_detector.detect(img2, key_points);
+ feature_matcher.add(img2, key_points);
+ }
+
+ void process (FrameData &frameData) {
+
+ for (int i = 0; i < (int) frameData.images.size(); i++)
+ {
+ processOne (frameData, i);
+ /*
+
+ vector<FrameData::DetectedFeature> &detectedFeatures = frameData.detectedFeatures[_stepName];
+
+ // Convert to a floating point image with pixels in range [0,1].
+ SFImage image = CreateImage(img->height, img->width);
+ for (int y = 0; y < img->height; y++)
+ for (int x = 0; x < img->width; x++)
+ image->pixels[y*image->stride+x] = ((float) img->data[img->width*y+x]) * 1./255.;
+
+ Keypoint keypts = GetKeypoints(image);
+ Keypoint key = keypts;
+ while (key) {
+
+ detectedFeatures.resize(detectedFeatures.size()+1);
+
+ detectedFeatures.back().imageIdx = i;
+
+ detectedFeatures.back().descriptor.resize(128);
+ for (int x=0; x<128; x++) detectedFeatures.back().descriptor[x] = key->descrip[x];
+
+ detectedFeatures.back().coord2D[0] = key->col;
+ detectedFeatures.back().coord2D[1] = key->row;
+
+ key = key->next;
+ }
+
+ FreeKeypoints(keypts);
+ DestroyAllImages(); // we can't destroy just one!
+ */
+ }
+ }
+ };
+};
diff --git a/libmoped/src/sfm/SFM_TRACKING_DISPLAY.hpp b/libmoped/src/sfm/SFM_TRACKING_DISPLAY.hpp
new file mode 100644
index 0000000..a3f5ddc
--- /dev/null
+++ b/libmoped/src/sfm/SFM_TRACKING_DISPLAY.hpp
@@ -0,0 +1,116 @@
+#pragma once
+
+#include "window_pair.h"
+
+namespace MopedNS {
+
+ class SFM_TRACKING_DISPLAY:public MopedAlg {
+
+ int display;
+
+ IplImage *prevprev;
+ IplImage *prev;
+
+ public:
+
+ SFM_TRACKING_DISPLAY(int display)
+ : display (display),
+ prevprev (NULL),
+ prev (NULL)
+ {}
+
+ void getConfig (map<string,string> &config) const {
+ GET_CONFIG(display );
+ }
+
+ void setConfig (map<string,string> &config) {
+ SET_CONFIG(display);
+ }
+
+ void processOne (FrameData &frameData, int i) {
+ Image *image = frameData.images[i].get ();
+ IplImage* img0 = cvCreateImage (cvSize (image->width,
+ image->height),
+ IPL_DEPTH_8U, 3);
+
+ for (int y = 0; y < image->height; y++) {
+ for (int x = 0; x < image->width; x++) {
+ img0->imageData[y*img0->widthStep+3*x + 0] = image->data[y*image->width + x];
+ img0->imageData[y*img0->widthStep+3*x + 1] = image->data[y*image->width + x];
+ img0->imageData[y*img0->widthStep+3*x + 2] = image->data[y*image->width + x];
+ }
+ }
+ IplImage* img = cvCreateImage (cvSize (image->width,
+ image->height),
+ IPL_DEPTH_8U, 1);
+ cvCvtColor (img0, img, CV_BGR2GRAY);
+ if (!prev)
+ {
+ prev = img;
+ return;
+ }
+ // Show the frames (with optional annotations)
+ WindowPair window_pair(prev, img, "Tracking");
+ vector<KeyPoint> & key_points = frameData.trackedFeatures[i];
+ vector<int> & old_points_indices = frameData.trackingPrevIndices[i];
+ vector<Point2f> & new_points = frameData.trackingNewPoints[i];
+ // Draw these keypoints
+ for (int i = 0; i < (int) old_points_indices.size (); ++i){
+ window_pair.DrawArrow (key_points[old_points_indices[i]].pt, new_points[i], CV_RGB(255,0,0));
+ }
+ prevprev = prev;
+ prev = img;
+ window_pair.Show();
+ }
+
+ void process(FrameData &frameData) {
+
+ if (display < 1) return;
+
+ clog << "FEATURES TRACKED: ";
+ foreach( tf, frameData.trackedFeatures )
+ clog << tf.first << " : " << tf.second.size() << " ";
+ clog << endl;
+
+ if (display < 2) return;
+
+ for( int i=0; i<(int)frameData.images.size(); i++) {
+ processOne (frameData, i);
+ /*
+ string windowName = _stepName + " #" + toString(i) + ":" + frameData.images[i]->name;
+
+ cvNamedWindow( windowName.c_str(), CV_WINDOW_AUTOSIZE);
+
+ IplImage* img = cvCreateImage(cvSize(frameData.images[i]->width,frameData.images[i]->height), IPL_DEPTH_8U, 3);
+
+ for (int y = 0; y < frameData.images[i]->height; y++) {
+ for (int x = 0; x < frameData.images[i]->width; x++) {
+ img->imageData[y*img->widthStep+3*x + 0] = frameData.images[i]->data[y*frameData.images[i]->width + x];
+ img->imageData[y*img->widthStep+3*x + 1] = frameData.images[i]->data[y*frameData.images[i]->width + x];
+ img->imageData[y*img->widthStep+3*x + 2] = frameData.images[i]->data[y*frameData.images[i]->width + x];
+ }
+ }
+
+ foreach( featureKind, frameData.detectedFeatures ) {
+
+ int objectHash = 0;
+ for(unsigned int x=0; x<featureKind.first.size(); x++) objectHash = objectHash ^ featureKind.first[x];
+ CvScalar color = objectColors[objectHash % 256];
+
+ foreach( corresp, featureKind.second ) {
+ if( corresp.imageIdx==i )
+ cvCircle(img, cvPoint( corresp.coord2D[0], corresp.coord2D[1]), 2, color , CV_FILLED, CV_AA );
+ }
+ }
+
+ cvShowImage (windowName.c_str(), img);
+
+ cvReleaseImage(&img);
+ }
+
+ cvWaitKey( 10 );
+ */
+ }
+ }
+ };
+};
diff --git a/libmoped/src/util.hpp b/libmoped/src/util.hpp
index abcac09..2937336 100644
--- a/libmoped/src/util.hpp
+++ b/libmoped/src/util.hpp
@@ -6,6 +6,9 @@
#include <cv.h>
#include <highgui.h>
+using namespace std;
+using namespace cv;
+
#define foreach( i, c ) for( typeof((c).begin()) i##_hid=(c).begin(), *i##_hid2=((typeof((c).begin())*)1); i##_hid2 && i##_hid!=(c).end(); ++i##_hid) for( typeof( *(c).begin() ) &i=*i##_hid, *i##_hid3=(typeof( *(c).begin() )*)(i##_hid2=NULL); !i##_hid3 ; ++i##_hid3, ++i##_hid2)
#define eforeach( i, it, c ) for( typeof((c).begin()) it=(c).begin(), i##_hid = (c).begin(), *i##_hid2=((typeof((c).begin())*)1); i##_hid2 && it!=(c).end(); (it==i##_hid)?++it,++i##_hid:i##_hid=it) for( typeof(*(c).begin()) &i=*it, *i##_hid3=(typeof( *(c).begin() )*)(i##_hid2=NULL); !i##_hid3 ; ++i##_hid3, ++i##_hid2)
@@ -46,7 +49,11 @@ namespace MopedNS {
typedef list<int> Cluster;
vector<SP_Image> images;
-
+
+ /* Features in the /previous/ image */
+ map< int, vector<KeyPoint> > trackedFeatures;
+ map< int, vector<int> > trackingPrevIndices;
+ map< int, vector<Point2f> > trackingNewPoints;
map< string, vector< DetectedFeature > > detectedFeatures;
vector< vector< Match > > matches;