OdometryICP: added p2p (point to point) option

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1631 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Odometry.h

@@ -99,6 +99,7 @@ public:
 			float maxCorrespondenceDistance = 0.05f,
 			int maxIterations = 30,
 			float maxFitness = 0.01f,
+			bool pointToPlane = true,
 			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
 	void reset();
 
@@ -112,8 +113,10 @@ private:
 	float _maxCorrespondenceDistance;
 	int	_maxIterations;
 	float _maxFitness;
+	bool _pointToPlane;
 
-	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloud;
+	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
+	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
 };
 
 // return true if odometry is correctly computed

corelib/src/Odometry.cpp

@@ -355,6 +355,7 @@ OdometryICP::OdometryICP(int decimation,
 		float maxCorrespondenceDistance,
 		int maxIterations,
 		float maxFitness,
+		bool pointToPlane,
 		const ParametersMap & odometryParameter) :
 	Odometry(odometryParameter),
 	_decimation(decimation),
@@ -363,14 +364,17 @@ OdometryICP::OdometryICP(int decimation,
 	_maxCorrespondenceDistance(maxCorrespondenceDistance),
 	_maxIterations(maxIterations),
 	_maxFitness(maxFitness),
-	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
+	_pointToPlane(pointToPlane),
+	_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
+	_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
 {
 }
 
 void OdometryICP::reset()
 {
 	Odometry::reset();
-	_previousCloud.reset(new pcl::PointCloud<pcl::PointNormal>);
+	_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
+	_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
 }
 
 // return not null transform if odometry is correctly computed
@@ -396,6 +400,8 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 						_samples,
 						image.localTransform());
 
+		if(_pointToPlane)
+		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
 
 			std::vector<int> indices;
@@ -405,10 +411,10 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 				UWARN("removed nan normals...");
 			}
 
-		if(_previousCloud->size() > minPoints && newCloud->size() > minPoints)
+			if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
 			{
 				Transform transform = util3d::icpPointToPlane(newCloud,
-					_previousCloud,
+						_previousCloudNormal,
 						_maxCorrespondenceDistance,
 						_maxIterations,
 						hasConverged,
@@ -422,7 +428,7 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 				if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
 				{
 					output = transform;
-				_previousCloud = newCloud;
+					_previousCloudNormal = newCloud;
 				}
 				else
 				{
@@ -433,7 +439,42 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 			else if(newCloud->size() > minPoints)
 			{
 				output.setIdentity();
-			_previousCloud = newCloud;
+				_previousCloudNormal = newCloud;
+			}
+		}
+		else
+		{
+			//point to point
+			if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
+			{
+				Transform transform = util3d::icp(newCloudXYZ,
+						_previousCloud,
+						_maxCorrespondenceDistance,
+						_maxIterations,
+						hasConverged,
+						fitness);
+
+				//pcl::io::savePCDFile("old.pcd", *_previousCloudNormal);
+				//pcl::io::savePCDFile("new.pcd", *newCloud);
+				//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
+				//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
+
+				if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
+				{
+					output = transform;
+					_previousCloud = newCloudXYZ;
+				}
+				else
+				{
+					UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
+							hasConverged?"true":"false", fitness, _maxFitness);
+				}
+			}
+			else if(newCloudXYZ->size() > minPoints)
+			{
+				output.setIdentity();
+				_previousCloud = newCloudXYZ;
+			}
 		}
 	}
 	else
@@ -445,7 +486,7 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
 			timer.elapsed(),
 			hasConverged?"true":"false",
 			fitness,
-			(int)_previousCloud->size());
+			(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
 
 	return output;
 }
@@ -511,7 +552,7 @@ void OdometryThread::mainLoop()
 	getImage(image);
 	if(!image.empty())
 	{
-		int quality = 0;
+		int quality = -1;
 		Transform pose = _odometry->process(image, &quality);
 		image.setPose(pose); // a null pose notify that odometry could not be computed
 		this->post(new OdometryEvent(image, quality));

tools/OdometryViewer/main.cpp

@@ -45,6 +45,7 @@ void showUsage()
 			"  -v #                      ICP voxel size (default 0.005)\n"
 			"  -s #                      ICP samples (default 0, not used if voxel is set.)\n"
 			"  -f #.#                    ICP fitness (default 0.01)\n"
+			"  -p2p                      ICP point to point (default point to plane)"
 			"\n"
 			"  -debug                    Log debug messages\n"
 			"\n"
@@ -88,6 +89,7 @@ int main (int argc, char * argv[])
 	float sec = 0.0f;
 	bool gpu = false;
 	int localHistory = 0;
+	bool p2p = false;
 
 	for(int i=1; i<argc; ++i)
 	{
@@ -493,6 +495,11 @@ int main (int argc, char * argv[])
 			icp = true;
 			continue;
 		}
+		if(strcmp(argv[i], "-p2p") == 0)
+		{
+			p2p = true;
+			continue;
+		}
 		if(strcmp(argv[i], "-debug") == 0)
 		{
 			ULogger::setLevel(ULogger::kDebug);
@@ -642,8 +649,9 @@ int main (int argc, char * argv[])
 		UINFO("Cloud voxel size =        %f", voxel);
 		UINFO("Cloud samples =           %d", samples);
 		UINFO("Cloud fitness =           %f", fitness);
+		UINFO("Cloud point to plane =    %s", p2p?"false":"true");
 
-		odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness);
+		odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness, !p2p);
 	}
 	rtabmap::OdometryThread odomThread(odom);
 	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);