summaryrefslogtreecommitdiff
path: root/libmoped/src/sfm/SFM_TRACKING_DISPLAY.hpp
blob: a3f5ddc12b4eefb5d6ed7cb39b174411c941b9a1 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
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 );
			*/
			}
		}
	};
};