Stereo: Fixed min/max depth filtering

corelib/include/rtabmap/core/Features2d.h

@@ -138,6 +138,8 @@ public:
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
 	int getMaxFeatures() const {return maxFeatures_;}
+	float getMinDepth() const {return _minDepth;}
+	float getMaxDepth() const {return _maxDepth;}
 
 public:
 	virtual ~Feature2D();

corelib/include/rtabmap/core/util3d_features.h

@@ -47,23 +47,31 @@ namespace util3d
 std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		const CameraModel & cameraModel);
+		const CameraModel & cameraModel,
+		float minDepth = 0,
+		float maxDepth = 0);
 
 std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		const std::vector<CameraModel> & cameraModels);
+		const std::vector<CameraModel> & cameraModels,
+		float minDepth = 0,
+		float maxDepth = 0);
 
 std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
-		const StereoCameraModel & stereoCameraMode);
+		const StereoCameraModel & stereoCameraModel,
+		float minDepth = 0,
+		float maxDepth = 0);
 
 std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DStereo(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
 		const StereoCameraModel & model,
-		const std::vector<unsigned char> & mask = std::vector<unsigned char>());
+		const std::vector<unsigned char> & mask = std::vector<unsigned char>(),
+		float minDepth = 0,
+		float maxDepth = 0);
 
 std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
 		const std::map<int, cv::KeyPoint> & kpts,

corelib/src/Features2d.cpp

@@ -592,7 +592,7 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 		{
 			imageMono = data.imageRaw();
 		}
-		//generate a disparity map
+
 		std::vector<cv::Point2f> leftCorners;
 		cv::KeyPoint::convert(keypoints, leftCorners);
 		std::vector<unsigned char> status;
@@ -604,60 +604,22 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 				leftCorners,
 				status);
 
-		if(_maxDepth > 0.0f || _minDepth > 0.0f)
-		{
-			UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
-			for(unsigned int i=0; i<status.size(); ++i)
-			{
-				if(status[i] != 0)
-				{
-					float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
-					if((_minDepth > 0.0f && d < _minDepth) ||
-					   (_maxDepth > 0.0f && d > _maxDepth))
-					{
-						status[i] = 0;
-					}
-				}
-			}
-		}
-
 		keypoints3D = util3d::generateKeypoints3DStereo(
 				leftCorners,
 				rightCorners,
 				data.stereoCameraModel(),
-				status);
+				status,
+				_minDepth,
+				_maxDepth);
 	}
 	else if(!data.depthRaw().empty() && data.cameraModels().size())
 	{
 		keypoints3D = util3d::generateKeypoints3DDepth(
 				keypoints,
 				data.depthOrRightRaw(),
-				data.cameraModels());
+				data.cameraModels(),
-
+				_minDepth,
-		if(_maxDepth > 0.0f || _minDepth > 0.0f)
+				_maxDepth);
-		{
-			UASSERT(keypoints3D.size() == keypoints.size());
-			float bad_point = std::numeric_limits<float>::quiet_NaN ();
-			for(unsigned int i=0; i<keypoints.size(); ++i)
-			{
-				float d = util2d::getDepth(
-						data.depthRaw(),
-						keypoints[i].pt.x/float((data.imageRaw().cols/data.depthRaw().cols)),
-						keypoints[i].pt.y/float((data.imageRaw().rows/data.depthRaw().rows)),
-						false);
-				bool reject = true;
-				if(uIsFinite(d) && d>_minDepth && (_maxDepth <= 0.0f || d < _maxDepth))
-				{
-					reject = false;
-				}
-				if(reject)
-				{
-					keypoints3D[i].x = bad_point;
-					keypoints3D[i].y = bad_point;
-					keypoints3D[i].z = bad_point;
-				}
-			}
-		}
 	}
 
 	return keypoints3D;

corelib/src/Memory.cpp

@@ -3209,6 +3209,34 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 					(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
 			{
 				keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+				if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
+				{
+					UDEBUG("");
+					//remove all keypoints/descriptors with no valid 3D points
+					UASSERT((int)keypoints.size() == descriptors.rows &&
+							keypoints3D.size() == keypoints.size());
+					std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
+					std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
+					cv::Mat validDescriptors(descriptors.size(), descriptors.type());
+
+					int oi=0;
+					for(unsigned int i=0; i<keypoints3D.size(); ++i)
+					{
+						if(util3d::isFinite(keypoints3D[i]))
+						{
+							validKeypoints[oi] = keypoints[i];
+							validKeypoints3D[oi] = keypoints3D[i];
+							descriptors.row(i).copyTo(validDescriptors.row(oi));
+							++oi;
+						}
+					}
+					UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
+					validKeypoints.resize(oi);
+					validKeypoints3D.resize(oi);
+					keypoints = validKeypoints;
+					keypoints3D = validKeypoints3D;
+					descriptors = validDescriptors.rowRange(0, oi).clone();
+				}
 				t = timer.ticks();
 				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
@@ -3258,19 +3286,48 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 		}
 
+		if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
+		{
+			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+			if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
+			{
+				UDEBUG("");
+				//remove all keypoints/descriptors with no valid 3D points
+				UASSERT((int)keypoints.size() == descriptors.rows &&
+						keypoints3D.size() == keypoints.size());
+				std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
+				std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
+				cv::Mat validDescriptors(descriptors.size(), descriptors.type());
+
+				int oi=0;
+				for(unsigned int i=0; i<keypoints3D.size(); ++i)
+				{
+					if(util3d::isFinite(keypoints3D[i]))
+					{
+						validKeypoints[oi] = keypoints[i];
+						validKeypoints3D[oi] = keypoints3D[i];
+						descriptors.row(i).copyTo(validDescriptors.row(oi));
+						++oi;
+					}
+				}
+				UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
+				validKeypoints.resize(oi);
+				validKeypoints3D.resize(oi);
+				keypoints = validKeypoints;
+				keypoints3D = validKeypoints3D;
+				descriptors = validDescriptors.rowRange(0, oi).clone();
+			}
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+		}
+
 		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
 		{
 			descriptors = cv::Mat();
 		}
-		else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
-				(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
-		{
-			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
-		}
 	}
 
 	if(_parallelized)

corelib/src/RegistrationVis.cpp

@@ -320,7 +320,6 @@ Transform RegistrationVis::computeTransformationImpl(
 
 			if(!toSignature.sensorData().imageRaw().empty())
 			{
-
 				std::vector<cv::Point2f> cornersFrom;
 				cv::KeyPoint::convert(kptsFrom, cornersFrom);
 				std::vector<cv::Point2f> cornersTo;
@@ -533,6 +532,34 @@ Transform RegistrationVis::computeTransformationImpl(
 						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).");
 				}
 				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
+				if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
+				{
+					UDEBUG("");
+					//remove all keypoints/descriptors with no valid 3D points
+					UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
+							kptsFrom3D.size() == kptsFrom.size());
+					std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
+					std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
+					cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
+
+					int oi=0;
+					for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
+					{
+						if(util3d::isFinite(kptsFrom3D[i]))
+						{
+							validKeypoints[oi] = kptsFrom[i];
+							validKeypoints3D[oi] = kptsFrom3D[i];
+							descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
+							++oi;
+						}
+					}
+					UDEBUG("Removed %d invalid 3D points", (int)kptsFrom3D.size()-oi);
+					validKeypoints.resize(oi);
+					validKeypoints3D.resize(oi);
+					kptsFrom = validKeypoints;
+					kptsFrom3D = validKeypoints3D;
+					descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
+				}
 			}
 			else
 			{
@@ -546,6 +573,34 @@ Transform RegistrationVis::computeTransformationImpl(
 						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
 				}
 				kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
+				if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
+				{
+					UDEBUG("");
+					//remove all keypoints/descriptors with no valid 3D points
+					UASSERT((int)kptsTo.size() == descriptorsTo.rows &&
+							kptsTo3D.size() == kptsTo.size());
+					std::vector<cv::KeyPoint> validKeypoints(kptsTo.size());
+					std::vector<cv::Point3f> validKeypoints3D(kptsTo.size());
+					cv::Mat validDescriptors(descriptorsTo.size(), descriptorsTo.type());
+
+					int oi=0;
+					for(unsigned int i=0; i<kptsTo3D.size(); ++i)
+					{
+						if(util3d::isFinite(kptsTo3D[i]))
+						{
+							validKeypoints[oi] = kptsTo[i];
+							validKeypoints3D[oi] = kptsTo3D[i];
+							descriptorsTo.row(i).copyTo(validDescriptors.row(oi));
+							++oi;
+						}
+					}
+					UDEBUG("Removed %d invalid 3D points", (int)kptsTo3D.size()-oi);
+					validKeypoints.resize(oi);
+					validKeypoints3D.resize(oi);
+					kptsTo = validKeypoints;
+					kptsTo3D = validKeypoints3D;
+					descriptorsTo = validDescriptors.rowRange(0, oi).clone();
+				}
 			}
 			else
 			{

corelib/src/Stereo.cpp

@@ -128,6 +128,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 			cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
 	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
 	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
+	int countFlowRejected = 0;
+	int countDisparityRejected = 0;
 	for(unsigned int i=0; i<status.size(); ++i)
 	{
 		if(status[i]!=0)
@@ -136,9 +138,21 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 			if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()))
 			{
 				status[i] = 0;
+				++countDisparityRejected;
 			}
 		}
+		else
+		{
+			++countFlowRejected;
+		}
 	}
+	UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
+
+	if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
+	{
+		UWARN("A large number (%d/%d) of stereo correspondences are rejected! Optical flow may have failed, images are not calibrated or the background is too far (no disparity between the images).", countFlowRejected+countDisparityRejected, (int)status.size());
+	}
+
 	return rightCorners;
 }
 

corelib/src/util3d_features.cpp

@@ -51,18 +51,22 @@ namespace util3d
 std::vector<cv::Point3f> generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		const CameraModel & cameraModel)
+		const CameraModel & cameraModel,
+		float minDepth,
+		float maxDepth)
 {
 	UASSERT(cameraModel.isValidForProjection());
 	std::vector<CameraModel> models;
 	models.push_back(cameraModel);
-	return generateKeypoints3DDepth(keypoints, depth, models);
+	return generateKeypoints3DDepth(keypoints, depth, models, minDepth, maxDepth);
 }
 
 std::vector<cv::Point3f> generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		const std::vector<CameraModel> & cameraModels)
+		const std::vector<CameraModel> & cameraModels,
+		float minDepth,
+		float maxDepth)
 {
 	UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
 	UASSERT(cameraModels.size());
@@ -74,6 +78,7 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
 		keypoints3d.resize(keypoints.size());
 		float rgbToDepthFactorX = 1.0f/(cameraModels[0].imageWidth()>0?cameraModels[0].imageWidth()/subImageWidth:1);
 		float rgbToDepthFactorY = 1.0f/(cameraModels[0].imageHeight()>0?cameraModels[0].imageHeight()/depth.rows:1);
+		float bad_point = std::numeric_limits<float>::quiet_NaN ();
 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
 			float x = keypoints[i].pt.x*rgbToDepthFactorX;
@@ -93,12 +98,17 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
 					cameraModels.at(cameraIndex).fy()*rgbToDepthFactorY,
 					true);
 
-			cv::Point3f pt(ptXYZ.x, ptXYZ.y, ptXYZ.z);
+			cv::Point3f pt(bad_point, bad_point, bad_point);
-			if(util3d::isFinite(pt) &&
+			if(pcl::isFinite(ptXYZ) &&
-				!cameraModels.at(cameraIndex).localTransform().isNull() &&
+				(minDepth <= 0.0f || ptXYZ.z >= minDepth) &&
-			    !cameraModels.at(cameraIndex).localTransform().isIdentity())
+				(maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
 			{
-				pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
+				pt = cv::Point3f(ptXYZ.x, ptXYZ.y, ptXYZ.z);
+				if(!cameraModels.at(cameraIndex).localTransform().isNull() &&
+				   !cameraModels.at(cameraIndex).localTransform().isIdentity())
+				{
+					pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
+				}
 			}
 			keypoints3d.at(i) = pt;
 		}
@@ -109,24 +119,33 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
 std::vector<cv::Point3f> generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
-		const StereoCameraModel & stereoCameraModel)
+		const StereoCameraModel & stereoCameraModel,
+		float minDepth,
+		float maxDepth)
 {
 	UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
 	UASSERT(stereoCameraModel.isValidForProjection());
 	std::vector<cv::Point3f> keypoints3d;
 	keypoints3d.resize(keypoints.size());
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
 	for(unsigned int i=0; i!=keypoints.size(); ++i)
 	{
-		cv::Point3f pt = util3d::projectDisparityTo3D(
+		cv::Point3f tmpPt = util3d::projectDisparityTo3D(
 				keypoints[i].pt,
 				disparity,
 				stereoCameraModel);
 
-		if(util3d::isFinite(pt) &&
+		cv::Point3f pt(bad_point, bad_point, bad_point);
-			!stereoCameraModel.left().localTransform().isNull() &&
+		if(util3d::isFinite(tmpPt) &&
-			!stereoCameraModel.left().localTransform().isIdentity())
+			(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
+			(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
 		{
-			pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
+			pt = tmpPt;
+			if(!stereoCameraModel.left().localTransform().isNull() &&
+				!stereoCameraModel.left().localTransform().isIdentity())
+			{
+				pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
+			}
 		}
 		keypoints3d.at(i) = pt;
 	}
@@ -137,7 +156,9 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
 		const StereoCameraModel & model,
-		const std::vector<unsigned char> & mask)
+		const std::vector<unsigned char> & mask,
+		float minDepth,
+		float maxDepth)
 {
 	UASSERT(leftCorners.size() == rightCorners.size());
 	UASSERT(mask.size() == 0 || leftCorners.size() == mask.size());
@@ -159,7 +180,9 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
 						disparity,
 						model);
 
-				if(util3d::isFinite(tmpPt))
+				if(util3d::isFinite(tmpPt) &&
+				   (minDepth <= 0.0f || tmpPt.z >= minDepth) &&
+				   (maxDepth <= 0.0f || tmpPt.z <= maxDepth))
 				{
 					pt = tmpPt;
 					if(!model.localTransform().isNull() &&

guilib/src/DatabaseViewer.cpp

@@ -2437,7 +2437,13 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		// generate kpts
 		std::vector<cv::KeyPoint> kpts;
 		uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
-		uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), std::string(opticalFlow?"0.03 0.03 0.04 0.04":"0 0 0 0")));
+		uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
+		uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
+		uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
+		uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
+		uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
+		uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
+		uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
 		Feature2D * kptDetector = Feature2D::create(parameters);
 		kpts = kptDetector->generateKeypoints(leftMono);
 		delete kptDetector;