Odometry and stereo: added parameter OdomStereo/MaxSlope to filter bad stereo pair matches from cvOpticalFlow

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1967 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-02 01:20:25 +08:00 · 2014-11-04 18:58:40 +00:00
parent 54084fc5e1
commit cbb0185adf
12 changed files with 948 additions and 533 deletions
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -258,10 +258,11 @@ private:
 	int _stereoFlowIterations;
 	double _stereoFlowEpsilon;
 	int _stereoFlowMaxLevel;
+	float _stereoMaxSlope;

-	int _stereoSubPixWinSize;
-	int _stereoSubPixIterations;
-	double _stereoSubPixEps;
+	int _subPixWinSize;
+	int _subPixIterations;
+	double _subPixEps;
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -143,6 +143,7 @@ private:
 	int flowIterations_;
 	double flowEps_;
 	int flowMaxLevel_;
+	float stereoMaxSlope_;

 	int subPixWinSize_;
 	int subPixIterations_;
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -175,6 +175,10 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM_STR(Kp, DictionaryPath,    "", 				"Path of the pre-computed dictionary");
 	RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,	"When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");

+	RTABMAP_PARAM(Kp, SubPixWinSize,         int, 3,        "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Kp, SubPixIterations,      int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(Kp, SubPixEps,             double, 0.02,  "See cv::cornerSubPix().");
+
 	//Database
 	RTABMAP_PARAM(DbSqlite3, InMemory, 	   bool, false, 		"Using database in the memory instead of a file on the hard disk.");
 	RTABMAP_PARAM(DbSqlite3, CacheSize,    unsigned int, 10000, "Sqlite cache size (default is 2000).");
@@ -276,14 +280,17 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");

-	// Odometry Optical Flow
-	RTABMAP_PARAM(OdomFlow, WinSize,               int, 9,       "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, Iterations,            int, 20,       "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, Eps,                   double, 0.02,  "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, MaxLevel,              int, 4,        "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(OdomFlow, SubPixWinSize,         int, 3,        "See cv::cornerSubPix().");
-	RTABMAP_PARAM(OdomFlow, SubPixIterations,      int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
-	RTABMAP_PARAM(OdomFlow, SubPixEps,             double, 0.02,  "See cv::cornerSubPix().");
+	// Odometry common stuff between BOW and Optical Flow approaches
+	RTABMAP_PARAM(OdomFlow, WinSize,               int, 9,       "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, Iterations,            int, 20,       "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, Eps,                   double, 0.02,  "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, MaxLevel,              int, 4,        "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
+
+	RTABMAP_PARAM(OdomSubPix, WinSize,         int, 3,        "Can be used with BOW and optical flow approaches. See cv::cornerSubPix().");
+	RTABMAP_PARAM(OdomSubPix, Iterations,      int, 0,       "Can be used with BOW and optical flow approaches. See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(OdomSubPix, Eps,             double, 0.02,  "Can be used with BOW and optical flow approaches. See cv::cornerSubPix().");
+
+	RTABMAP_PARAM(OdomStereo, MaxSlope,              float, 0.1,    "The maximum slope for each stereo pairs.");

 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
@@ -321,9 +328,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, Iterations,            int, 20,       "See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(Stereo, Eps,                   double, 0.02,  "See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(Stereo, MaxLevel,              int, 4,        "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(Stereo, SubPixWinSize,         int, 3,        "See cv::cornerSubPix().");
-	RTABMAP_PARAM(Stereo, SubPixIterations,      int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
-	RTABMAP_PARAM(Stereo, SubPixEps,             double, 0.02,  "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Stereo, MaxSlope,              float, 0.1,    "The maximum slope for each stereo pairs.");

 public:
 	virtual ~Parameters();
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -190,7 +190,8 @@ cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		int flowWinSize = 9,
 		int flowMaxLevel = 4,
 		int flowIterations = 20,
-		double flowEps = 0.02);
+		double flowEps = 0.02,
+		float maxCorrespondencesSlope = 0.1f);

 cv::Mat RTABMAP_EXP depthFromStereoImages(
 		const cv::Mat & leftImage,
@@ -207,7 +208,8 @@ cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
 		const cv::Mat & leftImage,
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		const std::vector<unsigned char> & mask);
+		const std::vector<unsigned char> & mask,
+		float maxSlope = 0.1f);

 cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
 		const cv::Mat & leftImage,
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -108,10 +108,11 @@ Memory::Memory(const ParametersMap & parameters) :
 	_stereoFlowIterations(Parameters::defaultStereoIterations()),
 	_stereoFlowEpsilon(Parameters::defaultStereoEps()),
 	_stereoFlowMaxLevel(Parameters::defaultStereoMaxLevel()),
+	_stereoMaxSlope(Parameters::defaultStereoMaxSlope()),

-	_stereoSubPixWinSize(Parameters::defaultStereoSubPixWinSize()),
-	_stereoSubPixIterations(Parameters::defaultStereoSubPixIterations()),
-	_stereoSubPixEps(Parameters::defaultStereoSubPixEps())
+	_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
+	_subPixIterations(Parameters::defaultKpSubPixIterations()),
+	_subPixEps(Parameters::defaultKpSubPixEps())
 {
 	_vwd = new VWDictionary(parameters);
 	this->parseParameters(parameters);
@@ -418,9 +419,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kStereoIterations(), _stereoFlowIterations);
 	Parameters::parse(parameters, Parameters::kStereoEps(), _stereoFlowEpsilon);
 	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), _stereoFlowMaxLevel);
-	Parameters::parse(parameters, Parameters::kStereoSubPixWinSize(), _stereoSubPixWinSize);
-	Parameters::parse(parameters, Parameters::kStereoSubPixIterations(), _stereoSubPixIterations);
-	Parameters::parse(parameters, Parameters::kStereoSubPixEps(), _stereoSubPixEps);
+	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), _stereoMaxSlope);

 	UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
 	UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
@@ -452,6 +451,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
 	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _wordsPerImageTarget);

+	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
+	Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
+	Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
+
 	if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
 	{
 		this->setRoi((*iter).second);
@@ -3098,101 +3101,166 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 			{
 				//stereo
 				cv::Mat disparity;
-				keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
+				bool subPixelOn = false;
+				if(_subPixWinSize > 0 && _subPixIterations > 0)
+				{
+					subPixelOn = true;
+				}
+				keypoints = _feature2D->generateKeypoints(imageMono, subPixelOn?_wordsPerImageTarget:0, roi);
 				t = timer.ticks();
 				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
 				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);

-				std::vector<cv::Point2f> leftCorners;
-				cv::KeyPoint::convert(keypoints, leftCorners);
-				if(_stereoSubPixWinSize > 0 && _stereoSubPixIterations > 0)
-				{
-					cv::cornerSubPix( imageMono, leftCorners,
-							cv::Size( _stereoSubPixWinSize, _stereoSubPixWinSize ),
-							cv::Size( -1, -1 ),
-							cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _stereoSubPixIterations, _stereoSubPixEps ) );
-					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemStereo_subpixel(), t*1000.0f);
-					UDEBUG("time subpix left kpts=%fs", t);
-				}
-
-				//generate a disparity map
-				disparity = util3d::disparityFromStereoImages(
-						imageMono,
-						data.rightImage(),
-						leftCorners,
-						_stereoFlowWinSize,
-						_stereoFlowMaxLevel,
-						_stereoFlowIterations,
-						_stereoFlowEpsilon);
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
-				UDEBUG("generate disparity = %fs", t);
-
-				if(_wordsMaxDepth > 0.0f)
-				{
-					// disparity = baseline * fx / depth;
-					float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
-					Feature2D::filterKeypointsByDisparity(keypoints, disparity, minDisparity);
-					UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size());
-				}
-
-				if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
-				{
-					Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-					UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
-				}
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
-				UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
-
 				if(keypoints.size())
 				{
-					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
+					std::vector<cv::Point2f> leftCorners;
+					if(subPixelOn)
+					{
+						// descriptors should be extracted before subpixel
+						descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+						t = timer.ticks();
+						if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+						UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);

-					keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
+						cv::KeyPoint::convert(keypoints, leftCorners);
+						cv::cornerSubPix( imageMono, leftCorners,
+								cv::Size( _subPixWinSize, _subPixWinSize ),
+								cv::Size( -1, -1 ),
+								cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
+
+						for(unsigned int i=0;i<leftCorners.size(); ++i)
+						{
+							keypoints[i].pt = leftCorners[i];
+						}
+
+						t = timer.ticks();
+						if(stats) stats->addStatistic(Statistics::kTimingMemStereo_subpixel(), t*1000.0f);
+						UDEBUG("time subpix left kpts=%fs", t);
+					}
+					else
+					{
+						cv::KeyPoint::convert(keypoints, leftCorners);
+					}
+
+					//generate a disparity map
+					disparity = util3d::disparityFromStereoImages(
+							imageMono,
+							data.rightImage(),
+							leftCorners,
+							_stereoFlowWinSize,
+							_stereoFlowMaxLevel,
+							_stereoFlowIterations,
+							_stereoFlowEpsilon,
+							_stereoMaxSlope);
 					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
+					if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
+					UDEBUG("generate disparity = %fs", t);
+
+					if(_wordsMaxDepth > 0.0f)
+					{
+						// disparity = baseline * fx / depth;
+						float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
+						Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
+						UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size());
+					}
+
+					if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
+					{
+						Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+						UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
+					}
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+					UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
+
+
+					if(keypoints.size())
+					{
+						if(!subPixelOn)
+						{
+							descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+							t = timer.ticks();
+							if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+							UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
+						}
+
+						keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
+						t = timer.ticks();
+						if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+						UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
+					}
 				}
 			}
 			else if(!data.depth().empty())
 			{
 				//depth
-				keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
+				bool subPixelOn = false;
+				if(_subPixWinSize > 0 && _subPixIterations > 0)
+				{
+					subPixelOn = true;
+				}
+				keypoints = _feature2D->generateKeypoints(imageMono, subPixelOn?_wordsPerImageTarget:0, roi);
 				t = timer.ticks();
 				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
 				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);

-				if(_wordsMaxDepth > 0.0f)
-				{
-					Feature2D::filterKeypointsByDepth(keypoints, data.depth(), _wordsMaxDepth);
-					UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
-				}
-
-				if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
-				{
-					Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-					UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
-				}
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
-				UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
-
 				if(keypoints.size())
 				{
-					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
+					if(subPixelOn)
+					{
+						// descriptors should be extracted before subpixel
+						descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+						t = timer.ticks();
+						if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+						UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);

-					keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
+						std::vector<cv::Point2f> leftCorners;
+						cv::KeyPoint::convert(keypoints, leftCorners);
+						cv::cornerSubPix( imageMono, leftCorners,
+								cv::Size( _subPixWinSize, _subPixWinSize ),
+								cv::Size( -1, -1 ),
+								cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
+
+						for(unsigned int i=0;i<leftCorners.size(); ++i)
+						{
+							keypoints[i].pt = leftCorners[i];
+						}
+
+						t = timer.ticks();
+						if(stats) stats->addStatistic(Statistics::kTimingMemStereo_subpixel(), t*1000.0f);
+						UDEBUG("time subpix left kpts=%fs", t);
+					}
+
+					if(_wordsMaxDepth > 0.0f)
+					{
+						Feature2D::filterKeypointsByDepth(keypoints, descriptors, data.depth(), _wordsMaxDepth);
+						UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
+					}
+
+					if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
+					{
+						Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+						UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
+					}
 					t = timer.ticks();
-					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+					UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
+
+					if(keypoints.size())
+					{
+						if(!subPixelOn)
+						{
+							descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+							t = timer.ticks();
+							if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+							UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
+						}
+
+						keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
+						t = timer.ticks();
+						if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+						UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
+					}
 				}
 			}
 			else
@@ -3252,7 +3320,8 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 					_stereoFlowWinSize,
 					_stereoFlowMaxLevel,
 					_stereoFlowIterations,
-					_stereoFlowEpsilon);
+					_stereoFlowEpsilon,
+					_stereoMaxSlope);
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
 			UDEBUG("generate disparity = %fs", t);
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -157,23 +157,25 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	int flowIterations_ = Parameters::defaultOdomFlowIterations();
 	double flowEps_ = Parameters::defaultOdomFlowEps();
 	int flowMaxLevel_ = Parameters::defaultOdomFlowMaxLevel();
-	int subPixWinSize_ = Parameters::defaultOdomFlowSubPixWinSize();
-	int subPixIterations_ = Parameters::defaultOdomFlowSubPixIterations();
-	double subPixEps_ = Parameters::defaultOdomFlowSubPixEps();
+	float stereoMaxSlope_ = Parameters::defaultOdomStereoMaxSlope();
+	int subPixWinSize_ = Parameters::defaultOdomSubPixWinSize();
+	int subPixIterations_ = Parameters::defaultOdomSubPixIterations();
+	double subPixEps_ = Parameters::defaultOdomSubPixEps();
 	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize);
 	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
 	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
 	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
-	Parameters::parse(parameters, Parameters::kOdomFlowSubPixWinSize(), subPixWinSize_);
-	Parameters::parse(parameters, Parameters::kOdomFlowSubPixIterations(), subPixIterations_);
-	Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
 	customParameters.insert(ParametersPair(Parameters::kStereoWinSize(), uNumber2Str(flowWinSize)));
 	customParameters.insert(ParametersPair(Parameters::kStereoIterations(), uNumber2Str(flowIterations_)));
 	customParameters.insert(ParametersPair(Parameters::kStereoEps(), uNumber2Str(flowEps_)));
 	customParameters.insert(ParametersPair(Parameters::kStereoMaxLevel(), uNumber2Str(flowMaxLevel_)));
-	customParameters.insert(ParametersPair(Parameters::kStereoSubPixWinSize(), uNumber2Str(subPixWinSize_)));
-	customParameters.insert(ParametersPair(Parameters::kStereoSubPixIterations(), uNumber2Str(subPixIterations_)));
-	customParameters.insert(ParametersPair(Parameters::kStereoSubPixEps(), uNumber2Str(subPixEps_)));
+	customParameters.insert(ParametersPair(Parameters::kStereoMaxSlope(), uNumber2Str(stereoMaxSlope_)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize_)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations_)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps_)));

 	// add only feature stuff
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
@@ -445,18 +447,20 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 	flowIterations_(Parameters::defaultOdomFlowIterations()),
 	flowEps_(Parameters::defaultOdomFlowEps()),
 	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
-	subPixWinSize_(Parameters::defaultOdomFlowSubPixWinSize()),
-	subPixIterations_(Parameters::defaultOdomFlowSubPixIterations()),
-	subPixEps_(Parameters::defaultOdomFlowSubPixEps()),
+	stereoMaxSlope_(Parameters::defaultOdomStereoMaxSlope()),
+	subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
+	subPixIterations_(Parameters::defaultOdomSubPixIterations()),
+	subPixEps_(Parameters::defaultOdomSubPixEps()),
 	lastCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
 {
 	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
 	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
 	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
 	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
-	Parameters::parse(parameters, Parameters::kOdomFlowSubPixWinSize(), subPixWinSize_);
-	Parameters::parse(parameters, Parameters::kOdomFlowSubPixIterations(), subPixIterations_);
-	Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);
+	Parameters::parse(parameters, Parameters::kOdomStereoMaxSlope(), stereoMaxSlope_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);

 	ParametersMap::const_iterator iter;
 	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
@@ -624,7 +628,10 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 				{
 					float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
 					float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
-					if(lastDisparity > 0.0f && newDisparity > 0.0f)
+					float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
+					float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
+					if(lastDisparity > 0.0f && newDisparity > 0.0f &&
+						lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_)
 					{
 						pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
 								lastCornersKept[i],
@@ -697,14 +704,14 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
 				}

-				/*if(correspondencesLast->size() >= 6)
-				{
-					UWARN("saved pcd");
-					pcl::io::savePCDFile("last.pcd", *correspondencesLast);
-					pcl::io::savePCDFile("new.pcd", *correspondencesNew);
-					correspondencesNew = util3d::transformPointCloud(correspondencesNew, output);
-					pcl::io::savePCDFile("new2.pcd", *correspondencesNew);
-				}*/
+				//if(correspondencesLast->size() >= 6)
+				//{
+				//	UWARN("saved pcd");
+				//	pcl::io::savePCDFile("last.pcd", *correspondencesLast);
+				//	pcl::io::savePCDFile("new.pcd", *correspondencesNew);
+					//correspondencesNew = util3d::transformPointCloud(correspondencesNew, output);
+					//pcl::io::savePCDFile("new2.pcd", *correspondencesNew);
+				//}
 			}
 			else
 			{
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -893,7 +893,8 @@ cv::Mat disparityFromStereoImages(
 		int flowWinSize,
 		int flowMaxLevel,
 		int flowIterations,
-		double flowEps)
+		double flowEps,
+		float maxCorrespondencesSlope)
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty() &&
 			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
@@ -917,7 +918,7 @@ cv::Mat disparityFromStereoImages(
 			cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
 	UDEBUG("cv::calcOpticalFlowPyrLK() end");

-	return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status);
+	return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
 }

 cv::Mat depthFromStereoImages(
@@ -961,7 +962,8 @@ cv::Mat disparityFromStereoCorrespondences(
 		const cv::Mat & leftImage,
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		const std::vector<unsigned char> & mask)
+		const std::vector<unsigned char> & mask,
+		float maxSlope)
 {
 	UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
 	UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
@@ -971,7 +973,8 @@ cv::Mat disparityFromStereoCorrespondences(
 		if(mask.size() == 0 || mask[i])
 		{
 			float d = leftCorners[i].x - rightCorners[i].x;
-			if(d > 0.0f)
+			float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
+			if(d > 0.0f && slope < maxSlope)
 			{
 				disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
 			}