modified how correspondences ratio is computed (icp 2D and 3D), also fixed build with OpenCV3+Cuda

corelib/include/rtabmap/core/Memory.h

@@ -272,7 +272,7 @@ private:
 	int _bowRefineIterations;
 	bool _bowForce2D;
 	float _bowEpipolarGeometryVar;
-	bool _bowEstimationType;
+	int _bowEstimationType;
 	double _bowPnPReprojError;
 	int _bowPnPFlags;
 	float _icpMaxTranslation;

corelib/include/rtabmap/core/util3d_registration.h

@@ -56,32 +56,42 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		std::vector<int> * inliers = 0,
 		double * variance = 0);
 
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);
+
 Transform RTABMAP_EXP icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool * hasConverged = 0,
+		bool & hasConverged,
-		double * variance = 0,
+		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
-		int * correspondences = 0);
 
 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool * hasConverged = 0,
+		bool & hasConverged,
-		double * variance = 0,
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
-		int * correspondences = 0);
 
 Transform RTABMAP_EXP icp2D(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool * hasConverged = 0,
+		bool & hasConverged,
-		double * variance = 0,
+		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
-		int * correspondences = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		const cv::Mat & depth,

corelib/src/CameraRGB.cpp

@@ -115,7 +115,7 @@ unsigned int CameraImages::imagesCount() const
 {
 	if(_dir)
 	{
-		return _dir->getFileNames().size();
+		return (unsigned int)_dir->getFileNames().size();
 	}
 	return 0;
 }

corelib/src/Memory.cpp

@@ -2348,28 +2348,44 @@ Transform Memory::computeIcpTransform(
 
 					if(newCloud->size() && oldCloud->size())
 					{
-						icpT = util3d::icpPointToPlane(newCloud,
+						pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
+						icpT = util3d::icpPointToPlane(
+								newCloud,
 								oldCloud,
 							   _icpMaxCorrespondenceDistance,
 							   _icpMaxIterations,
-							   &hasConverged,
+							   hasConverged,
-							   &variance,
+							   *newCloudRegistered);
-							   &correspondences);
+
+						util3d::computeVarianceAndCorrespondences(
+								newCloudRegistered,
+								oldCloud,
+								_icpMaxCorrespondenceDistance,
+								variance,
+								correspondences);
 					}
 				}
 				else
 				{
-					icpT = util3d::icp(newCloudXYZ,
+					pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+					icpT = util3d::icp(
+							newCloudXYZ,
 							oldCloudXYZ,
 							_icpMaxCorrespondenceDistance,
 							_icpMaxIterations,
-							&hasConverged,
+							hasConverged,
-							&variance,
+							*newCloudRegistered);
-							&correspondences);
+
+					util3d::computeVarianceAndCorrespondences(
+							newCloudRegistered,
+							oldCloudXYZ,
+							_icpMaxCorrespondenceDistance,
+							variance,
+							correspondences);
 				}
 
 				// verify if there are enough correspondences
-				correspondencesRatio = float(correspondences)/float(newS.sensorData().depthOrRightRaw().total());
+				correspondencesRatio = float(correspondences)/float(newCloudXYZ->size()>oldCloudXYZ->size()?newCloudXYZ->size():oldCloudXYZ->size());
 
 				UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
 						hasConverged?"true":"false",
@@ -2456,10 +2472,12 @@ Transform Memory::computeIcpTransform(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newS.sensorData().laserScanRaw(), guess);
 
 			//voxelize
+			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudVoxelized = oldCloud;
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudVoxelized = newCloud;
 			if(_icp2VoxelSize > _laserScanVoxelSize)
 			{
-				oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize);
+				oldCloudVoxelized = util3d::voxelize(oldCloud, _icp2VoxelSize);
-				newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
+				newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize);
 			}
 
 			if(newCloud->size() && oldCloud->size())
@@ -2469,33 +2487,14 @@ Transform Memory::computeIcpTransform(
 				float correspondencesRatio = -1.0f;
 				int correspondences = 0;
 				double variance = 1;
-				icpT = util3d::icp2D(newCloud,
+				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-						oldCloud,
+				icpT = util3d::icp2D(
+						newCloudVoxelized,
+						oldCloudVoxelized,
 					   _icp2MaxCorrespondenceDistance,
 					   _icp2MaxIterations,
-					   &hasConverged,
+					   hasConverged,
-					   &variance,
+					   *newCloudRegistered);
-					   &correspondences);
-
-				// verify if there are enough correspondences
-
-				if(newS.sensorData().laserScanMaxPts())
-				{
-					correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts());
-				}
-				else
-				{
-					UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
-							newS.id());
-				}
-
-				UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
-						newS.id(), oldS.id(),
-						hasConverged?"true":"false",
-						variance,
-						correspondences,
-						(int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()),
-						correspondencesRatio*100.0f);
 
 				//pcl::io::savePCDFile("oldCloud.pcd", *oldCloud);
 				//pcl::io::savePCDFile("newCloud.pcd", *newCloud);
@@ -2507,22 +2506,8 @@ Transform Memory::computeIcpTransform(
 				//	UWARN("saved newCloudFinal.pcd");
 				//}
 
-				if(varianceOut)
-				{
-					*varianceOut = variance;
-				}
-				if(correspondencesOut)
-				{
-					*correspondencesOut = correspondences;
-				}
-				if(correspondencesRatioOut)
-				{
-					*correspondencesRatioOut = correspondencesRatio;
-				}
-
 				if(!icpT.isNull() &&
-					hasConverged &&
+					hasConverged)
-					correspondencesRatio >= _icp2CorrespondenceRatio)
 				{
 					float ix,iy,iz, iroll,ipitch,iyaw;
 					icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
@@ -2530,8 +2515,8 @@ Transform Memory::computeIcpTransform(
 						(fabs(ix) > _icpMaxTranslation ||
 						 fabs(iy) > _icpMaxTranslation ||
 						 fabs(iz) > _icpMaxTranslation))
-					    ||
+						||
-					    (_icpMaxRotation>0.0f &&
+						(_icpMaxRotation>0.0f &&
 						 (fabs(iroll) > _icpMaxRotation ||
 						  fabs(ipitch) > _icpMaxRotation ||
 						  fabs(iyaw) > _icpMaxRotation)))
@@ -2541,14 +2526,72 @@ Transform Memory::computeIcpTransform(
 					}
 					else
 					{
-						transform = icpT * guess;
+						if(_icp2VoxelSize <= _laserScanVoxelSize)
-						transform = transform.inverse();
+						{
+							newCloud = util3d::transformPointCloud(newCloud, icpT);
+						}
+						else
+						{
+							newCloud = newCloudRegistered;
+						}
+
+						pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+						util3d::computeVarianceAndCorrespondences(
+								newCloud,
+								oldCloud,
+								_icpMaxCorrespondenceDistance,
+								variance,
+								correspondences);
+
+						// verify if there are enough correspondences
+						if(newS.sensorData().laserScanMaxPts())
+						{
+							correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts());
+						}
+						else
+						{
+							UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
+									newS.id());
+						}
+
+						UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
+								newS.id(), oldS.id(),
+								hasConverged?"true":"false",
+								variance,
+								correspondences,
+								(int)(newS.sensorData().laserScanMaxPts()),
+								correspondencesRatio*100.0f);
+
+						if(varianceOut)
+						{
+							*varianceOut = variance;
+						}
+						if(correspondencesOut)
+						{
+							*correspondencesOut = correspondences;
+						}
+						if(correspondencesRatioOut)
+						{
+							*correspondencesRatioOut = correspondencesRatio;
+						}
+
+						if(correspondencesRatio < _icp2CorrespondenceRatio)
+						{
+							msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
+									correspondences, correspondencesRatio, _icp2CorrespondenceRatio);
+							UINFO(msg.c_str());
+						}
+						else
+						{
+							transform = icpT * guess;
+							transform = transform.inverse();
+						}
 					}
 				}
 				else
 				{
-					msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)",
+					msg = uFormat("Cannot compute transform (converged=%s var=%f)",
-							hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio);
+							hasConverged?"true":"false", variance);
 					UINFO(msg.c_str());
 				}
 			}
@@ -2638,49 +2681,28 @@ Transform Memory::computeScanMatchingTransform(
 	newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId));
 
 	//voxelize
+	pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudVoxelized = newCloud;
 	if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize)
 	{
-		newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
+		newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize);
 	}
 
 	Transform transform;
-	if(assembledOldClouds->size() && newCloud->size())
+	if(assembledOldClouds->size() && newCloudVoxelized->size())
 	{
 		int correspondences = 0;
 		bool hasConverged = false;
-		Transform icpT = util3d::icp2D(newCloud,
+		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+		Transform icpT = util3d::icp2D(
+				newCloudVoxelized,
 				assembledOldClouds,
 			   _icp2MaxCorrespondenceDistance,
 			   _icp2MaxIterations,
-			   &hasConverged,
+			   hasConverged,
-			   variance,
+			   *newCloudRegistered);
-			   &correspondences);
 
 		UDEBUG("icpT=%s", icpT.prettyPrint().c_str());
 
-		// verify if there enough correspondences
-		float correspondencesRatio = 0.0f;
-		if(newS->sensorData().laserScanMaxPts())
-		{
-			correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts());
-		}
-		else
-		{
-			UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
-					newS->id());
-		}
-
-		UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
-				variance?*variance:-1,
-				correspondences,
-				(int)newCloud->size(),
-				correspondencesRatio*100.0f);
-
-		if(inliers)
-		{
-			*inliers = correspondences;
-		}
-
 		//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
 		//pcl::io::savePCDFile("new.pcd", *newCloud, true);
 		//UWARN("local scan matching old.pcd, new.pcd saved!");
@@ -2691,19 +2713,71 @@ Transform Memory::computeScanMatchingTransform(
 		//	UWARN("local scan matching newFinal.pcd saved!");
 		//}
 
-		if(!icpT.isNull() && hasConverged &&
+		if(!icpT.isNull() && hasConverged)
-		   correspondencesRatio >= _icp2CorrespondenceRatio)
 		{
-			transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
+			if(_icp2VoxelSize <= _laserScanVoxelSize)
+			{
+				newCloud = util3d::transformPointCloud(newCloud, icpT);
+			}
+			else
+			{
+				newCloud = newCloudRegistered;
+			}
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+			double v = 1;
+			util3d::computeVarianceAndCorrespondences(
+					newCloud,
+					assembledOldClouds,
+					_icpMaxCorrespondenceDistance,
+					v,
+					correspondences);
+			if(variance)
+			{
+				*variance = v;
+			}
+
+			// verify if there enough correspondences
+			float correspondencesRatio = 0.0f;
+			if(newS->sensorData().laserScanMaxPts())
+			{
+				correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts());
+			}
+			else
+			{
+				UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
+						newS->id());
+			}
+
+			UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
+					variance?*variance:-1,
+					correspondences,
+					(int)newCloud->size(),
+					correspondencesRatio*100.0f);
+
+			if(inliers)
+			{
+				*inliers = correspondences;
+			}
+
+			if(correspondencesRatio >= _icp2CorrespondenceRatio)
+			{
+				transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
+			}
+			else
+			{
+				msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)",
+					variance?*variance:-1,
+					correspondences,
+					(int)newCloud->size(),
+					correspondencesRatio);
+				UINFO(msg.c_str());
+			}
 		}
 		else
 		{
-			msg = uFormat("Constraints failed... hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
+			msg = uFormat("Constraints failed... hasConverged=%s",
-				hasConverged?"true":"false",
+				hasConverged?"true":"false");
-				variance?*variance:-1,
-				correspondences,
-				(int)newCloud->size(),
-				correspondencesRatio);
 			UINFO(msg.c_str());
 		}
 	}

corelib/src/OdometryICP.cpp

@@ -112,14 +112,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 
 			if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
 			{
-				int correspondences = 0;
+				pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
-				Transform transform = util3d::icpPointToPlane(newCloud,
+				Transform transform = util3d::icpPointToPlane(
+						newCloud,
 						_previousCloudNormal,
 						_maxCorrespondenceDistance,
 						_maxIterations,
-						&hasConverged,
+						hasConverged,
-						&variance,
+						*newCloudRegistered);
-						&correspondences);
+
+				int correspondences = 0;
+				util3d::computeVarianceAndCorrespondences(
+						newCloudRegistered,
+						_previousCloudNormal,
+						_maxCorrespondenceDistance,
+						variance,
+						correspondences);
 
 				// verify if there are enough correspondences
 				float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
@@ -147,14 +155,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 			//point to point
 			if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
 			{
-				int correspondences = 0;
+				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
-				Transform transform = util3d::icp(newCloudXYZ,
+				Transform transform = util3d::icp(
+						newCloudXYZ,
 						_previousCloud,
 						_maxCorrespondenceDistance,
 						_maxIterations,
-						&hasConverged,
+						hasConverged,
-						&variance,
+						*newCloudRegistered);
-						&correspondences);
+
+				int correspondences = 0;
+				util3d::computeVarianceAndCorrespondences(
+						newCloudRegistered,
+						_previousCloud,
+						_maxCorrespondenceDistance,
+						variance,
+						correspondences);
 
 				// verify if there are enough correspondences
 				float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());

corelib/src/Rtabmap.cpp

@@ -1553,7 +1553,7 @@ bool Rtabmap::process(
 				iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
 				++iter)
 			{
-				std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false);
+				std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false);
 				for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
 					jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
 					++jter)
@@ -2311,7 +2311,7 @@ bool Rtabmap::process(
 		statistics_.setConstraints(constraints);
 		statistics_.setSignatures(signatures);
 		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
-		localGraphSize = poses.size();
+		localGraphSize = (int)poses.size();
 	}
 
 	//Start trashing
@@ -3053,7 +3053,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
 		{
 			// set goal to latest signature
 			std::string goalStr = uFormat("GOAL:%d", targetNode);
-			setUserData(0, cv::Mat(1, goalStr.size()+1, CV_8SC1, (void *)goalStr.c_str()).clone());
+			setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
 		}
 		updateGoalIndex();
 	}

corelib/src/VWDictionary.cpp

@@ -506,15 +506,16 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 #ifdef HAVE_OPENCV_CUDAFEATURES2D
 			cv::cuda::GpuMat newDescriptorsGpu(descriptors);
 			cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
+			cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
 			if(type==CV_8U)
 			{
-				cv::cuda::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
+				gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
-				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+				gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 			}
 			else
 			{
-				cv::cuda::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
+				gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
-				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+				gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 			}
 #endif
 #endif
@@ -742,12 +743,12 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 				if(type==CV_8U)
 				{
 					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
-					gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+					gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 				}
 				else
 				{
 					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
-					gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+					gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 				}
 #endif
 #endif

corelib/src/util3d_registration.cpp

@@ -222,14 +222,79 @@ Transform transformFromXYZCorrespondences(
 	return Transform();
 }
 
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	variance = 1;
+	correspondencesOut = 0;
+	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
+	est->setInputTarget(cloudA);
+	est->setInputSource(cloudB);
+	pcl::Correspondences correspondences;
+	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+
+	if(correspondences.size()>=3)
+	{
+		std::vector<double> distances(correspondences.size());
+		for(unsigned int i=0; i<correspondences.size(); ++i)
+		{
+			distances[i] = correspondences[i].distance;
+		}
+
+		//variance
+		std::sort(distances.begin (), distances.end ());
+		double median_error_sqr = distances[distances.size () >> 1];
+		variance = (2.1981 * median_error_sqr);
+	}
+
+	correspondencesOut = (int)correspondences.size();
+}
+
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	variance = 1;
+	correspondencesOut = 0;
+	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
+	est->setInputTarget(cloudA);
+	est->setInputSource(cloudB);
+	pcl::Correspondences correspondences;
+	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+
+	if(correspondences.size()>=3)
+	{
+		std::vector<double> distances(correspondences.size());
+		for(unsigned int i=0; i<correspondences.size(); ++i)
+		{
+			distances[i] = correspondences[i].distance;
+		}
+
+		//variance
+		std::sort(distances.begin (), distances.end ());
+		double median_error_sqr = distances[distances.size () >> 1];
+		variance = (2.1981 * median_error_sqr);
+	}
+
+	correspondencesOut = (int)correspondences.size();
+}
+
 // return transform from source to target (All points must be finite!!!)
 Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
-			  bool * hasConvergedOut,
+			  bool & hasConverged,
-			  double * variance,
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered)
-			  int * correspondencesOut)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -247,63 +312,8 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
+	icp.align (cloud_source_registered);
-	icp.align (*cloud_source_registered);
+	hasConverged = icp.hasConverged();
-	bool hasConverged = icp.hasConverged();
-
-	// compute variance
-	if((correspondencesOut || variance) && hasConverged)
-	{
-		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-		est->setInputTarget(cloud_target);
-		est->setInputSource(cloud_source_registered);
-		pcl::Correspondences correspondences;
-		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
-		if(variance)
-		{
-			if(correspondences.size()>=3)
-			{
-				std::vector<double> distances(correspondences.size());
-				for(unsigned int i=0; i<correspondences.size(); ++i)
-				{
-					distances[i] = correspondences[i].distance;
-				}
-
-				//variance
-				std::sort(distances.begin (), distances.end ());
-				double median_error_sqr = distances[distances.size () >> 1];
-				*variance = (2.1981 * median_error_sqr);
-			}
-			else
-			{
-				hasConverged = false;
-				*variance = -1.0;
-			}
-		}
-
-		if(correspondencesOut)
-		{
-			*correspondencesOut = (int)correspondences.size();
-		}
-	}
-	else
-	{
-		if(correspondencesOut)
-		{
-			*correspondencesOut = 0;
-		}
-		if(variance)
-		{
-			*variance = -1;
-		}
-	}
-
-	if(hasConvergedOut)
-	{
-		*hasConvergedOut = hasConverged;
-	}
-
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
@@ -313,9 +323,8 @@ Transform icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool * hasConvergedOut,
+		bool & hasConverged,
-		double * variance,
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered)
-		int * correspondencesOut)
 {
 	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
 	// Set the input source and target
@@ -337,63 +346,8 @@ Transform icpPointToPlane(
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
+	icp.align (cloud_source_registered);
-	icp.align (*cloud_source_registered);
+	hasConverged = icp.hasConverged();
-	bool hasConverged = icp.hasConverged();
-
-	// compute variance
-	if((correspondencesOut || variance) && hasConverged)
-	{
-		pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-		est->setInputTarget(cloud_target);
-		est->setInputSource(cloud_source_registered);
-		pcl::Correspondences correspondences;
-		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
-		if(variance)
-		{
-			if(correspondences.size()>=3)
-			{
-				std::vector<double> distances(correspondences.size());
-				for(unsigned int i=0; i<correspondences.size(); ++i)
-				{
-					distances[i] = correspondences[i].distance;
-				}
-
-				//variance
-				std::sort(distances.begin (), distances.end ());
-				double median_error_sqr = distances[distances.size () >> 1];
-				*variance = (2.1981 * median_error_sqr);
-			}
-			else
-			{
-				hasConverged = false;
-				*variance = -1.0;
-			}
-		}
-
-		if(correspondencesOut)
-		{
-			*correspondencesOut = (int)correspondences.size();
-		}
-	}
-	else
-	{
-		if(correspondencesOut)
-		{
-			*correspondencesOut = 0;
-		}
-		if(variance)
-		{
-			*variance = -1;
-		}
-	}
-
-	if(hasConvergedOut)
-	{
-		*hasConvergedOut = hasConverged;
-	}
-
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
@@ -402,9 +356,8 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
-			  bool * hasConvergedOut,
+			  bool & hasConverged,
-			  double * variance,
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered)
-			  int * correspondencesOut)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -426,63 +379,8 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
+	icp.align (cloud_source_registered);
-	icp.align (*cloud_source_registered);
+	hasConverged = icp.hasConverged();
-	bool hasConverged = icp.hasConverged();
-
-	// compute variance
-	if((correspondencesOut || variance) && hasConverged)
-	{
-		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-		est->setInputTarget(cloud_target);
-		est->setInputSource(cloud_source_registered);
-		pcl::Correspondences correspondences;
-		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
-		if(variance)
-		{
-			if(correspondences.size()>=3)
-			{
-				std::vector<double> distances(correspondences.size());
-				for(unsigned int i=0; i<correspondences.size(); ++i)
-				{
-					distances[i] = correspondences[i].distance;
-				}
-
-				//variance
-				std::sort(distances.begin (), distances.end ());
-				double median_error_sqr = distances[distances.size () >> 1];
-				*variance = (2.1981 * median_error_sqr);
-			}
-			else
-			{
-				hasConverged = false;
-				*variance = -1.0;
-			}
-		}
-
-		if(correspondencesOut)
-		{
-			*correspondencesOut = (int)correspondences.size();
-		}
-	}
-	else
-	{
-		if(correspondencesOut)
-		{
-			*correspondencesOut = 0;
-		}
-		if(variance)
-		{
-			*variance = -1;
-		}
-	}
-
-	if(hasConvergedOut)
-	{
-		*hasConvergedOut = hasConverged;
-	}
-
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 

guilib/src/DatabaseViewer.cpp

@@ -2764,8 +2764,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr scanAVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr scanBVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
 	float correspondenceRatio = 0.0f;
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
@@ -2776,6 +2776,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 		if(!oldLaserScan.empty() && !newLaserScan.empty())
 		{
 			// 2D
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
 			scanA = util3d::cvMat2Cloud(oldLaserScan);
 			scanB = util3d::cvMat2Cloud(newLaserScan, t);
 
@@ -2785,21 +2787,40 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 				scanA = util3d::voxelize(scanA, ui_->doubleSpinBox_icp_voxel->value());
 				scanB = util3d::voxelize(scanB, ui_->doubleSpinBox_icp_voxel->value());
 			}
+			else
+			{
+				scanAVoxelized = scanA;
+				scanBVoxelized = scanB;
+			}
 
 			if(scanB->size() && scanA->size())
 			{
-				transform = util3d::icp2D(scanB,
+				pcl::PointCloud<pcl::PointXYZ>::Ptr scanBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+				transform = util3d::icp2D(
+						scanB,
 						scanA,
 						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 						ui_->spinBox_icp_iteration->value(),
-					   &hasConverged,
+						hasConverged,
-					   &variance,
+						*scanBRegistered);
-					   &correspondences);
 
 				if(!transform.isNull())
 				{
 					if(dataTo.laserScanMaxPts())
 					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr scanBTransformed = scanBRegistered;
+						if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
+						{
+							scanBTransformed = util3d::transformPointCloud(scanB, transform);
+						}
+
+						util3d::computeVarianceAndCorrespondences(
+								scanBTransformed,
+								scanA,
+								ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+								variance,
+								correspondences);
+
 						correspondenceRatio =  float(correspondences)/float(dataTo.laserScanMaxPts());
 					}
 					else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
@@ -2844,25 +2865,38 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 					UWARN("removed nan normals...");
 				}
 
-				transform = util3d::icpPointToPlane(cloudBNormals,
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
+				transform = util3d::icpPointToPlane(
+						cloudBNormals,
 						cloudANormals,
 						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 						ui_->spinBox_icp_iteration->value(),
-						&hasConverged,
+						hasConverged,
-						&variance,
+						*cloudBRegistered);
-						&correspondences);
+				util3d::computeVarianceAndCorrespondences(
+						cloudBRegistered,
+						cloudANormals,
+						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+						variance,
+						correspondences);
 			}
 			else
 			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
 				transform = util3d::icp(cloudB,
 						cloudA,
 						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 						ui_->spinBox_icp_iteration->value(),
-						&hasConverged,
+						hasConverged,
-						&variance,
+						*cloudBRegistered);
-						&correspondences);
+				util3d::computeVarianceAndCorrespondences(
+						cloudBRegistered,
+						cloudA,
+						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+						variance,
+						correspondences);
 			}
-			correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total());
+			correspondenceRatio = float(correspondences)/float(cloudA->size()>cloudB->size()?cloudA->size():cloudB->size());
 		}
 		else
 		{
@@ -2913,8 +2947,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 			if(ui_->dockWidget_constraints->isVisible())
 			{
 				cloudB = util3d::transformPointCloud(cloudB, transform);
-				scanB = util3d::transformPointCloud(scanB, transform);
+				scanBVoxelized = util3d::transformPointCloud(scanBVoxelized, transform);
-				this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
+				this->updateConstraintView(newLink, true, cloudA, cloudB, scanAVoxelized, scanBVoxelized);
 			}
 		}
 	}

guilib/src/MainWindow.cpp

@@ -3505,22 +3505,22 @@ void MainWindow::postProcessing()
 		}
 		_initProgressDialog->appendText(tr("Refining links..."));
 
-		int decimation=8;
+		int decimation=Parameters::defaultLccIcp3Decimation();
-		float maxDepth=2.0f;
+		float maxDepth=Parameters::defaultLccIcp3MaxDepth();
-		float voxelSize=0.01f;
+		float voxelSize=Parameters::defaultLccIcp3VoxelSize();
-		int samples = 0;
+		int samples = Parameters::defaultLccIcp3Samples();
-		float maxCorrespondences = 0.05f;
+		float maxCorrespondenceDistance = Parameters::defaultLccIcp3MaxCorrespondenceDistance();
-		float correspondenceRatio = 0.7f;
+		float correspondenceRatio = Parameters::defaultLccIcp3CorrespondenceRatio();
-		float icpIterations = 30;
+		float icpIterations = Parameters::defaultLccIcp3Iterations();
 		Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
 		Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
 		Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
 		Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
 		Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
-		Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
+		Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondenceDistance);
 		Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
-		bool pointToPlane = false;
+		bool pointToPlane = Parameters::defaultLccIcp3PointToPlane();
-		int pointToPlaneNormalNeighbors = 20;
+		int pointToPlaneNormalNeighbors = Parameters::defaultLccIcp3PointToPlaneNormalNeighbors();
 		Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
 		Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
 
@@ -3605,24 +3605,36 @@ void MainWindow::postProcessing()
 								UWARN("removed nan normals...");
 							}
 
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
 							transform = util3d::icpPointToPlane(cloudBNormals,
 									cloudANormals,
-									maxCorrespondences,
+									maxCorrespondenceDistance,
 									icpIterations,
-									&hasConverged,
+									hasConverged,
-									&variance,
+									*cloudBRegistered);
-									&correspondences);
+							util3d::computeVarianceAndCorrespondences(
+									cloudBRegistered,
+									cloudANormals,
+									maxCorrespondenceDistance,
+									variance,
+									correspondences);
 						}
 						else
 						{
 							UDEBUG("");
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
 							transform = util3d::icp(cloudB,
 									cloudA,
-									maxCorrespondences,
+									maxCorrespondenceDistance,
 									icpIterations,
-									&hasConverged,
+									hasConverged,
-									&variance,
+									*cloudBRegistered);
-									&correspondences);
+							util3d::computeVarianceAndCorrespondences(
+									cloudBRegistered,
+									cloudA,
+									maxCorrespondenceDistance,
+									variance,
+									correspondences);
 						}
 
 						float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());

tools/DataRecorder/CMakeLists.txt

@@ -8,6 +8,7 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${CMAKE_CURRENT_SOURCE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
 )
 
@@ -16,6 +17,7 @@ IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
 ENDIF()
 
 SET(LIBRARIES
+    ${OpenCV_LIBRARIES} 
 	${PCL_LIBRARIES} 
 	${QT_LIBRARIES} 
 )

tools/OdometryViewer/CMakeLists.txt

@@ -4,6 +4,7 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${CMAKE_CURRENT_SOURCE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
 )
 
@@ -12,6 +13,7 @@ IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
 ENDIF()
 
 SET(LIBRARIES
+    ${OpenCV_LIBRARIES} 
 	${PCL_LIBRARIES} 
 	${QT_LIBRARIES} 
 )