├── .gitignore
├── CMakeLists.txt
├── README.md
├── my_slam_prototype.cpp
├── my_slam_tracking.cpp
├── my_slam_tracking.h
└── run.txt


/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | x64/


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.11)
 2 | project(slamPrototype)
 3 | 
 4 | set(CMAKE_INCLUDE_CURRENT_DIR ON)
 5 | 
 6 | find_package(OpenCV REQUIRED COMPONENTS core calib3d imgproc imgcodecs highgui )
 7 | include_directories($(OpenCV_INCLUDE_DIRS))
 8 | 
 9 | find_package(PCL 1.8.0 REQUIRED COMPONENTS common io registration segmentation visualization)
10 | include_directories(${PCL_INCLUDE_DIRS})
11 | 
12 | 
13 | set(SOURCES my_slam_prototype.cpp my_slam_tracking.cpp my_slam_tracking.h)
14 | 
15 | add_executable(slamPrototype ${SOURCES} )
16 | target_link_libraries(slamPrototype ${OpenCV_LIBS} ${PCL_LIBRARIES})
17 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Stereo SLAM based on OpenCV and PCL
 2 | This is a C++ Stereo SLAM prototype implementation based on OpenCV and PCL.
 3 | The system requires two stereo calibrated USB webcams.
 4 | 
 5 | ## Approach
 6 | In each frame we extract ORB features and match them with the features of previous frames.
 7 | If matches are found along the epipolar line, we triangulate the points and determine their 3D position.
 8 | 
 9 | Based on the 3D-3D correspondences, we estimate the transformation between the frames by SVD decomposition with RANSAC.
10 | 
11 | Finally, the active points are transformed and merged into a PCL point cloud.
12 | 
13 | ## Calibration
14 | We use the [OpenCV Stereo calibration routine](https://github.com/opencv/opencv/blob/master/samples/cpp/stereo_calib.cpp) that can be found in the official OpenCV example directory.
15 | 
16 | ## Issues
17 | The implementation does not include global optimization.
18 | Estimation errors are not reduced and will be summed up over time.
19 | In order to compensate errors, further investigations in pose graph optimization (such as implemented in the g2o library) would be required.


--------------------------------------------------------------------------------
/my_slam_prototype.cpp:
--------------------------------------------------------------------------------
  1 | #include <boost/thread/thread.hpp>
  2 | 
  3 | #include "opencv2/calib3d/calib3d.hpp"
  4 | #include "opencv2/imgproc.hpp"
  5 | #include "opencv2/imgcodecs.hpp"
  6 | #include "opencv2/highgui.hpp"
  7 | #include "opencv2/core/utility.hpp"
  8 | #include "opencv2/xfeatures2d.hpp"
  9 | 
 10 | #include <pcl/common/common_headers.h>
 11 | #include <pcl/features/normal_3d.h>
 12 | #include <pcl/io/pcd_io.h>
 13 | #include <pcl/visualization/pcl_visualizer.h>
 14 | #include <pcl/console/parse.h>
 15 | #include <pcl/registration/icp.h>
 16 | 
 17 | #include <iostream>
 18 | #include <stdio.h>
 19 | #include <cmath>
 20 | 
 21 | #include "my_slam_tracking.h"
 22 | 
 23 | 
 24 | using namespace cv;
 25 | using namespace std;
 26 | using namespace pcl;
 27 | 
 28 | enum { STEREO_BM = 0, STEREO_SGBM = 1, STEREO_HH = 2, STEREO_VAR = 3, STEREO_3WAY = 4 };
 29 | 
 30 | Size img_size = Size(640, 480);
 31 | 
 32 | // Stereo parameters
 33 | Mat Q;
 34 | Mat P1, P2;
 35 | Rect roi1, roi2;
 36 | Mat map11, map12, map21, map22;
 37 | float scale;
 38 | 
 39 | // Stereo matcher parameters
 40 | int alg = STEREO_SGBM;
 41 | int uniquenessRatio = 1;
 42 | int speckleWindowSize = 10;
 43 | int speckleRange = 80;
 44 | 
 45 | int SADWindowSize, numberOfDisparities, blockSize;
 46 | 
 47 | string calibration_filename = "";
 48 | string disparity_filename = "";
 49 | string point_cloud_filename = "";
 50 | 
 51 | string cloud_name = "Scene";
 52 | 
 53 | 
 54 | static void print_help()
 55 | {
 56 | 	printf("\nStereo matching converting L and R images into disparity and point clouds\n");
 57 | 	printf("\nUsage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh|sgbm3way] [--blocksize=<block_size>]\n"
 58 | 		"[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>]\n"
 59 | 		"[--no-display] [-o=<disparity_image>] [-p=<point_cloud_file>]\n");
 60 | }
 61 | 
 62 | 
 63 | boost::shared_ptr<pcl::visualization::PCLVisualizer> rgb_vis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, string cloud_name)
 64 | {
 65 | 	// --------------------------------------------
 66 | 	// -----Open 3D viewer and add point cloud-----
 67 | 	// --------------------------------------------
 68 | 	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
 69 | 	viewer->setBackgroundColor(0, 0, 0);
 70 | 	//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
 71 | 	//viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, cloud_name);
 72 | 	viewer->addPointCloud<pcl::PointXYZ>(cloud, cloud_name);
 73 | 	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloud_name);
 74 | 	viewer->addCoordinateSystem(1.0);
 75 | 	viewer->initCameraParameters();
 76 | 	viewer->setCameraPosition(0, 0, -1, 0, -1, 0);
 77 | 	return (viewer);
 78 | }
 79 | 
 80 | 
 81 | void filter_stereo_features(const vector<cv::DMatch>& matches, vector<cv::KeyPoint>& keypoints1, vector<cv::KeyPoint>& keypoints2, vector<cv::DMatch>& goodMatches, double maxYDistance)
 82 | {
 83 | 	if (matches.size() == 0) return;
 84 | 
 85 | 	goodMatches.clear();
 86 | 
 87 | 	for (vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it)
 88 | 	{
 89 | 		// Get the position of left keypoints
 90 | 		float xl = keypoints1[it->queryIdx].pt.x;
 91 | 		float yl = keypoints1[it->queryIdx].pt.y;
 92 | 
 93 | 		// Get the position of right keypoints
 94 | 		float xr = keypoints2[it->trainIdx].pt.x;
 95 | 		float yr = keypoints2[it->trainIdx].pt.y;
 96 | 
 97 | 		if (abs(yl - yr) <= maxYDistance) {
 98 | 			goodMatches.push_back(*it);
 99 | 		}
100 | 	}
101 | }
102 | 
103 | void stereo_match(const Mat &img1, const Mat &img2, Mat &disparity, pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud) {
104 | 	numberOfDisparities = ((img_size.width / 8) + 15) & -16;
105 | 
106 | 	// Stereo matchers
107 | 	Ptr<StereoBM> bm = StereoBM::create(16, 9);
108 | 	Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
109 | 
110 | 	bm->setROI1(roi1);
111 | 	bm->setROI2(roi2);
112 | 	bm->setPreFilterCap(61);
113 | 	bm->setBlockSize(blockSize > 0 ? blockSize : 9);
114 | 	bm->setMinDisparity(-39);
115 | 	bm->setNumDisparities(numberOfDisparities);
116 | 	bm->setTextureThreshold(507);
117 | 	bm->setUniquenessRatio(0);
118 | 	bm->setSpeckleWindowSize(0);
119 | 	bm->setSpeckleRange(8);
120 | 	bm->setDisp12MaxDiff(1);
121 | 
122 | 	sgbm->setPreFilterCap(10);
123 | 	int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
124 | 	sgbm->setBlockSize(sgbmWinSize);
125 | 
126 | 	int cn = img1.channels();
127 | 
128 | 	sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
129 | 	sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
130 | 	sgbm->setMinDisparity(0);
131 | 	sgbm->setNumDisparities(numberOfDisparities);
132 | 	sgbm->setUniquenessRatio(uniquenessRatio);
133 | 	sgbm->setSpeckleWindowSize(speckleWindowSize);
134 | 	sgbm->setSpeckleRange(speckleRange);
135 | 	sgbm->setDisp12MaxDiff(1);
136 | 
137 | 	if (alg == STEREO_HH)
138 | 		sgbm->setMode(StereoSGBM::MODE_HH);
139 | 	else if (alg == STEREO_SGBM)
140 | 		sgbm->setMode(StereoSGBM::MODE_SGBM);
141 | 	else if (alg == STEREO_3WAY)
142 | 		sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);
143 | 
144 | 	Mat disp;
145 | 
146 | 	if (alg == STEREO_BM)
147 | 		bm->compute(img1, img2, disp);
148 | 	else if (alg == STEREO_SGBM || alg == STEREO_HH || alg == STEREO_3WAY)
149 | 		sgbm->compute(img1, img2, disp);
150 | 
151 | 	if (alg != STEREO_VAR)
152 | 		disp.convertTo(disparity, CV_8U, 255 / (numberOfDisparities*16.));
153 | 	else
154 | 		disp.convertTo(disparity, CV_8U);
155 | 
156 | 	Mat xyz;
157 | 	cv::reprojectImageTo3D(disp, xyz, Q, true);
158 | 
159 | 	// --- Create point cloud
160 | 	const double max_z = 1.0e4;
161 | 	for (int y = 0; y < xyz.rows; y++)
162 | 	{
163 | 		for (int x = 0; x < xyz.cols; x++)
164 | 		{
165 | 			Vec3f point = xyz.at<Vec3f>(y, x);
166 | 			if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
167 | 
168 | 			double X = point[0];
169 | 			double Y = point[1];
170 | 			double Z = point[2];
171 | 
172 | 			//pcl::PointXYZRGB rgbPoint;
173 | 			pcl::PointXYZ rgbPoint;
174 | 			rgbPoint.x = X;
175 | 			rgbPoint.y = Y;
176 | 			rgbPoint.z = Z;
177 | 
178 | 			//Vec3b colorLeft = img1.at<Vec3b>(y, x);
179 | 
180 | 			//uint32_t rgb = (static_cast<uint32_t>(colorLeft.val[2]) << 16 |
181 | 			//	static_cast<uint32_t>(colorLeft.val[1]) << 8 | static_cast<uint32_t>(colorLeft.val[0]));
182 | 			//rgbPoint.rgb = *reinterpret_cast<float*>(&rgb);
183 | 
184 | 			point_cloud->points.push_back(rgbPoint);
185 | 		}
186 | 	}
187 | }
188 | 
189 | void tracks_to_pointcloud(const std::list<slam::FeatureTrack> &tracks, pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud) {
190 | 
191 | 	const double max_z = 1.0e4;
192 | 
193 | 	std::list<slam::FeatureTrack>::const_iterator track_it;
194 | 	int i;
195 | 	for (i = 0, track_it = tracks.begin(); track_it != tracks.end(); i++, track_it++) {
196 | 
197 | 		if (track_it->missed_frames == 0) {
198 | 			Vec3f point = track_it->active_position_3d;
199 | 
200 | 			if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
201 | 
202 | 			pcl::PointXYZ rgbPoint;
203 | 			rgbPoint.x = point[0];
204 | 			rgbPoint.y = point[1];
205 | 			rgbPoint.z = point[2];
206 | 
207 | 			point_cloud->points.push_back(rgbPoint);
208 | 		}
209 | 	}
210 | }
211 | 
212 | 
213 | int main(int argc, char** argv)
214 | {
215 | 
216 | 	cv::CommandLineParser parser(argc, argv,
217 | 		"{@arg1||}{@arg2||}{help h||}{algorithm||}{max-disparity|0|}{blocksize|0|}{scale|1|}{i||}{e||}{o||}{p||}");
218 | 
219 | 	if (parser.has("help"))
220 | 	{
221 | 		print_help();
222 | 		return 0;
223 | 	}
224 | 
225 | 	if (parser.has("algorithm"))
226 | 	{
227 | 		string _alg = parser.get<string>("algorithm");
228 | 		alg = _alg == "bm" ? STEREO_BM :
229 | 			_alg == "sgbm" ? STEREO_SGBM :
230 | 			_alg == "hh" ? STEREO_HH :
231 | 			_alg == "var" ? STEREO_VAR :
232 | 			_alg == "sgbm3way" ? STEREO_3WAY : -1;
233 | 	}
234 | 
235 | 	numberOfDisparities = parser.get<int>("max-disparity");
236 | 	SADWindowSize = parser.get<int>("blocksize");
237 | 	scale = parser.get<float>("scale");
238 | 
239 | 	if (parser.has("i"))
240 | 		calibration_filename = parser.get<string>("i");
241 | 	if (parser.has("o"))
242 | 		disparity_filename = parser.get<string>("o");
243 | 	if (parser.has("p"))
244 | 		point_cloud_filename = parser.get<string>("p");
245 | 
246 | 	if (!parser.check())
247 | 	{
248 | 		parser.printErrors();
249 | 		return 1;
250 | 	}
251 | 	if (alg < 0)
252 | 	{
253 | 		printf("Command-line parameter error: Unknown stereo algorithm\n\n");
254 | 		print_help();
255 | 		return -1;
256 | 	}
257 | 	if (numberOfDisparities % 16 != 0)
258 | 	{
259 | 		printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
260 | 		print_help();
261 | 		return -1;
262 | 	}
263 | 	if (scale < 0)
264 | 	{
265 | 		printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
266 | 		return -1;
267 | 	}
268 | 	if (SADWindowSize != 0 && SADWindowSize % 2 != 1)
269 | 	{
270 | 		printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
271 | 		return -1;
272 | 	}
273 | 	if (calibration_filename.empty())
274 | 	{
275 | 		printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to triangulate 3d points\n");
276 | 		return -1;
277 | 	}
278 | 
279 | 	if (!calibration_filename.empty())
280 | 	{
281 | 		// reading intrinsic parameters
282 | 		FileStorage fs(calibration_filename, FileStorage::READ);
283 | 		if (!fs.isOpened())
284 | 		{
285 | 			printf("Failed to open file %s\n", calibration_filename.c_str());
286 | 			return -1;
287 | 		}
288 | 
289 | 		fs["P1"] >> P1;
290 | 		fs["P2"] >> P2;
291 | 		fs["Q"] >> Q;
292 | 
293 | 		Mat M1, D1, M2, D2;
294 | 		fs["CM1"] >> M1;
295 | 		fs["D1"] >> D1;
296 | 		fs["CM2"] >> M2;
297 | 		fs["D2"] >> D2;
298 | 
299 | 		M1 *= scale;
300 | 		M2 *= scale;
301 | 
302 | 		Mat R, T, R1, R2;
303 | 		fs["R"] >> R;
304 | 		fs["T"] >> T;
305 | 		fs["R1"] >> R1;
306 | 		fs["R2"] >> R2;
307 | 
308 | 		initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
309 | 		initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
310 | 	}
311 | 
312 | 
313 | 	VideoCapture capA, capB;
314 | 
315 | 	if (!capA.open(0) || !capB.open(1))
316 | 		return 0;
317 | 
318 | 	pcl::PointCloud<pcl::PointXYZ>::Ptr scene_point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
319 | 	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
320 | 
321 | 	viewer = rgb_vis(scene_point_cloud, cloud_name);
322 | 
323 | 	bool do_tracking;
324 | 	long frame_count = 0;
325 | 	bool initialized = false;
326 | 
327 | 	Mat prev_img1;
328 | 
329 | 	slam::TrackingSharedData shared;
330 | 
331 | 	while (1)
332 | 	{
333 | 		Mat frameAraw, frameBraw, frameA, frameB;
334 | 
335 | 		capA >> frameAraw;
336 | 		capB >> frameBraw;
337 | 
338 | 		if (frameAraw.empty() || frameBraw.empty()) continue;
339 | 
340 | 		frame_count++;
341 | 
342 | 		do_tracking = frame_count > 10;
343 | 
344 | 		if (!do_tracking) continue;
345 | 
346 | 		// Remapping stereo image pair to share same epipolar lines
347 | 		Mat img1, img2;
348 | 
349 | 		if (!calibration_filename.empty())
350 | 		{
351 | 			remap(frameAraw, img1, map11, map12, INTER_LINEAR);
352 | 			remap(frameBraw, img2, map21, map22, INTER_LINEAR);
353 | 		}
354 | 		else {
355 | 			img1 = frameAraw;
356 | 			img2 = frameBraw;
357 | 		}
358 | 
359 | 
360 | 		//////////////// Matching features test (stereo) ////////////////////
361 | 
362 | 		// Extracting features
363 | 		/*Ptr<cv::ORB> detectorORB = ORB::create();
364 | 		vector<KeyPoint> keypoints1, keypoints2;
365 | 		Mat descriptors1, descriptors2;
366 | 
367 | 		cout << "detectStart\n";
368 | 		detectorORB->detectAndCompute(img1, noArray(), keypoints1, descriptors1);
369 | 		detectorORB->detectAndCompute(img2, noArray(), keypoints2, descriptors2);
370 | 		cout << "detectEnd\n";
371 | 
372 | 		// Match keypoints
373 | 		vector<DMatch> matches, goodMatches;
374 | 
375 | 		if (keypoints1.size() == 0 || keypoints2.size() == 0) continue;
376 | 
377 | 		cout << "matchStart\n";
378 | 		//Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
379 | 		//matcher->match(descriptors1, descriptors2, matches, noArray());
380 | 		//matchFeatures(descriptors1, descriptors2, matches);
381 | 
382 | 		cout << "matchEnd\n";
383 | 
384 | 		cout << "Cols: " << descriptors1.cols << endl;
385 | 
386 | 		if (matches.size() == 0) continue;
387 | 
388 | 		// Filter features along y-axis
389 | 		double maxYDistance = 10;
390 | 		filterFeatures(matches, keypoints1, keypoints2, goodMatches, maxYDistance);
391 | 
392 | 		if (goodMatches.size() == 0) continue;
393 | 
394 | 		// Draw stereo matches
395 | 		Mat res;
396 | 		cout << "drawStart\n";
397 | 		drawMatches(img1, keypoints1, img2, keypoints2, goodMatches, res, Scalar(255, 0, 0), Scalar(255, 0, 0));
398 | 		imshow("stereo matches", res);
399 | 		cout << "drawEnd\n";*/
400 | 
401 | 
402 | 		/////////////// Matching features test (time) /////////////////////
403 | 
404 | 		/*if (prev_img1.empty()) {
405 | 			prev_img1 = img1.clone();
406 | 			continue;
407 | 		}
408 | 
409 | 		Ptr<cv::ORB> detectorORB = ORB::create();
410 | 		vector<KeyPoint> keypoints1, keypoints2;
411 | 		Mat descriptors1, descriptors2;
412 | 
413 | 		cout << "detectStart\n";
414 | 		detectorORB->detectAndCompute(img1, noArray(), keypoints1, descriptors1);
415 | 		detectorORB->detectAndCompute(prev_img1, noArray(), keypoints2, descriptors2);
416 | 		cout << "detectEnd\n";
417 | 
418 | 		// Match keypoints
419 | 		vector<DMatch> matches, goodMatches;
420 | 
421 | 		if (keypoints1.size() == 0 || keypoints2.size() == 0) continue;
422 | 
423 | 		cout << "matchStart\n";
424 | 		Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
425 | 		matcher->match(descriptors1, descriptors2, matches, noArray());
426 | 
427 | 		cout << "matchEnd\n";
428 | 
429 | 		cout << "Cols: " << descriptors1.cols << endl;
430 | 
431 | 		if (matches.size() == 0) continue;
432 | 
433 | 		// Filter features along y-axis
434 | 		double maxYDistance = 10;
435 | 		filterFeatures(matches, keypoints1, keypoints2, goodMatches, maxYDistance);
436 | 		//goodMatches = matches;
437 | 
438 | 		if (goodMatches.size() == 0) continue;
439 | 
440 | 		// Draw stereo matches
441 | 		Mat res;
442 | 		cout << "drawStart\n";
443 | 		drawMatches(img1, keypoints1, prev_img1, keypoints2, goodMatches, res, Scalar(255, 0, 0), Scalar(255, 0, 0));
444 | 		imshow("stereo matches", res);
445 | 		cout << "drawEnd\n";
446 | 
447 | 		prev_img1 = img1.clone();*/
448 | 
449 | 		////////////////////////////////////
450 | 
451 | 
452 | 
453 | 		// Extracting features
454 | 		Ptr<cv::ORB> detectorORB = ORB::create();
455 | 		vector<KeyPoint> keypoints1, keypoints2;
456 | 		Mat descriptors1, descriptors2;
457 | 
458 | 		detectorORB->detectAndCompute(img1, noArray(), keypoints1, descriptors1);
459 | 		detectorORB->detectAndCompute(img2, noArray(), keypoints2, descriptors2);
460 | 
461 | 
462 | 		// Match features (stereo)
463 | 		if (keypoints1.size() == 0 || keypoints2.size() == 0) continue;
464 | 
465 | 		vector<DMatch> matches, goodMatches;
466 | 
467 | 		Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
468 | 		matcher->match(descriptors1, descriptors2, matches, noArray());
469 | 
470 | 		if (matches.size() == 0) continue;
471 | 
472 | 		// Filter features to share same epipolar line
473 | 		double maxYDistance = 10;
474 | 		filter_stereo_features(matches, keypoints1, keypoints2, goodMatches, maxYDistance);
475 | 
476 | 		if (goodMatches.size() == 0) continue;
477 | 
478 | 		// Draw stereo matches
479 | 		Mat res;
480 | 		drawMatches(img1, keypoints1, img2, keypoints2, goodMatches, res, Scalar(255, 0, 0), Scalar(255, 0, 0));
481 | 		imshow("Stereo Matches", res);
482 | 
483 | 
484 | 
485 | 		// Match features with tracks (temporal)
486 | 		std::vector<int> match_idx;
487 | 		slam::TrackingModule::match_features(shared.tracks, descriptors1, match_idx);
488 | 
489 | 		cout << "Tracks:" << shared.tracks.size() << endl;
490 | 
491 | 
492 | 		// Triangulate stereo features
493 | 		std::vector<cv::Point3f> pnts3D;
494 | 		slam::TrackingModule::triangulate_matches(matches, keypoints1, keypoints2, P1, P2, pnts3D);
495 | 
496 | 		// Update tracks
497 | 		slam::TrackingModule::update_tracks(shared.tracks, keypoints1, descriptors1, match_idx, pnts3D);
498 | 
499 | 		// Draw active, tracked and triangulated features
500 | 		Mat keypoints_img;
501 | 
502 | 		std::vector<cv::KeyPoint> tracked_keypoints1;
503 | 		slam::TrackingModule::get_tracked_keypoints(keypoints1, tracked_keypoints1, match_idx);
504 | 
505 | 		std::vector<cv::KeyPoint> triangulated_keypoints1;
506 | 		slam::TrackingModule::get_triangulated_keypoints(shared.tracks, keypoints1, triangulated_keypoints1, match_idx, pnts3D);
507 | 
508 | 		drawKeypoints(img1, keypoints1, keypoints_img, Scalar(0, 255, 0));
509 | 		drawKeypoints(keypoints_img, tracked_keypoints1, keypoints_img, Scalar(0, 0, 255));
510 | 		drawKeypoints(keypoints_img, triangulated_keypoints1, keypoints_img, Scalar(255, 0, 0));
511 | 		cv::imshow("Active(Green), Tracked(Red) and Triangulated(Blue) Keypoints", keypoints_img);
512 | 
513 | 		// Create new view
514 | 		bool add_view = false;
515 | 		slam::TrackedView new_view;
516 | 		cv::Mat3f pointmap;
517 | 		if (shared.views.empty()) {
518 | 			new_view.R = shared.base_R;
519 | 			new_view.T = shared.base_T;
520 | 			add_view = true;
521 | 		}
522 | 		else {
523 | 			float movement = slam::TrackingModule::get_median_feature_movement(shared.tracks);
524 | 
525 | 			cout << "Feature movement:" << movement << endl;
526 | 
527 | 			if (movement > 120) {
528 | 				std::cout << "Movement is " << movement << "! Computing transformation...";
529 | 
530 | 				cv::Matx33f stepR;
531 | 				cv::Matx31f stepT;
532 | 				slam::TrackingModule::transformation_from_tracks(shared.tracks, stepR, stepT);
533 | 				new_view.R = shared.base_R * stepR;
534 | 				new_view.T = shared.base_T + shared.base_R*stepT;
535 | 				add_view = true;
536 | 			}
537 | 		}
538 | 
539 | 		if (add_view) {
540 | 			shared.views.push_back(new_view);
541 | 			shared.base_R = new_view.R;
542 | 			shared.base_T = new_view.T;
543 | 
544 | 			shared.tracks.clear();
545 | 
546 | 			for (unsigned int i = 0; i < keypoints1.size(); i++)
547 | 			{
548 | 				slam::FeatureTrack track;
549 | 				track.base_position = keypoints1[i].pt;
550 | 				track.base_position_3d = pnts3D[i];
551 | 
552 | 				track.active_position = keypoints1[i].pt;
553 | 				track.active_position_3d = pnts3D[i];
554 | 
555 | 				descriptors1.row(i).copyTo(track.descriptor);
556 | 
557 | 				track.missed_frames = 0;
558 | 
559 | 				shared.tracks.push_back(track);
560 | 			}
561 | 
562 | 			// Create point cloud
563 | 			Mat disparity, xyz;
564 | 			pcl::PointCloud<pcl::PointXYZ>::Ptr active_point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
565 | 
566 | 			//stereo_match(img1, img2, disparity, active_point_cloud);
567 | 			tracks_to_pointcloud(shared.tracks, active_point_cloud);
568 | 
569 | 
570 | 			Eigen::Matrix4f active_camera_pose = Eigen::Matrix4f::Identity();
571 | 
572 | 			//   M11 M21 M31 T1
573 | 			//   M12 M22 M32 T2
574 | 			//   M13 M23 M33 T3
575 | 
576 | 			// rotation	
577 | 			active_camera_pose(0, 0) = shared.base_R(0, 0);
578 | 			active_camera_pose(0, 1) = shared.base_R(0, 1);
579 | 			active_camera_pose(0, 2) = shared.base_R(0, 2);
580 | 			active_camera_pose(1, 0) = shared.base_R(1, 0);
581 | 			active_camera_pose(1, 1) = shared.base_R(1, 1);
582 | 			active_camera_pose(1, 2) = shared.base_R(1, 2);
583 | 			active_camera_pose(2, 0) = shared.base_R(2, 0);
584 | 			active_camera_pose(2, 1) = shared.base_R(2, 1);
585 | 			active_camera_pose(2, 2) = shared.base_R(2, 2);
586 | 
587 | 			// translation
588 | 			active_camera_pose(0, 3) = shared.base_T(0, 0);
589 | 			active_camera_pose(1, 3) = shared.base_T(1, 0);
590 | 			active_camera_pose(2, 3) = shared.base_T(2, 0);
591 | 
592 | 			//shared.base_R * shared.base_T;
593 | 			printf("Camera pose:\n");
594 | 			cout << active_camera_pose << endl;
595 | 
596 | 			pcl::PointCloud<pcl::PointXYZ>::Ptr active_point_cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
597 | 			pcl::transformPointCloud(*active_point_cloud, *active_point_cloud_transformed, active_camera_pose);
598 | 
599 | 
600 | 			// Add the active point cloud data to the scene cloud
601 | 			*scene_point_cloud += *active_point_cloud_transformed;
602 | 
603 | 			// Draw disparity map
604 | 			/*Mat colorDisparity;
605 | 			applyColorMap(disparity, colorDisparity, cv::ColormapTypes::COLORMAP_JET);
606 | 			imshow("disparity", colorDisparity);*/
607 | 
608 | 			// Display point cloud
609 | 			//viewer->updatePointCloud(point_cloud_ptr, cloud_name);
610 | 			viewer->removePointCloud(cloud_name);
611 | 			viewer->addPointCloud<pcl::PointXYZ>(scene_point_cloud, cloud_name);
612 | 			viewer->spinOnce(100);
613 | 		}
614 | 
615 | 		char key = waitKey(10);
616 | 		printf("\n");
617 | 	}
618 | 
619 | 	return 0;
620 | }
621 | 


--------------------------------------------------------------------------------
/my_slam_tracking.cpp:
--------------------------------------------------------------------------------
  1 | #include "my_slam_tracking.h"
  2 | 
  3 | // Ratio to the second neighbor to consider a good match.
  4 | #define RATIO    0.6
  5 | 
  6 | // Max z-value (depth in mm)
  7 | // 40*stereobaseline is usually a good value
  8 | #define MAX_Z 3800
  9 | 
 10 | using namespace cv;
 11 | using namespace std;
 12 | 
 13 | namespace slam {
 14 | 
 15 | 	TrackingSharedData::TrackingSharedData() :
 16 | 		is_data_new(false),
 17 | 		is_tracking_enabled(true),
 18 | 		base_rgb(new cv::Mat3b(480, 640)),
 19 | 		//base_pointmap(new cv::Mat3f(480, 640)),
 20 | 		active_rgb(new cv::Mat3b(480, 640)),
 21 | 		//active_depth(new cv::Mat1s(480, 640)),
 22 | 		base_R(1, 0, 0, 0, 1, 0, 0, 0, 1),
 23 | 		base_T(0, 0, 0)
 24 | 	{
 25 | 	}
 26 | 
 27 | 	TrackingSharedData::~TrackingSharedData() {
 28 | 		delete base_rgb;
 29 | 		//delete base_pointmap;
 30 | 		delete active_rgb;
 31 | 		//delete active_depth;
 32 | 	}
 33 | 
 34 | 	/**
 35 | 	Matches new ORB features with tracks
 36 | 	*/
 37 | 	void TrackingModule::match_features(const std::list<FeatureTrack> &tracks, const cv::Mat &new_descriptors, std::vector<int> &match_idx) {
 38 | 
 39 | 		// Create a FlannMatcher based on values provided in docs
 40 | 		FlannBasedMatcher matcher(new cv::flann::LshIndexParams(12, 20, 2));
 41 | 
 42 | 		vector<Mat> train_vector;
 43 | 		train_vector.push_back(new_descriptors);
 44 | 
 45 | 		matcher.add(train_vector);
 46 | 
 47 | 		match_idx.resize(tracks.size());
 48 | 
 49 | 		int match_count = 0;
 50 | 		list<slam::FeatureTrack>::const_iterator track_it;
 51 | 		int i;
 52 | 
 53 | 		for (i = 0, track_it = tracks.begin(); track_it != tracks.end(); i++, track_it++) {
 54 | 			vector<vector<DMatch>> matches;
 55 | 			matcher.knnMatch(track_it->descriptor, matches, 2);
 56 | 
 57 | 			if (matches[0].size() >= 2) {
 58 | 				// Do a second neighbor ratio test
 59 | 				float best_dist = matches[0][0].distance;
 60 | 				float next_dist = matches[0][1].distance;
 61 | 
 62 | 				if (best_dist < next_dist * RATIO) {
 63 | 					match_idx[i] = matches[0][0].trainIdx;
 64 | 					match_count++;
 65 | 				}
 66 | 				else {
 67 | 					match_idx[i] = -1;
 68 | 				}
 69 | 			}
 70 | 		}
 71 | 
 72 | 		cout << "Matched features:" << match_count << endl;
 73 | 	}
 74 | 
 75 | 	void TrackingModule::update_tracks(std::list<FeatureTrack> &tracks, const std::vector<cv::KeyPoint> &feature_points,
 76 | 		const cv::Mat &feature_descriptors, std::vector<int> &match_idx, std::vector<cv::Point3f>& pnts3D) {
 77 | 
 78 | 		std::list<FeatureTrack>::iterator track_it;
 79 | 		int i, updated = 0, missed = 0;
 80 | 		for (i = 0, track_it = tracks.begin(); track_it != tracks.end(); i++, track_it++) {
 81 | 			int j = match_idx[i];
 82 | 			if (j >= 0) {
 83 | 
 84 | 				Point3d point = pnts3D[j];
 85 | 
 86 | 				if (point.x != 0 && point.y != 0 && point.z != 0) {
 87 | 					track_it->missed_frames = 0;
 88 | 					track_it->active_position = feature_points[j].pt;
 89 | 					track_it->active_position_3d = pnts3D[j];
 90 | 					feature_descriptors.row(j).copyTo(track_it->descriptor);
 91 | 					updated++;
 92 | 					continue;
 93 | 				}
 94 | 			}
 95 | 			// If not updated yet
 96 | 			track_it->missed_frames++;
 97 | 			missed++;
 98 | 		}
 99 | 
100 | 		cout << "Updated Tracks:" << updated << " Missed:" << missed;
101 | 
102 | 		// Delete tracks
103 | 		tracks.remove_if(TrackingModule::is_track_stale);
104 | 	}
105 | 
106 | 	bool TrackingModule::is_track_stale(const FeatureTrack &track) {
107 | 		return track.missed_frames > 10;
108 | 	}
109 | 
110 | 	float TrackingModule::get_median_feature_movement(const std::list<FeatureTrack> &tracks) {
111 | 
112 | 		std::vector<float> vals;
113 | 		std::list<FeatureTrack>::const_iterator track_it;
114 | 		for (track_it = tracks.begin(); track_it != tracks.end(); track_it++) {
115 | 
116 | 			if (track_it->missed_frames == 0) {
117 | 				// Diff. of base and active position
118 | 				vals.push_back(fabs(track_it->base_position.x - track_it->active_position.x) +
119 | 					fabs(track_it->base_position.y - track_it->active_position.y));
120 | 			}
121 | 		}
122 | 
123 | 		if (vals.empty())
124 | 			return 0;
125 | 		else {
126 | 			int n = vals.size() / 2;
127 | 
128 | 			// Sort vector
129 | 			std::nth_element(vals.begin(), vals.begin() + n, vals.end());
130 | 
131 | 			return vals[n];
132 | 		}
133 | 	}
134 | 
135 | 	void TrackingModule::transformation_from_tracks(const std::list<FeatureTrack> &tracks, cv::Matx33f &R, cv::Matx31f &T) {
136 | 		std::list<FeatureTrack>::const_iterator track_it;
137 | 
138 | 		cv::Mat1f X(0, 3), Y(0, 3);
139 | 		X.reserve(tracks.size());
140 | 		Y.reserve(tracks.size());
141 | 
142 | 		for (track_it = tracks.begin(); track_it != tracks.end(); track_it++) {
143 | 
144 | 			if (track_it->missed_frames != 0)
145 | 				continue;
146 | 
147 | 			const cv::Point3f &base_point = track_it->base_position_3d;
148 | 			if (base_point.z <= 0)
149 | 				continue;
150 | 
151 | 			const cv::Point3f &active_point = track_it->active_position_3d;
152 | 			if (active_point.z <= 0)
153 | 				continue;
154 | 
155 | 			// Add new row to matrices
156 | 			int i = X.rows;
157 | 			X.resize(i + 1);
158 | 			X(i, 0) = base_point.x;
159 | 			X(i, 1) = base_point.y;
160 | 			X(i, 2) = base_point.z;
161 | 
162 | 			Y.resize(i + 1);
163 | 			Y(i, 0) = active_point.x;
164 | 			Y(i, 1) = active_point.y;
165 | 			Y(i, 2) = active_point.z;
166 | 		}
167 | 
168 | 		if (X.rows > 0 && Y.rows)
169 | 			ransac_transformation(X, Y, R, T);
170 | 	}
171 | 
172 | 	void TrackingModule::ransac_transformation(const cv::Mat1f &X, const cv::Mat1f &Y, cv::Matx33f &R, cv::Matx31f &T) {
173 | 		const int max_iterations = 200;
174 | 		const int min_support = 3;
175 | 		const float inlier_error_threshold = 0.2f * 1000;
176 | 
177 | 		const int pcount = X.rows;
178 | 		cv::RNG rng;
179 | 		cv::Mat1f Xk(min_support, 3), Yk(min_support, 3);
180 | 		cv::Matx33f Rk;
181 | 		cv::Matx31f Tk;
182 | 		std::vector<int> best_inliers;
183 | 
184 | 		for (int k = 0; k < max_iterations; k++) {
185 | 
186 | 			// Select random points
187 | 			for (int i = 0; i < min_support; i++) {
188 | 				int idx = rng(pcount);
189 | 				Xk(i, 0) = X(idx, 0);
190 | 				Xk(i, 1) = X(idx, 1);
191 | 				Xk(i, 2) = X(idx, 2);
192 | 				Yk(i, 0) = Y(idx, 0);
193 | 				Yk(i, 1) = Y(idx, 1);
194 | 				Yk(i, 2) = Y(idx, 2);
195 | 			}
196 | 
197 | 			// Get orientation
198 | 			absolute_orientation(Xk, Yk, Rk, Tk);
199 | 
200 | 			// Get error
201 | 			std::vector<int> inliers;
202 | 			for (int i = 0; i < pcount; i++) {
203 | 				float a, b, c, errori;
204 | 				cv::Matx31f py, pyy;
205 | 				py(0) = Y(i, 0);
206 | 				py(1) = Y(i, 1);
207 | 				py(2) = Y(i, 2);
208 | 				pyy = Rk*py + T;
209 | 				a = pyy(0) - X(i, 0);
210 | 				b = pyy(1) - X(i, 1);
211 | 				c = pyy(2) - X(i, 2);
212 | 				errori = sqrt(a*a + b*b + c*c);
213 | 				if (errori < inlier_error_threshold) {
214 | 					inliers.push_back(i);
215 | 				}
216 | 			}
217 | 
218 | 			if (inliers.size() > best_inliers.size()) {
219 | 				best_inliers = inliers;
220 | 			}
221 | 		}
222 | 		std::cout << "Inlier count: " << best_inliers.size() << "/" << pcount << endl;
223 | 
224 | 		// Do final estimation with inliers
225 | 		Xk.resize(best_inliers.size());
226 | 		Yk.resize(best_inliers.size());
227 | 
228 | 		for (unsigned int i = 0; i < best_inliers.size(); i++) {
229 | 			int idx = best_inliers[i];
230 | 			Xk(i, 0) = X(idx, 0);
231 | 			Xk(i, 1) = X(idx, 1);
232 | 			Xk(i, 2) = X(idx, 2);
233 | 			Yk(i, 0) = Y(idx, 0);
234 | 			Yk(i, 1) = Y(idx, 1);
235 | 			Yk(i, 2) = Y(idx, 2);
236 | 		}
237 | 
238 | 		absolute_orientation(Xk, Yk, R, T);
239 | 	}
240 | 
241 | 	void TrackingModule::absolute_orientation(cv::Mat1f &X, cv::Mat1f &Y, cv::Matx33f &R, cv::Matx31f &T) {
242 | 
243 | 		cv::Matx31f meanX(0, 0, 0), meanY(0, 0, 0);
244 | 
245 | 		int point_count = X.rows;
246 | 
247 | 		// Calculate mean
248 | 		for (int i = 0; i < point_count; i++) {
249 | 			meanX(0) += X(i, 0);
250 | 			meanX(1) += X(i, 1);
251 | 			meanX(2) += X(i, 2);
252 | 			meanY(0) += Y(i, 0);
253 | 			meanY(1) += Y(i, 1);
254 | 			meanY(2) += Y(i, 2);
255 | 		}
256 | 		meanX *= 1.0f / point_count;
257 | 		meanY *= 1.0f / point_count;
258 | 
259 | 		// Subtract mean
260 | 		for (int i = 0; i < point_count; i++) {
261 | 			X(i, 0) -= meanX(0);
262 | 			X(i, 1) -= meanX(1);
263 | 			X(i, 2) -= meanX(2);
264 | 			Y(i, 0) -= meanY(0);
265 | 			Y(i, 1) -= meanY(1);
266 | 			Y(i, 2) -= meanY(2);
267 | 		}
268 | 
269 | 		// Rotation
270 | 		cv::Mat1f A;
271 | 		A = Y.t() * X;
272 | 
273 | 		cv::SVD svd(A);
274 | 
275 | 		cv::Mat1f Rmat;
276 | 		Rmat = svd.vt.t() * svd.u.t();
277 | 		Rmat.copyTo(R);
278 | 
279 | 		// Translation
280 | 		T = meanX - R*meanY;
281 | 	}
282 | 
283 | 	void TrackingModule::triangulate_matches(vector<DMatch>& matches, const vector<KeyPoint>&keypoints1, const vector<KeyPoint>& keypoints2,
284 | 		Mat& cam1P, Mat& cam2P, vector<Point3f>& pnts3D)
285 | 	{
286 | 		// Convert keypoints into Point2f
287 | 		vector<cv::Point2f> points1, points2;
288 | 
289 | 		int i = 0;
290 | 		for (auto it = matches.begin(); it != matches.end(); ++it)
291 | 		{
292 | 			// Get the position of left keypoints
293 | 			float xl = keypoints1[it->queryIdx].pt.x;
294 | 			float yl = keypoints1[it->queryIdx].pt.y;
295 | 
296 | 			points1.push_back(Point2f(xl, yl));
297 | 
298 | 			// Get the position of right keypoints
299 | 			float xr = keypoints2[it->trainIdx].pt.x;
300 | 			float yr = keypoints2[it->trainIdx].pt.y;
301 | 
302 | 			points2.push_back(Point2f(xr, yr));
303 | 
304 | 			i++;
305 | 		}
306 | 
307 | 		Mat pnts3DMat;
308 | 		cv::triangulatePoints(cam1P, cam2P, points1, points2, pnts3DMat);
309 | 
310 | 		assert(pnts3DMat.cols == keypoints1.size());
311 | 
312 | 		for (int x = 0; x < pnts3DMat.cols; x++) {
313 | 			float W = pnts3DMat.at<float>(3, x);
314 | 			float Z = pnts3DMat.at<float>(2, x) / W; /// 1000;
315 | 
316 | 			if (fabs(Z - MAX_Z) < FLT_EPSILON || fabs(Z) > MAX_Z || Z < 0) {
317 | 				pnts3D.push_back(Point3f(0, 0, 0)); // Push empty point TODO: replace with lookup table?
318 | 				continue;
319 | 			}
320 | 
321 | 			float X = pnts3DMat.at<float>(0, x) / W; /// 1000;
322 | 			float Y = pnts3DMat.at<float>(1, x) / W; /// 1000;
323 | 
324 | 			pnts3D.push_back(Point3f(X, Y, Z));
325 | 		}
326 | 
327 | 		assert(pnts3D.size() == keypoints1.size());
328 | 	}
329 | 
330 | };


--------------------------------------------------------------------------------
/my_slam_tracking.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <iostream>
  4 | #include <list>
  5 | 
  6 | #include <opencv2/opencv.hpp>
  7 | 
  8 | using namespace cv;
  9 | using namespace std;
 10 | 
 11 | namespace slam {
 12 | 
 13 | 	class FeatureTrack {
 14 | 	public:
 15 | 		cv::Point2f base_position;
 16 | 		cv::Point2f active_position;
 17 | 
 18 | 		cv::Point3f base_position_3d;
 19 | 		cv::Point3f active_position_3d;
 20 | 
 21 | 		cv::Mat descriptor;
 22 | 		int missed_frames;
 23 | 	};
 24 | 
 25 | 	class TrackedView {
 26 | 	public:
 27 | 		cv::Matx33f R;
 28 | 		cv::Matx31f T;
 29 | 	};
 30 | 
 31 | 	class TrackingSharedData {
 32 | 	public:
 33 | 		//Commands
 34 | 		bool is_data_new;   // True if this class has new data that should be rendered
 35 | 		bool is_tracking_enabled; //True if the tracking thread should process images
 36 | 
 37 | 		cv::Matx33f base_R;
 38 | 		cv::Matx31f base_T;
 39 | 		cv::Mat3b *base_rgb;
 40 | 		//cv::Mat3f *base_pointmap;
 41 | 
 42 | 		//Last tracked image
 43 | 		cv::Mat3b *active_rgb;
 44 | 		//cv::Mat1s *active_depth;
 45 | 
 46 | 		//Model
 47 | 		std::list<FeatureTrack> tracks; // Tracked features since last base frame
 48 | 		std::vector<TrackedView> views; // All registered views
 49 | 
 50 | 		TrackingSharedData();
 51 | 		~TrackingSharedData();
 52 | 	};
 53 | 
 54 | 	class TrackingModule
 55 | 	{
 56 | 	public:
 57 | 		//void compute_pointmap(const cv::Mat1s &depth, cv::Mat3f &pointmap);
 58 | 		//void cloud_from_pointmap(const cv::Mat3b &rgb, const cv::Mat3f &pointmap, boost::container::vector<pcl::PointXYZRGB> &cloud);
 59 | 
 60 | 		static void match_features(const std::list<FeatureTrack> &tracks, const cv::Mat &new_descriptors, std::vector<int> &match_idx);
 61 | 
 62 | 		static void update_tracks(std::list<FeatureTrack> &tracks, const std::vector<cv::KeyPoint> &feature_points, const cv::Mat &feature_descriptors, 
 63 | 			std::vector<int> &match_idx, std::vector<cv::Point3f>& pnts3D);
 64 | 
 65 | 		static bool is_track_stale(const FeatureTrack &track);
 66 | 		
 67 | 		static float get_median_feature_movement(const std::list<FeatureTrack> &tracks);
 68 | 
 69 | 		static void get_tracked_keypoints(const std::vector<cv::KeyPoint> &feature_points, std::vector<cv::KeyPoint> &feature_output, const std::vector<int> &match_idx) {
 70 | 			
 71 | 			for (auto it = match_idx.begin(); it != match_idx.end(); ++it) {
 72 | 				int idx = *it;
 73 | 				if(idx > 0)
 74 | 					feature_output.push_back(feature_points.at(idx));
 75 | 			}
 76 | 		}
 77 | 
 78 | 		static void get_triangulated_keypoints(std::list<FeatureTrack> &tracks,
 79 | 			const std::vector<cv::KeyPoint> &feature_points, std::vector<cv::KeyPoint> &feature_output, 
 80 | 			const std::vector<int> &match_idx, std::vector<cv::Point3f>& pnts3D) {
 81 | 
 82 | 			std::list<FeatureTrack>::iterator track_it;
 83 | 			int i;
 84 | 			for (i = 0, track_it = tracks.begin(); track_it != tracks.end(); i++, track_it++) {
 85 | 				int j = match_idx[i];
 86 | 				if (j >= 0) {
 87 | 					Point3d point = pnts3D[j];
 88 | 
 89 | 					if (point.x != 0 && point.y != 0 && point.z != 0) {
 90 | 						feature_output.push_back(feature_points.at(j));
 91 | 						continue;
 92 | 					}
 93 | 				}
 94 | 			}
 95 | 		}
 96 | 
 97 | 		static void triangulate_matches(std::vector<cv::DMatch>& matches, const std::vector<cv::KeyPoint>&keypoints1, const std::vector<cv::KeyPoint>& keypoints2,
 98 | 			cv::Mat& cam1P, cv::Mat& cam2P, std::vector<cv::Point3f>& pnts3D);
 99 | 
100 | 		static void transformation_from_tracks(const std::list<FeatureTrack> &tracks, cv::Matx33f &R, cv::Matx31f &T);
101 | 		
102 | 		static void ransac_transformation(const cv::Mat1f &X, const cv::Mat1f &Y, cv::Matx33f &R, cv::Matx31f &T);
103 | 		static void absolute_orientation(cv::Mat1f &X, cv::Mat1f &Y, cv::Matx33f &R, cv::Matx31f &T);
104 | 	};
105 | 
106 | }


--------------------------------------------------------------------------------
/run.txt:
--------------------------------------------------------------------------------
1 | <left_image> <right_image> [--algorithm=bm|sgbm|hh|sgbm3way] [--blocksize=<block_size>]\n"
2 |            "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>]\n"
3 |            "[--no-display] [-o=<disparity_image>] [-p=<point_cloud_file>]
4 | 		   
5 | 0 0 --algorithm=bm -i=D:\Development\Seminar\stereo-calibration\mystereocalib_unit22.yml
6 | 
7 | 0 0 --algorithm=sgbm -i=D:\Development\Seminar\stereo-calibration\mystereocalib-5x7.yml


--------------------------------------------------------------------------------