GUI: Added "Edit->Post processing..." action to detect more loop closures and/or refine all links in the graph with ICP 3D.

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

corelib/include/rtabmap/core/Memory.h

@@ -245,6 +245,8 @@ private:
 	float _icpMaxCorrespondenceDistance;
 	int _icpMaxIterations;
 	float _icpMaxFitness;
+	bool _icpPointToPlane;
+	int _icpPointToPlaneNormalNeighbors;
 	float _icp2MaxCorrespondenceDistance;
 	int _icp2MaxIterations;
 	float _icp2MaxFitness;

corelib/include/rtabmap/core/Parameters.h

@@ -302,11 +302,13 @@ class RTABMAP_EXP Parameters
 
 	RTABMAP_PARAM(LccIcp3, Decimation,      int, 8, 		"Depth image decimation.");
 	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 4.0, 	"Max cloud depth.");
-	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.005, 	"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.01, 	"Voxel size to be used for ICP computation.");
 	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
 	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"ICP 3D: Max iterations.");
 	RTABMAP_PARAM(LccIcp3, MaxFitness,      float, 1.0, 	"ICP 3D: Maximum fitness to accept the computed transform.");
+	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"ICP 3D: Use point to plane ICP.");
+	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"ICP 3D: Number of neighbors to compute normals for point to plane.");
 
 	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"ICP 2D: Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");

corelib/include/rtabmap/core/util3d.h

@@ -443,11 +443,30 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumAngle = 2*M_PI/3,
 		bool gp3NormalConsistency = false);
 
+std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
+		std::multimap<int, Link> & links,
+		int from,
+		int to);
+
+// <int, depth> depth=0 means infinite depth
+std::map<int, int> RTABMAP_EXP generateDepthGraph(
+		const std::multimap<int, Link> & links,
+		int fromId,
+		int depth = 0);
+
+void RTABMAP_EXP optimizeTOROGraph(
+		const std::map<int, int> & depthGraph,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		std::map<int, Transform> & optimizedPoses,
+		int toroIterations = 100,
+		bool toroInitialGuess = true,
+		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+
 void RTABMAP_EXP optimizeTOROGraph(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
 		std::map<int, Transform> & optimizedPoses,
-		Transform & mapCorrection,
 		int toroIterations = 100,
 		bool toroInitialGuess = true,
 		std::list<std::map<int, Transform> > * intermediateGraphes = 0);

corelib/src/Memory.cpp

@@ -95,6 +95,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_icpMaxCorrespondenceDistance(Parameters::defaultLccIcp3MaxCorrespondenceDistance()),
 	_icpMaxIterations(Parameters::defaultLccIcp3Iterations()),
 	_icpMaxFitness(Parameters::defaultLccIcp3MaxFitness()),
+	_icpPointToPlane(Parameters::defaultLccIcp3PointToPlane()),
+	_icpPointToPlaneNormalNeighbors(Parameters::defaultLccIcp3PointToPlaneNormalNeighbors()),
 
 	_icp2MaxCorrespondenceDistance(Parameters::defaultLccIcp2MaxCorrespondenceDistance()),
 	_icp2MaxIterations(Parameters::defaultLccIcp2Iterations()),
@@ -403,6 +405,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance);
 	Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), _icpMaxIterations);
 	Parameters::parse(parameters, Parameters::kLccIcp3MaxFitness(), _icpMaxFitness);
+	Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), _icpPointToPlane);
+	Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors);
 	Parameters::parse(parameters, Parameters::kLccIcp2MaxCorrespondenceDistance(), _icp2MaxCorrespondenceDistance);
 	Parameters::parse(parameters, Parameters::kLccIcp2Iterations(), _icp2MaxIterations);
 	Parameters::parse(parameters, Parameters::kLccIcp2MaxFitness(), _icp2MaxFitness);
@@ -429,6 +433,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str());
 	UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str());
 	UASSERT_MSG(_icpMaxFitness > 0.0f, uFormat("value=%f", _icpMaxFitness).c_str());
+	UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str());
 	UASSERT_MSG(_icp2MaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icp2MaxCorrespondenceDistance).c_str());
 	UASSERT_MSG(_icp2MaxIterations > 0, uFormat("value=%d", _icp2MaxIterations).c_str());
 	UASSERT_MSG(_icp2MaxFitness > 0.0f, uFormat("value=%f", _icp2MaxFitness).c_str());
@@ -1844,28 +1849,17 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
 		//make sure data are uncompressed
 		if(icp3D)
 		{
-			if(oldS->getDepthRaw().empty())
+			cv::Mat tmp1, tmp2;
-			{
+			oldS->uncompressData(0, &tmp1, 0);
-				oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepthCompressed()));
+			newS->uncompressData(0, &tmp2, 0);
-			}
-			if(newS->getDepthRaw().empty())
-			{
-				newS->setDepthRaw(util3d::uncompressImage(newS->getDepthCompressed()));
-			}
 		}
 		else
 		{
-			if(oldS->getDepth2DRaw().empty())
+			cv::Mat tmp1, tmp2;
-			{
+			oldS->uncompressData(0, 0, &tmp1);
-				oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2DCompressed()));
+			newS->uncompressData(0, 0, &tmp2);
-			}
-			if(newS->getDepth2DRaw().empty())
-			{
-				newS->setDepth2DRaw(util3d::uncompressData(newS->getDepth2DCompressed()));
-			}
 		}
 
-
 		t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg);
 	}
 	else
@@ -1933,25 +1927,40 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 						_icpSamples,
 						guess * newS.getLocalTransform());
 
-				pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ);
-				pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
-
-				std::vector<int> indices;
-				newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
-				oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
-
 				// 3D
 				double fitness = 0;
 				bool hasConverged = false;
 				Transform icpT;
-				if(newCloud->size() && oldCloud->size())
+				if(newCloudXYZ->size() && oldCloudXYZ->size())
 				{
-					icpT = util3d::icpPointToPlane(newCloud,
+					if(_icpPointToPlane)
-							oldCloud,
+					{
-						   _icpMaxCorrespondenceDistance,
+						pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors);
-						   _icpMaxIterations,
+						pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ, _icpPointToPlaneNormalNeighbors);
-						   hasConverged,
+
-						   fitness);
+						std::vector<int> indices;
+						newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
+						oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
+
+						if(newCloud->size() && oldCloud->size())
+						{
+							icpT = util3d::icpPointToPlane(newCloud,
+									oldCloud,
+								   _icpMaxCorrespondenceDistance,
+								   _icpMaxIterations,
+								   hasConverged,
+								   fitness);
+						}
+					}
+					else
+					{
+						transform = util3d::icp(newCloudXYZ,
+								oldCloudXYZ,
+								_icpMaxCorrespondenceDistance,
+								_icpMaxIterations,
+								hasConverged,
+								fitness);
+					}
 
 					//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
 					//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);

corelib/src/Rtabmap.cpp

@@ -2002,78 +2002,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		else
 		{
-
+			util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true);
-			// Modify IDs using the margin from the current signature (TORO root will be the last signature)
-			int m = 0;
-			int toroId = 1;
-			std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
-			std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
-			while(ids.size())
-			{
-				for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end();)
-				{
-					if(m == iter->second)
-					{
-						rtabmapToToro.insert(std::make_pair(iter->first, toroId));
-						toroToRtabmap.insert(std::make_pair(toroId, iter->first));
-						++toroId;
-						ids.erase(iter++);
-					}
-					else
-					{
-						++iter;
-					}
-				}
-				++m;
-			}
-
-			//
-			std::map<int, Transform> posesToro;
-			std::multimap<int, Link> edgeConstraintsToro;
-			for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-			{
-				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
-			}
-			for(std::multimap<int, Link>::iterator iter = edgeConstraints.begin();
-				iter!=edgeConstraints.end();
-				++iter)
-			{
-				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
-			}
-
-			if(posesToro.size() > 1 && edgeConstraintsToro.size() > 0)
-			{
-				///UDEBUG("TORO optimize begin");
-				//util3d::saveTOROGraph("toroIdRtabmap.graph", poses, edgeConstraints);
-				//util3d::saveTOROGraph("toroIdToro.graph", posesToro, edgeConstraintsToro);
-				//UDEBUG("graph saved");
-
-				std::map<int, Transform> optimizedPosesToro;
-				Transform mapCorrectionToro;
-				util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, mapCorrectionToro, _toroIterations, true);
-
-				//UDEBUG("saving optimized graph...");
-				//util3d::saveTOROGraph("toroOptimized.graph", optimizedPosesToro, edgeConstraintsToro);
-				//UDEBUG("TORO optimize end");
-
-				for(std::map<int, Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
-				{
-					optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
-				}
-
-				//util3d::saveTOROGraph("toro.graph", poses, edgeConstraints);
-				//util3d::saveTOROGraph("toroOptimized.graph", _optimizedPoses, edgeConstraints);
-			}
-			else if(poses.size() == 1 && edgeConstraints.size() == 0)
-			{
-				optimizedPoses = poses;
-			}
-			else if(poses.size() || edgeConstraints.size())
-			{
-				UFATAL("Poses=%d and edges=%d (poses must "
-					   "not be null if there are edges, and edges must be null if poses <= 1)",
-					   poses.size(), edgeConstraints.size());
-			}
 		}
 	}
 }

corelib/src/util3d.cpp

@@ -2049,16 +2049,201 @@ pcl::PolygonMesh::Ptr createMesh(
 	return mesh;
 }
 
+std::multimap<int, Link>::iterator findLink(
+		std::multimap<int, Link> & links,
+		int from,
+		int to)
+{
+	std::multimap<int, Link>::iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second.to() == to)
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	// let's try to -> from
+	iter = links.find(to);
+	while(iter != links.end() && iter->first == to)
+	{
+		if(iter->second.to() == from)
+		{
+			return iter;
+		}
+		++iter;
+	}
+	return links.end();
+}
+
+
+// <int, depth> margin=0 means infinite margin
+std::map<int, int> generateDepthGraph(
+		const std::multimap<int, Link> & links,
+		int fromId,
+		int depth)
+{
+	UASSERT(depth >= 0);
+	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
+	std::map<int, int> ids;
+	if(fromId<=0)
+	{
+		return ids;
+	}
+
+	std::list<int> curentDepthList;
+	std::set<int> nextDepth;
+	nextDepth.insert(fromId);
+	int d = 0;
+	while((depth == 0 || d < depth) && nextDepth.size())
+	{
+		curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
+		nextDepth.clear();
+
+		for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
+		{
+			if(ids.find(*jter) == ids.end())
+			{
+				std::set<int> marginIds;
+
+				ids.insert(std::pair<int, int>(*jter, d));
+
+				for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->second.from() == *jter)
+					{
+						marginIds.insert(iter->second.to());
+					}
+					else if(iter->second.to() == *jter)
+					{
+						marginIds.insert(iter->second.from());
+					}
+				}
+
+				// Margin links
+				for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
+				{
+					if( !uContains(ids, *iter) && nextDepth.find(*iter) == nextDepth.end())
+					{
+						nextDepth.insert(*iter);
+					}
+				}
+			}
+		}
+		++d;
+	}
+	return ids;
+}
+
+void optimizeTOROGraph(
+		const std::map<int, int> & depthGraph,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		std::map<int, Transform> & optimizedPoses,
+		int toroIterations,
+		bool toroInitialGuess,
+		std::list<std::map<int, Transform> > * intermediateGraphes)
+{
+	optimizedPoses.clear();
+	if(depthGraph.size() && poses.size()>=2 && links.size()>=1)
+	{
+		// Modify IDs using the margin from the current signature (TORO root will be the last signature)
+		int m = 0;
+		int toroId = 1;
+		std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
+		std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
+		std::map<int, int> idsTmp = depthGraph;
+		while(idsTmp.size())
+		{
+			for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
+			{
+				if(m == iter->second)
+				{
+					rtabmapToToro.insert(std::make_pair(iter->first, toroId));
+					toroToRtabmap.insert(std::make_pair(toroId, iter->first));
+					++toroId;
+					idsTmp.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+			++m;
+		}
+
+		std::map<int, rtabmap::Transform> posesToro;
+		std::multimap<int, rtabmap::Link> edgeConstraintsToro;
+		for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(uContains(depthGraph, iter->first))
+			{
+				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
+			}
+		}
+		for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
+			iter!=links.end();
+			++iter)
+		{
+			if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
+			{
+				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
+			}
+		}
+
+		std::map<int, rtabmap::Transform> optimizedPosesToro;
+
+		// Optimize!
+		if(posesToro.size() && edgeConstraintsToro.size())
+		{
+			std::list<std::map<int, rtabmap::Transform> > graphesToro;
+			rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, toroIterations, toroInitialGuess, &graphesToro);
+			for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
+			{
+				optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
+			}
+
+			if(intermediateGraphes)
+			{
+				for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
+				{
+					std::map<int, rtabmap::Transform> tmp;
+					for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
+					{
+						tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
+					}
+					intermediateGraphes->push_back(tmp);
+				}
+			}
+		}
+		else
+		{
+			UERROR("No TORO poses and constraints!?");
+		}
+	}
+	else if(links.size() == 0 && poses.size() == 1)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UERROR("Wrong inputs! depthGraph=%d poses=%d links=%d",
+				(int)depthGraph.size(), (int)poses.size(), (int)links.size());
+	}
+}
+
 //On success, optimizedPoses is cleared and new poses are inserted in
 void optimizeTOROGraph(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
 		std::map<int, Transform> & optimizedPoses,
-		Transform & mapCorrection,
 		int toroIterations,
 		bool toroInitialGuess,
 		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
 {
+	UASSERT(toroIterations>0);
+	optimizedPoses.clear();
 	if(edgeConstraints.size()>=1 && poses.size()>=2)
 	{
 		// Apply TORO optimization
@@ -2133,7 +2318,6 @@ void optimizeTOROGraph(
 		}
 		UDEBUG("TORO iterate end");
 
-		optimizedPoses.clear();
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
@@ -2143,15 +2327,19 @@ void optimizeTOROGraph(
 			optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
 		}
 
-		Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
+		//Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
-		Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
+		//Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
-		Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
+		//Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
-		Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
+		//Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
-		mapCorrection = transformFromEigen4f(result);
+		//mapCorrection = transformFromEigen4f(result);
+	}
+	else if(edgeConstraints.size() == 0 && poses.size() == 1)
+	{
+		optimizedPoses = poses;
 	}
 	else
 	{
-		UWARN("This method should be called at least with 2 poses and one link!");
+		UWARN("This method should be called at least with 1 pose!");
 	}
 }
 
@@ -2707,7 +2895,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 	pcl::PointCloud<pcl::PointXYZ> minMax;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
-		if(uContains(scans, iter->first))
+		if(uContains(scans, iter->first) && scans.size())
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
 			pcl::PointXYZ min, max;

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -106,10 +106,6 @@ private:
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
 	void updateConstraintButtons();
-	std::multimap<int, Link>::iterator findLink(
-				std::multimap<int, Link> & links,
-				int from,
-				int to);
 	Link findActiveLink(int from, int to);
 	bool containsLink(
 			std::multimap<int, Link> & links,
@@ -120,12 +116,6 @@ private:
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
 	void refineConstraint(int from, int to);
 	bool addConstraint(int from, int to, bool silent);
-	std::map<int, int> generateGraph(int fromNode, int margin);
-	std::map<int, Transform> optimizeGraph(
-			const std::map<int, int> & ids,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & links,
-			std::list<std::map<int, rtabmap::Transform> > * graphes = 0);
 
 private:
 	Ui_DatabaseViewer * ui_;

guilib/include/rtabmap/gui/MainWindow.h

@@ -64,6 +64,7 @@ class StatsToolBox;
 class DetailedProgressDialog;
 class TwistGridWidget;
 class ExportCloudsDialog;
+class PostProcessingDialog;
 
 class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler
 {
@@ -123,6 +124,7 @@ private slots:
 	void generateMap();
 	void generateLocalMap();
 	void generateTOROMap();
+	void postProcessing();
 	void deleteMemory();
 	void openWorkingDirectory();
 	void updateEditMenu();
@@ -249,6 +251,7 @@ private:
 	PreferencesDialog * _preferencesDialog;
 	AboutDialog * _aboutDialog;
 	ExportCloudsDialog * _exportDialog;
+	PostProcessingDialog * _postProcessingDialog;
 
 	QSet<int> _lastIds;
 	int _lastId;

guilib/src/CMakeLists.txt

@@ -19,6 +19,7 @@ SET(headers_ui
 	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
 	../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
 	./ExportDialog.h
+	./PostProcessingDialog.h
 	./ExportCloudsDialog.h
 	./MapVisibilityWidget.h
 )
@@ -31,6 +32,7 @@ SET(uis
     ./ui/DatabaseViewer.ui
     ./ui/loopClosureViewer.ui
     ./ui/exportDialog.ui
+    ./ui/postProcessingDialog.ui
     ./ui/exportCloudsDialog.ui
     ./ui/calibrationDialog.ui
 )
@@ -70,6 +72,7 @@ SET(SRC_FILES
 	./DataRecorder.cpp
 	./CalibrationDialog.cpp
 	./ExportDialog.cpp
+	./PostProcessingDialog.cpp
 	./ExportCloudsDialog.cpp
 	./MapVisibilityWidget.cpp
 	./GraphViewer.cpp

guilib/src/DatabaseViewer.cpp

@@ -227,7 +227,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 			// Added links
 			for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
 			{
-				std::multimap<int, rtabmap::Link>::iterator refinedIter = this->findLink(linksRefined_, iter->second.from(), iter->second.to());
+				std::multimap<int, rtabmap::Link>::iterator refinedIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
 					memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), true);
@@ -541,156 +541,6 @@ void DatabaseViewer::generateTOROGraph()
 	}
 }
 
-// margin=0 means infinite margin
-std::map<int, int> DatabaseViewer::generateGraph(int fromNode, int margin)
-{
-	UASSERT(margin >= 0);
-	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
-	std::map<int, int> ids;
-	if(fromNode<=0)
-	{
-		return ids;
-	}
-
-	std::list<int> curentMarginList;
-	std::set<int> currentMargin;
-	std::set<int> nextMargin;
-	nextMargin.insert(fromNode);
-	int m = 0;
-	while((margin == 0 || m < margin) && nextMargin.size())
-	{
-		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
-		nextMargin.clear();
-
-		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
-		{
-			if(ids.find(*jter) == ids.end())
-			{
-				std::set<int> marginIds;
-
-				ids.insert(std::pair<int, int>(*jter, m));
-
-				for(int i=0; i<neighborLinks_.size(); ++i)
-				{
-					if(neighborLinks_[i].from() == *jter)
-					{
-						marginIds.insert(neighborLinks_[i].to());
-					}
-					else if(neighborLinks_[i].to() == *jter)
-					{
-						marginIds.insert(neighborLinks_[i].from());
-					}
-				}
-				for(int i=0; i<loopLinks_.size(); ++i)
-				{
-					if(loopLinks_[i].from() == *jter)
-					{
-						marginIds.insert(loopLinks_[i].to());
-					}
-					else if(loopLinks_[i].to() == *jter)
-					{
-						marginIds.insert(loopLinks_[i].from());
-					}
-				}
-
-				// Margin links
-				for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
-				{
-					if( !uContains(ids, *iter) && nextMargin.find(*iter) == nextMargin.end())
-					{
-						nextMargin.insert(*iter);
-					}
-				}
-			}
-		}
-		++m;
-	}
-	return ids;
-}
-
-std::map<int, Transform> DatabaseViewer::optimizeGraph(
-		const std::map<int, int> & ids,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		std::list<std::map<int, rtabmap::Transform> > * graphes)
-{
-	std::map<int, rtabmap::Transform> optimizedPoses;
-	if(ids.size() && poses.size())
-	{
-		// Modify IDs using the margin from the current signature (TORO root will be the last signature)
-		int m = 0;
-		int toroId = 1;
-		std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
-		std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
-		std::map<int, int> idsTmp = ids;
-		while(idsTmp.size())
-		{
-			for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
-			{
-				if(m == iter->second)
-				{
-					rtabmapToToro.insert(std::make_pair(iter->first, toroId));
-					toroToRtabmap.insert(std::make_pair(toroId, iter->first));
-					++toroId;
-					idsTmp.erase(iter++);
-				}
-				else
-				{
-					++iter;
-				}
-			}
-			++m;
-		}
-
-		std::map<int, rtabmap::Transform> posesToro;
-		std::multimap<int, rtabmap::Link> edgeConstraintsToro;
-		for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-		{
-			if(uContains(ids, iter->first))
-			{
-				posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
-			}
-		}
-		for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
-			iter!=links.end();
-			++iter)
-		{
-			if(uContains(ids, iter->second.from()) && uContains(ids, iter->second.to()))
-			{
-				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
-			}
-		}
-
-		std::map<int, rtabmap::Transform> optimizedPosesToro;
-		rtabmap::Transform mapCorrectionToro;
-
-		// Optimize!
-		if(posesToro.size() && edgeConstraintsToro.size())
-		{
-			std::list<std::map<int, rtabmap::Transform> > graphesToro;
-			rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, mapCorrectionToro, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphesToro);
-			for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
-			{
-				optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
-			}
-
-			if(graphes)
-			{
-				for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
-				{
-					std::map<int, rtabmap::Transform> tmp;
-					for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
-					{
-						tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
-					}
-					graphes->push_back(tmp);
-				}
-			}
-		}
-	}
-	return optimizedPoses;
-}
-
 void DatabaseViewer::view3DMap()
 {
 	if(!ids_.size() || !memory_)
@@ -715,17 +565,19 @@ void DatabaseViewer::view3DMap()
 			double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
 			if(ok)
 			{
+				std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
 				// <id, depth>
-				std::map<int, int> ids = generateGraph(ui_->spinBox_optimizationsFrom->value(), margin);
+				std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), margin);
-				if(ids.size() > 0)
+				if(depthGraph.size() > 0)
 				{
 					rtabmap::DetailedProgressDialog progressDialog(this);
-					progressDialog.setMaximumSteps(ids.size()+2);
+					progressDialog.setMaximumSteps(depthGraph.size()+2);
 					progressDialog.show();
 
 					progressDialog.appendText("Graph optimization...");
 					std::multimap<int, Link> links = updateLinksWithModifications(links_);
-					std::map<int, Transform> optimizedPoses = optimizeGraph(ids, poses_, links);
+					std::map<int, Transform> optimizedPoses;
+					util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
 					progressDialog.appendText("Graph optimization... done!");
 					progressDialog.incrementStep();
 
@@ -846,28 +698,29 @@ void DatabaseViewer::generate3DMap()
 					QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
 					if(!path.isEmpty())
 					{
-						std::map<int, int> ids = this->generateGraph(id, margin);
+						std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-						if(ids.size() > 0)
+						// <id, depth>
+						std::map<int, int> depthGraph = util3d::generateDepthGraph(links, id, margin);
+						if(depthGraph.size() > 0)
 						{
 							rtabmap::DetailedProgressDialog progressDialog;
-							progressDialog.setMaximumSteps((int)ids.size()+2);
+							progressDialog.setMaximumSteps((int)depthGraph.size()+2);
 							progressDialog.show();
 
 							progressDialog.appendText("Graph generation...");
 							std::map<int, rtabmap::Transform> poses, optimizedPoses;
 							std::multimap<int, rtabmap::Link> edgeConstraints;
-							memory_->getMetricConstraints(uKeys(ids), poses, edgeConstraints, true);
+							memory_->getMetricConstraints(uKeys(depthGraph), poses, edgeConstraints, true);
 							edgeConstraints = updateLinksWithModifications(edgeConstraints);
 							progressDialog.appendText("Graph generation... done!");
 							progressDialog.incrementStep();
 
 							progressDialog.appendText("Graph optimization...");
-							rtabmap::Transform mapCorrection;
+							rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
-							rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, mapCorrection, 100, true);
 							progressDialog.appendText("Graph optimization... done!");
 							progressDialog.incrementStep();
 
-							for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+							for(std::map<int, int>::iterator iter = depthGraph.begin(); iter!=depthGraph.end(); ++iter)
 							{
 								rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
 								if(!pose.isNull())
@@ -920,7 +773,7 @@ void DatabaseViewer::generate3DMap()
 							}
 							progressDialog.setValue(progressDialog.maximumSteps());
 
-							QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(ids.size()).arg(path));
+							QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(depthGraph.size()).arg(path));
 						}
 						else
 						{
@@ -936,29 +789,50 @@ void DatabaseViewer::generate3DMap()
 void DatabaseViewer::detectMoreLoopClosures()
 {
 	std::map<int, rtabmap::Transform> optimizedPoses;
-	std::map<int, int> ids = this->generateGraph(ui_->spinBox_optimizationsFrom->value(), 0);
-	std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-	optimizedPoses = optimizeGraph(ids, poses_, links);
 
-	std::multimap<int, int> clusters = util3d::radiusPosesClustering(
+	std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-			optimizedPoses,
+	std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value());
-			ui_->doubleSpinBox_detectMore_radius->value(),
+	util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
-			ui_->doubleSpinBox_detectMore_angle->value());
+
+	int iterations = ui_->doubleSpinBox_detectMore_iterations->value();
+	UASSERT(iterations > 0);
 	int added = 0;
-	for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
+	for(int n=0; n<iterations; ++n)
 	{
-		int from = iter->first;
+		UINFO("iteration %d/%d", n+1, iterations);
-		int to = iter->second;
+		std::multimap<int, int> clusters = util3d::radiusPosesClustering(
-		if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to))
+				optimizedPoses,
+				ui_->doubleSpinBox_detectMore_radius->value(),
+				ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
+		std::set<int> addedLinks;
+		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
 		{
-			if(addConstraint(from, to, true))
+			int from = iter->first;
+			int to = iter->second;
+			if(from < to)
 			{
-				UINFO("Added new loop closure between %d and %d.", from, to);
+				from = iter->second;
-				++added;
+				to = iter->first;
+			}
+			if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
+				addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
+			{
+				if(addConstraint(from, to, true))
+				{
+					UINFO("Added new loop closure between %d and %d.", from, to);
+					++added;
+					addedLinks.insert(from);
+					addedLinks.insert(to);
+				}
 			}
 		}
+		UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
+		if(addedLinks.size() == 0)
+		{
+			break;
+		}
 	}
-	UINFO("Added %d loop closures.", added);
+	UINFO("Total added %d loop closures.", added);
 }
 
 void DatabaseViewer::refineAllNeighborLinks()
@@ -1292,7 +1166,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo)
 {
-	std::multimap<int, Link>::iterator iter = findLink(linksRefined_, link.from(), link.to());
+	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, link.from(), link.to());
 	rtabmap::Transform t = link.transform();
 	if(iter != linksRefined_.end())
 	{
@@ -1342,101 +1216,104 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 					false); // don't update constraints view!
 	}
 
-	if(cloudFrom->size() == 0 && cloudTo->size() == 0)
+	if(ui_->constraintsViewer->isVisible())
 	{
-		Signature dataFrom, dataTo;
+		if(cloudFrom->size() == 0 && cloudTo->size() == 0)
-
-		dataFrom = memory_->getSignatureData(link.from(), true);
-		UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
-		UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
-
-		dataTo = memory_->getSignatureData(link.to(), true);
-		UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
-		UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
-
-
-		//cloud 3d
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
-		if(dataFrom.getDepthRaw().type() == CV_8UC1)
 		{
-			cloudFrom = rtabmap::util3d::cloudFromStereoImages(
+			Signature dataFrom, dataTo;
-					dataFrom.getImageRaw(),
+
-					dataFrom.getDepthRaw(),
+			dataFrom = memory_->getSignatureData(link.from(), true);
-					dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+			UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
-					dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+			UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
-					1);
+
+			dataTo = memory_->getSignatureData(link.to(), true);
+			UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
+			UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
+
+
+			//cloud 3d
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
+			if(dataFrom.getDepthRaw().type() == CV_8UC1)
+			{
+				cloudFrom = rtabmap::util3d::cloudFromStereoImages(
+						dataFrom.getImageRaw(),
+						dataFrom.getDepthRaw(),
+						dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+						dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+						1);
+			}
+			else
+			{
+				cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
+						dataFrom.getImageRaw(),
+						dataFrom.getDepthRaw(),
+						dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+						dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+						1);
+			}
+
+			cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
+			cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
+			if(dataTo.getDepthRaw().type() == CV_8UC1)
+			{
+				cloudTo = rtabmap::util3d::cloudFromStereoImages(
+						dataTo.getImageRaw(),
+						dataTo.getDepthRaw(),
+						dataTo.getDepthCx(), dataTo.getDepthCy(),
+						dataTo.getDepthFx(), dataTo.getDepthFy(),
+						1);
+			}
+			else
+			{
+				cloudTo = rtabmap::util3d::cloudFromDepthRGB(
+						dataTo.getImageRaw(),
+						dataTo.getDepthRaw(),
+						dataTo.getDepthCx(), dataTo.getDepthCy(),
+						dataTo.getDepthFx(), dataTo.getDepthFy(),
+						1);
+			}
+
+			cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
+			cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
+
+			//cloud 2d
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
+			scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
+			scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
+
+			if(cloudFrom->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
+			}
+			if(cloudTo->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+			}
+			if(scanA->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
+			}
+			if(scanB->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
+			}
 		}
 		else
 		{
-			cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
+			if(cloudFrom->size())
-					dataFrom.getImageRaw(),
+			{
-					dataFrom.getDepthRaw(),
+				ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
-					dataFrom.getDepthCx(), dataFrom.getDepthCy(),
+			}
-					dataFrom.getDepthFx(), dataFrom.getDepthFy(),
+			if(cloudTo->size())
-					1);
+			{
-		}
+				ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
-
+			}
-		cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
-		cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
-
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
-		if(dataTo.getDepthRaw().type() == CV_8UC1)
-		{
-			cloudTo = rtabmap::util3d::cloudFromStereoImages(
-					dataTo.getImageRaw(),
-					dataTo.getDepthRaw(),
-					dataTo.getDepthCx(), dataTo.getDepthCy(),
-					dataTo.getDepthFx(), dataTo.getDepthFy(),
-					1);
-		}
-		else
-		{
-			cloudTo = rtabmap::util3d::cloudFromDepthRGB(
-					dataTo.getImageRaw(),
-					dataTo.getDepthRaw(),
-					dataTo.getDepthCx(), dataTo.getDepthCy(),
-					dataTo.getDepthFx(), dataTo.getDepthFy(),
-					1);
-		}
-
-		cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
-		cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
-
-		//cloud 2d
-		pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-		scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
-		scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
-		scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
-
-		if(cloudFrom->size())
-		{
-			ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
-		}
-		if(cloudTo->size())
-		{
-			ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
-		}
-		if(scanA->size())
-		{
-			ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
-		}
-		if(scanB->size())
-		{
-			ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
 		}
+		ui_->constraintsViewer->render();
 	}
-	else
-	{
-		if(cloudFrom->size())
-		{
-			ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
-		}
-		if(cloudTo->size())
-		{
-			ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
-		}
-	}
-	ui_->constraintsViewer->render();
 
 	// update buttons
 	updateConstraintButtons();
@@ -1472,7 +1349,7 @@ void DatabaseViewer::updateConstraintButtons()
 
 		//check for modified link
 		bool modified = false;
-		std::multimap<int, Link>::iterator iter = findLink(linksRefined_, currentLink.from(), currentLink.to());
+		std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, currentLink.from(), currentLink.to());
 		if(iter != linksRefined_.end())
 		{
 			currentLink = iter->second;
@@ -1560,9 +1437,9 @@ void DatabaseViewer::updateGraphView()
 		std::map<int, rtabmap::Transform> finalPoses;
 		graphes_.push_back(poses_);
 		ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
-		std::map<int, int> ids = this->generateGraph(ui_->spinBox_optimizationsFrom->value(), 0);
 		std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-		finalPoses = optimizeGraph(ids, poses_, links, &graphes_);
+		std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
+		util3d::optimizeTOROGraph(depthGraph, poses_, links, finalPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphes_);
 		graphes_.push_back(finalPoses);
 	}
 	if(graphes_.size())
@@ -1578,52 +1455,24 @@ void DatabaseViewer::updateGraphView()
 	}
 }
 
-std::multimap<int, Link>::iterator DatabaseViewer::findLink(
-		std::multimap<int, Link> & links,
-		int from,
-		int to)
-{
-	std::multimap<int, Link>::iterator iter = links.find(from);
-	while(iter != links.end() && iter->first == from)
-	{
-		if(iter->second.to() == to)
-		{
-			return iter;
-		}
-		++iter;
-	}
-
-	// let's try to -> from
-	iter = links.find(to);
-	while(iter != links.end() && iter->first == to)
-	{
-		if(iter->second.to() == from)
-		{
-			return iter;
-		}
-		++iter;
-	}
-	return links.end();
-}
-
 Link DatabaseViewer::findActiveLink(int from, int to)
 {
 	Link link;
-	std::multimap<int, Link>::iterator findIter = findLink(linksRefined_, from ,to);
+	std::multimap<int, Link>::iterator findIter = util3d::findLink(linksRefined_, from ,to);
 	if(findIter != linksRefined_.end())
 	{
 		link = findIter->second;
 	}
 	else
 	{
-		findIter = findLink(linksAdded_, from ,to);
+		findIter = util3d::findLink(linksAdded_, from ,to);
 		if(findIter != linksAdded_.end())
 		{
 			link = findIter->second;
 		}
 		else if(!containsLink(linksRemoved_, from ,to))
 		{
-			findIter = findLink(links_, from ,to);
+			findIter = util3d::findLink(links_, from ,to);
 			if(findIter != links_.end())
 			{
 				link = findIter->second;
@@ -1635,7 +1484,7 @@ Link DatabaseViewer::findActiveLink(int from, int to)
 
 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
 {
-	return findLink(links, from, to) != links.end();
+	return util3d::findLink(links, from, to) != links.end();
 }
 
 void DatabaseViewer::refineConstraint()
@@ -1900,7 +1749,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 	else if(containsLink(linksRemoved_, from, to))
 	{
 		//simply remove from linksRemoved
-		linksRemoved_.erase(findLink(linksRemoved_, from, to));
+		linksRemoved_.erase(util3d::findLink(linksRemoved_, from, to));
 		updateSlider = true;
 	}
 
@@ -1929,18 +1778,18 @@ void DatabaseViewer::resetConstraint()
 	}
 
 
-	std::multimap<int, Link>::iterator iter = findLink(linksRefined_, from, to);
+	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 	}
 
-	iter = findLink(links_, from, to);
+	iter = util3d::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		this->updateConstraintView(iter->second);
 	}
-	iter = findLink(linksAdded_, from, to);
+	iter = util3d::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		this->updateConstraintView(iter->second);
@@ -1966,7 +1815,7 @@ void DatabaseViewer::rejectConstraint()
 
 	// find the original one
 	std::multimap<int, Link>::iterator iter;
-	iter = findLink(links_, from, to);
+	iter = util3d::findLink(links_, from, to);
 	if(iter != links_.end())
 	{
 		if(iter->second.type() == Link::kNeighbor)
@@ -1978,12 +1827,12 @@ void DatabaseViewer::rejectConstraint()
 	}
 
 	// remove from refined and added
-	iter = findLink(linksRefined_, from, to);
+	iter = util3d::findLink(linksRefined_, from, to);
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
 	}
-	iter = findLink(linksAdded_, from, to);
+	iter = util3d::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		linksAdded_.erase(iter);
@@ -2001,7 +1850,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 	{
 		std::multimap<int, rtabmap::Link>::iterator findIter;
 
-		findIter = findLink(linksRemoved_, iter->second.from(), iter->second.to());
+		findIter = util3d::findLink(linksRemoved_, iter->second.from(), iter->second.to());
 		if(findIter != linksRemoved_.end())
 		{
 			if(!(iter->second.from() == findIter->second.from() &&
@@ -2019,7 +1868,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			}
 		}
 
-		findIter = findLink(linksRefined_, iter->second.from(), iter->second.to());
+		findIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
 		if(findIter!=linksRefined_.end())
 		{
 			if(iter->second.from() == findIter->second.from() &&

guilib/src/MainWindow.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/Signature.h"
+#include "rtabmap/core/Memory.h"
 
 #include "rtabmap/gui/ImageView.h"
 #include "rtabmap/gui/KeypointItem.h"
@@ -55,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "PdfPlot.h"
 #include "StatsToolBox.h"
 #include "DetailedProgressDialog.h"
+#include "PostProcessingDialog.h"
 
 #include <QtGui/QCloseEvent>
 #include <QtGui/QPixmap>
@@ -139,6 +141,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	// Create dialogs
 	_aboutDialog = new AboutDialog(this);
 	_exportDialog = new ExportCloudsDialog(this);
+	_postProcessingDialog = new PostProcessingDialog(this);
 
 	_ui = new Ui_mainWindow();
 	_ui->setupUi(this);
@@ -281,6 +284,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
 	connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
 	connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
+	connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
 
 	_ui->actionPause->setShortcut(Qt::Key_Space);
 	_ui->actionSave_point_cloud->setEnabled(false);
@@ -289,6 +293,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionView_scans->setEnabled(false);
 	_ui->actionView_high_res_point_cloud->setEnabled(false);
 	_ui->actionReset_Odometry->setEnabled(false);
+	_ui->actionPost_processing->setEnabled(false);
 
 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
 	connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
@@ -1011,6 +1016,8 @@ void MainWindow::updateMapCloud(
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
 			}
 		}
+
+		_ui->actionPost_processing->setEnabled(_currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 	}
 
 	// filter duplicated poses
@@ -1166,7 +1173,7 @@ void MainWindow::updateMapCloud(
 			int fillEmptyRadius = _preferencesDialog->getGridMapFillEmptyRadius();
 			map8S = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, resolution, xMin, yMin, fillEmptyRadius);
 		}
-		else
+		else if(_createdScans.size())
 		{
 			bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
 			map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
@@ -1602,7 +1609,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		UDEBUG("Cloud rendering settings changed...");
 		if(_currentPosesMap.size())
 		{
-			this->updateMapCloud(std::map<int, Transform>(_currentPosesMap), Transform(), std::multimap<int, Link>());
+			this->updateMapCloud(std::map<int, Transform>(_currentPosesMap), Transform(), std::multimap<int, Link>(_currentLinksMap));
 		}
 	}
 
@@ -2542,6 +2549,400 @@ void MainWindow::generateTOROMap()
 	}
 }
 
+void MainWindow::postProcessing()
+{
+	if(_postProcessingDialog->exec() != QDialog::Accepted)
+	{
+		return;
+	}
+
+	bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
+	bool reextractFeatures = _postProcessingDialog->isReextractFeatures();
+	bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
+	bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
+	double clusterRadius = _postProcessingDialog->clusterRadius();
+	double clusterAngle = _postProcessingDialog->clusterAngle();
+	int detectLoopClosureIterations = _postProcessingDialog->iterations();
+
+	if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks)
+	{
+		UWARN("No post-processing selection...");
+		return;
+	}
+
+	// First, verify that we have all data required in the GUI
+	bool allDataAvailable = true;
+	std::map<int, Transform> odomPoses;
+	for(std::map<int, Transform>::iterator iter = _currentPosesMap.begin();
+			iter!=_currentPosesMap.end() && allDataAvailable;
+			++iter)
+	{
+		QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
+		if(jter != _cachedSignatures.end())
+		{
+			if(jter->getPose().isNull())
+			{
+				UWARN("Odometry pose of %d is null.", iter->first);
+				allDataAvailable = false;
+			}
+			else
+			{
+				odomPoses.insert(*iter); // fill raw poses
+			}
+			if(jter->getLocalTransform().isNull())
+			{
+				UWARN("Local transform of %d is null.", iter->first);
+				allDataAvailable = false;
+			}
+			if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
+			{
+				// depth data required
+				if(jter->getDepthCompressed().empty() || jter->getDepthFx() <= 0.0f || jter->getDepthFy() <= 0.0f)
+				{
+					UWARN("Depth data of %d missing.", iter->first);
+					allDataAvailable = false;
+				}
+
+				if(reextractFeatures)
+				{
+					// rgb required
+					if(jter->getImageCompressed().empty())
+					{
+						UWARN("Rgb of %d missing.", iter->first);
+						allDataAvailable = false;
+					}
+				}
+			}
+		}
+		else
+		{
+			UWARN("Node %d missing.", iter->first);
+			allDataAvailable = false;
+		}
+	}
+
+	if(!allDataAvailable)
+	{
+		QMessageBox::warning(this, tr("Not all data available in the GUI..."),
+				tr("Some data missing in the cache to respect the constraints chosen. "
+				   "Try \"Edit->Download all clouds\" to update the cache and try again."));
+		return;
+	}
+
+	_initProgressDialog->setAutoClose(false, 1);
+	_initProgressDialog->resetProgress();
+	_initProgressDialog->clear();
+	_initProgressDialog->show();
+	_initProgressDialog->appendText("Post-processing beginning!");
+
+	int totalSteps = 0;
+	if(refineNeighborLinks)
+	{
+		totalSteps+=odomPoses.size();
+	}
+	if(refineLoopClosureLinks)
+	{
+		totalSteps+=_currentLinksMap.size() - odomPoses.size();
+	}
+	_initProgressDialog->setMaximumSteps(totalSteps);
+	_initProgressDialog->show();
+
+	ParametersMap parameters = _preferencesDialog->getAllParameters();
+	int toroIterations = 100;
+	bool toroOptimizeFromGraphEnd = false;
+	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd);
+
+	int loopClosuresAdded = 0;
+	if(detectMoreLoopClosures)
+	{
+		Memory memory(parameters);
+		if(reextractFeatures)
+		{
+			ParametersMap customParameters;
+			// override some parameters
+			customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+			customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+			customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+			customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+			customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kLccReextractNNType())));
+			customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kLccReextractNNDR())));
+			customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kLccReextractFeatureType())));
+			customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kLccReextractMaxWords())));
+			customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
+			memory.parseParameters(customParameters);
+		}
+
+		UASSERT(detectLoopClosureIterations>0);
+		for(int n=0; n<detectLoopClosureIterations; ++n)
+		{
+			_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
+					.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
+
+			std::multimap<int, int> clusters = util3d::radiusPosesClustering(
+					_currentPosesMap,
+					clusterRadius,
+					clusterAngle*CV_PI/180.0);
+
+			_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+clusters.size());
+			_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
+
+			int i=0;
+			std::set<int> addedLinks;
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
+			{
+				int from = iter->first;
+				int to = iter->second;
+				if(iter->first < iter->second)
+				{
+					from = iter->second;
+					to = iter->first;
+				}
+
+				// only add new links and one per cluster per iteration
+				if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
+				   util3d::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
+				{
+					if(!_cachedSignatures.contains(from))
+					{
+						UERROR("Didn't find signature %d", from);
+					}
+					else if(!_cachedSignatures.contains(to))
+					{
+						UERROR("Didn't find signature %d", to);
+					}
+					else
+					{
+						_initProgressDialog->incrementStep();
+						QApplication::processEvents();
+
+						Signature & signatureFrom = _cachedSignatures[from];
+						Signature & signatureTo = _cachedSignatures[to];
+
+						Transform transform;
+						std::string rejectedMsg;
+						int inliers;
+						if(reextractFeatures)
+						{
+							memory.init("", true); // clear previously added signatures
+
+							// Add signatures
+							SensorData dataFrom = signatureFrom.toSensorData();
+							SensorData dataTo = signatureTo.toSensorData();
+
+							if(dataFrom.isValid() &&
+							   dataFrom.isMetric() &&
+							   dataTo.isValid() &&
+							   dataTo.isMetric() &&
+							   dataFrom.id() != Memory::kIdInvalid &&
+							   signatureFrom.id() != Memory::kIdInvalid)
+							{
+								if(from > to)
+								{
+									memory.update(dataTo);
+									memory.update(dataFrom);
+								}
+								else
+								{
+									memory.update(dataFrom);
+									memory.update(dataTo);
+								}
+
+								transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers);
+							}
+							else
+							{
+								UERROR("not supposed to be here!");
+							}
+						}
+						else
+						{
+							transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers);
+						}
+						if(!transform.isNull())
+						{
+							UINFO("Added new loop closure between %d and %d.", from, to);
+							addedLinks.insert(from);
+							addedLinks.insert(to);
+							_currentLinksMap.insert(std::make_pair(from, Link(from, to, transform, Link::kUserClosure)));
+							++loopClosuresAdded;
+							_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
+						}
+					}
+				}
+			}
+			_initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!")
+					.arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
+			if(addedLinks.size() == 0)
+			{
+				break;
+			}
+
+			if(n+1 < detectLoopClosureIterations)
+			{
+				_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
+						.arg(odomPoses.size()).arg(_currentLinksMap.size()));
+				std::map<int, rtabmap::Transform> optimizedPoses;
+				std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+				util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations);
+				_currentPosesMap = optimizedPoses;
+				_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
+			}
+		}
+		UINFO("Added %d loop closures.", loopClosuresAdded);
+		_initProgressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
+	}
+
+	if(refineNeighborLinks || refineLoopClosureLinks)
+	{
+		if(refineLoopClosureLinks)
+		{
+			_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
+		}
+		_initProgressDialog->appendText(tr("Refining links..."));
+
+		int decimation=8;
+		float maxDepth=2.0f;
+		float voxelSize=0.01f;
+		int samples = 0;
+		float minFitness = 1.0f;
+		float maxCorrespondences = 0.05f;
+		float icpIterations = 30;
+		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::kLccIcp3MaxFitness(), minFitness);
+		Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
+		Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
+		bool pointToPlane = false;
+		int pointToPlaneNormalNeighbors = 20;
+		Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
+		Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
+
+		int i=0;
+		for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
+		{
+			int type = iter->second.type();
+
+			if((refineNeighborLinks && type==Link::kNeighbor) ||
+			   (refineLoopClosureLinks && type!=Link::kNeighbor))
+			{
+				int from = iter->second.from();
+				int to = iter->second.to();
+
+				_initProgressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
+				_initProgressDialog->incrementStep();
+				QApplication::processEvents();
+
+				if(!_cachedSignatures.contains(from))
+				{
+					UERROR("Didn't find signature %d",from);
+				}
+				else if(!_cachedSignatures.contains(to))
+				{
+					UERROR("Didn't find signature %d", to);
+				}
+				else
+				{
+					Signature & signatureFrom = _cachedSignatures[from];
+					Signature & signatureTo = _cachedSignatures[to];
+
+					//3D
+					cv::Mat depthA, depthB;
+					signatureFrom.uncompressData(0, &depthA, 0);
+					signatureTo.uncompressData(0, &depthB, 0);
+
+					if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
+					{
+						QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
+						UERROR("ICP 3D cannot be done on stereo images! Aborting refining links with ICP...");
+						break;
+					}
+
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
+							signatureFrom.getDepthFx(), signatureFrom.getDepthFy(), signatureFrom.getDepthCx(), signatureFrom.getDepthCy(),
+							decimation,
+							maxDepth,
+							voxelSize,
+							samples,
+							signatureFrom.getLocalTransform());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
+							signatureTo.getDepthFx(), signatureTo.getDepthFy(), signatureTo.getDepthCx(), signatureTo.getDepthCy(),
+							decimation,
+							maxDepth,
+							voxelSize,
+							samples,
+							iter->second.transform() * signatureTo.getLocalTransform());
+
+					bool hasConverged = false;
+					double fitness = -1;
+					Transform transform;
+					if(pointToPlane)
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
+
+						cloudANormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudANormals);
+						if(cloudA->size() != cloudANormals->size())
+						{
+							UWARN("removed nan normals...");
+						}
+
+						cloudBNormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudBNormals);
+						if(cloudB->size() != cloudBNormals->size())
+						{
+							UWARN("removed nan normals...");
+						}
+
+						transform = util3d::icpPointToPlane(cloudBNormals,
+								cloudANormals,
+								maxCorrespondences,
+								icpIterations,
+								hasConverged,
+								fitness);
+					}
+					else
+					{
+						transform = util3d::icp(cloudB,
+								cloudA,
+								maxCorrespondences,
+								icpIterations,
+								hasConverged,
+								fitness);
+					}
+
+					if(hasConverged && !transform.isNull() && fitness>=0.0f && fitness <= minFitness)
+					{
+						Link newLink(from, to, transform*iter->second.transform(), iter->second.type());
+						iter->second = newLink;
+					}
+					else
+					{
+						UWARN("Cannot refine link %d->%d (converged=%s fitness=%f)", from, to, hasConverged?"true":"false", fitness);
+					}
+				}
+			}
+		}
+		_initProgressDialog->appendText(tr("Refining links...done!"));
+	}
+
+	_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
+			.arg(odomPoses.size()).arg(_currentLinksMap.size()));
+	std::map<int, rtabmap::Transform> optimizedPoses;
+	std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
+	util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations);
+	_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
+	_initProgressDialog->incrementStep();
+
+	_initProgressDialog->appendText(tr("Updating map..."));
+	this->updateMapCloud(optimizedPoses, Transform(), _currentLinksMap, false);
+	_initProgressDialog->appendText(tr("Updating map... done!"));
+
+	_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
+	_initProgressDialog->appendText("Post-processing finished!");
+}
+
 void MainWindow::deleteMemory()
 {
 	QMessageBox::StandardButton button;
@@ -2778,12 +3179,14 @@ void MainWindow::clearTheCache()
 	_ui->widget_cloudViewer->removeAllClouds();
 	_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
 	_ui->widget_cloudViewer->clearTrajectory();
+	_ui->widget_mapVisibility->clear();
 	_currentPosesMap.clear();
 	_odometryCorrection = Transform::getIdentity();
 	_lastOdomPose.setNull();
 	//disable save cloud action
 	_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
 	_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
+	_ui->actionPost_processing->setEnabled(false);
 	_ui->actionSave_point_cloud->setEnabled(false);
 	_ui->actionView_scans->setEnabled(false);
 	_ui->actionView_high_res_point_cloud->setEnabled(false);

guilib/src/MapVisibilityWidget.cpp

@@ -57,6 +57,13 @@ void MapVisibilityWidget::showEvent(QShowEvent * event)
 	updateCheckBoxes();
 }
 
+void MapVisibilityWidget::clear()
+{
+	_poses.clear();
+	_mask.clear();
+	updateCheckBoxes();
+}
+
 void MapVisibilityWidget::updateCheckBoxes()
 {
 	QWidget * area = this->findChild<QWidget*>("area");

guilib/src/MapVisibilityWidget.h

@@ -43,6 +43,8 @@ public:
 	void setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask);
 	std::map<int, Transform> getVisiblePoses() const;
 
+	void clear();
+
 protected:
 	virtual void showEvent(QShowEvent * event);
 

guilib/src/PostProcessingDialog.cpp

@@ -0,0 +1,92 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "PostProcessingDialog.h"
+#include "ui_postProcessingDialog.h"
+
+#include <QtGui/QPushButton>
+
+namespace rtabmap {
+
+PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
+	QDialog(parent)
+{
+	_ui = new Ui_PostProcessingDialog();
+	_ui->setupUi(this);
+
+	connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
+	connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+	connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
+}
+
+PostProcessingDialog::~PostProcessingDialog()
+{
+	delete _ui;
+}
+
+void PostProcessingDialog::updateButtonBox()
+{
+	_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
+			isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks());
+}
+
+bool PostProcessingDialog::isDetectMoreLoopClosures() const
+{
+	return _ui->detectMoreLoopClosures->isChecked();
+}
+
+double PostProcessingDialog::clusterRadius() const
+{
+	return _ui->clusterRadius->value();
+}
+
+double PostProcessingDialog::clusterAngle() const
+{
+	return _ui->clusterAngle->value();
+}
+
+int PostProcessingDialog::iterations() const
+{
+	return _ui->iterations->value();
+}
+
+bool PostProcessingDialog::isReextractFeatures() const
+{
+	return _ui->reextractFeatures->isChecked();
+}
+
+bool PostProcessingDialog::isRefineNeighborLinks() const
+{
+	return _ui->refineNeighborLinks->isChecked();
+}
+
+bool PostProcessingDialog::isRefineLoopClosureLinks() const
+{
+	return _ui->refineLoopClosureLinks->isChecked();
+}
+
+}

guilib/src/PostProcessingDialog.h

@@ -0,0 +1,63 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef POSTPROCESSINGDIALOG_H_
+#define POSTPROCESSINGDIALOG_H_
+
+#include <QtGui/QDialog>
+
+class Ui_PostProcessingDialog;
+
+namespace rtabmap {
+
+class PostProcessingDialog : public QDialog
+{
+	Q_OBJECT
+
+public:
+	PostProcessingDialog(QWidget * parent = 0);
+
+	virtual ~PostProcessingDialog();
+
+	bool isDetectMoreLoopClosures() const;
+	double clusterRadius() const;
+	double clusterAngle() const;
+	int iterations() const;
+	bool isReextractFeatures() const;
+	bool isRefineNeighborLinks() const;
+	bool isRefineLoopClosureLinks() const;
+
+private slots:
+	void updateButtonBox();
+
+private:
+	Ui_PostProcessingDialog * _ui;
+};
+
+}
+
+#endif /* POSTPROCESSINGDIALOG_H_ */

guilib/src/PreferencesDialog.cpp

@@ -441,6 +441,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp3MaxCorrespondenceDistance().c_str());
 	_ui->loopClosure_icpIterations->setObjectName(Parameters::kLccIcp3Iterations().c_str());
 	_ui->loopClosure_icpMaxFitness->setObjectName(Parameters::kLccIcp3MaxFitness().c_str());
+	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kLccIcp3PointToPlane().c_str());
+	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kLccIcp3PointToPlaneNormalNeighbors().c_str());
 
 	_ui->loopClosure_icp2MaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp2MaxCorrespondenceDistance().c_str());
 	_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());

guilib/src/ui/DatabaseViewer.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1076</width>
-    <height>675</height>
+    <height>711</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -825,7 +825,7 @@
           <number>1000</number>
          </property>
          <property name="value">
-          <number>3</number>
+          <number>10</number>
          </property>
         </widget>
        </item>
@@ -851,7 +851,7 @@
           <double>1.000000000000000</double>
          </property>
          <property name="value">
-          <double>0.010000000000000</double>
+          <double>0.020000000000000</double>
          </property>
         </widget>
        </item>
@@ -948,19 +948,39 @@
        <item row="9" column="1">
         <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
          <property name="suffix">
-          <string> rad</string>
+          <string> degrees</string>
          </property>
          <property name="decimals">
-          <number>2</number>
+          <number>0</number>
          </property>
          <property name="maximum">
-          <double>3.140000000000000</double>
+          <double>180.000000000000000</double>
          </property>
          <property name="singleStep">
-          <double>0.100000000000000</double>
+          <double>1.000000000000000</double>
          </property>
          <property name="value">
-          <double>0.300000000000000</double>
+          <double>30.000000000000000</double>
+         </property>
+        </widget>
+       </item>
+       <item row="10" column="0">
+        <widget class="QLabel" name="label_31">
+         <property name="text">
+          <string>Detect more LCs: iterations</string>
+         </property>
+        </widget>
+       </item>
+       <item row="10" column="1">
+        <widget class="QSpinBox" name="doubleSpinBox_detectMore_iterations">
+         <property name="minimum">
+          <number>1</number>
+         </property>
+         <property name="maximum">
+          <number>100</number>
+         </property>
+         <property name="value">
+          <number>1</number>
          </property>
         </widget>
        </item>

guilib/src/ui/mainWindow.ui

@@ -68,6 +68,8 @@
     <addaction name="separator"/>
     <addaction name="actionData_recorder"/>
     <addaction name="separator"/>
+    <addaction name="actionPost_processing"/>
+    <addaction name="separator"/>
     <addaction name="actionView_high_res_point_cloud"/>
     <addaction name="actionView_scans"/>
    </widget>
@@ -1088,6 +1090,11 @@
     <string>Edit database...</string>
    </property>
   </action>
+  <action name="actionPost_processing">
+   <property name="text">
+    <string>Post-processing...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/postProcessingDialog.ui

@@ -0,0 +1,252 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>PostProcessingDialog</class>
+ <widget class="QDialog" name="PostProcessingDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>486</width>
+    <height>369</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Post-Processing</string>
+  </property>
+  <property name="windowIcon">
+   <iconset resource="../GuiLib.qrc">
+    <normaloff>:/images/RTAB-Map.ico</normaloff>:/images/RTAB-Map.ico</iconset>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout_3">
+   <item>
+    <widget class="QGroupBox" name="detectMoreLoopClosures">
+     <property name="title">
+      <string>Detect more loop closures</string>
+     </property>
+     <property name="checkable">
+      <bool>true</bool>
+     </property>
+     <layout class="QVBoxLayout" name="verticalLayout">
+      <item>
+       <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+        <item row="1" column="0">
+         <widget class="QDoubleSpinBox" name="clusterAngle">
+          <property name="suffix">
+           <string> degrees</string>
+          </property>
+          <property name="decimals">
+           <number>0</number>
+          </property>
+          <property name="minimum">
+           <double>0.000000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>180.000000000000000</double>
+          </property>
+          <property name="value">
+           <double>30.000000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="0">
+         <widget class="QDoubleSpinBox" name="clusterRadius">
+          <property name="suffix">
+           <string> m</string>
+          </property>
+          <property name="minimum">
+           <double>0.010000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>99.000000000000000</double>
+          </property>
+          <property name="singleStep">
+           <double>0.010000000000000</double>
+          </property>
+          <property name="value">
+           <double>0.300000000000000</double>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_3">
+          <property name="text">
+           <string>Cluster radius</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QLabel" name="clusterAngle_label">
+          <property name="text">
+           <string>Cluster angle</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="1">
+         <widget class="QLabel" name="label_5">
+          <property name="text">
+           <string>Re-extract features</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="0">
+         <widget class="QCheckBox" name="reextractFeatures">
+          <property name="text">
+           <string/>
+          </property>
+          <property name="checked">
+           <bool>false</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_6">
+          <property name="text">
+           <string>Iterations</string>
+          </property>
+          <property name="wordWrap">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QSpinBox" name="iterations">
+          <property name="minimum">
+           <number>1</number>
+          </property>
+          <property name="maximum">
+           <number>100</number>
+          </property>
+          <property name="value">
+           <number>1</number>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <widget class="QLabel" name="label">
+        <property name="text">
+         <string>See &quot;RTAB-Map settings/RGB-D SLAM/Loop closure constraint&quot; to tune the parameters used.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_3">
+     <property name="title">
+      <string>Refine links with ICP 3D</string>
+     </property>
+     <property name="checkable">
+      <bool>false</bool>
+     </property>
+     <layout class="QVBoxLayout" name="verticalLayout_2">
+      <item>
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>See &quot;RTAB-Map settings/RGB-D SLAM/ICP&quot; to tune the parameters used.</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QCheckBox" name="refineNeighborLinks">
+        <property name="text">
+         <string>Refine neighbor links</string>
+        </property>
+        <property name="checked">
+         <bool>false</bool>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QCheckBox" name="refineLoopClosureLinks">
+        <property name="text">
+         <string>Refine loop closure links</string>
+        </property>
+        <property name="checked">
+         <bool>false</bool>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources>
+  <include location="../GuiLib.qrc"/>
+ </resources>
+ <connections>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>accepted()</signal>
+   <receiver>PostProcessingDialog</receiver>
+   <slot>accept()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>248</x>
+     <y>254</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>157</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+  <connection>
+   <sender>buttonBox</sender>
+   <signal>rejected()</signal>
+   <receiver>PostProcessingDialog</receiver>
+   <slot>reject()</slot>
+   <hints>
+    <hint type="sourcelabel">
+     <x>316</x>
+     <y>260</y>
+    </hint>
+    <hint type="destinationlabel">
+     <x>286</x>
+     <y>274</y>
+    </hint>
+   </hints>
+  </connection>
+ </connections>
+</ui>

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5642,6 +5642,46 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
+                       <item row="7" column="1">
+                        <widget class="QLabel" name="label_144">
+                         <property name="text">
+                          <string>Point to plane ICP.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="0">
+                        <widget class="QCheckBox" name="loopClosure_icpPointToPlane">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QSpinBox" name="loopClosure_icpPointToPlaneNormals">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>1000</number>
+                         </property>
+                         <property name="value">
+                          <number>20</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="1">
+                        <widget class="QLabel" name="label_212">
+                         <property name="text">
+                          <string>Number of neighbors to compute normals for point to plane.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>