RegIcp: complexity, checking if second eigen value is also under Icp/PointToPlaneMinComplexity to limit to only one axis. DbViewer: fixed refine link with scans having RGB channel.

corelib/src/RegistrationIcp.cpp

@@ -591,6 +591,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 			double variance = 1.0;
 			bool transformComputed = false;
 			bool tooLowComplexityForPlaneToPlane = false;
+			float secondEigenValue = 1.0f;
 			cv::Mat complexityVectors;
 
 			if( _pointToPlane &&
@@ -614,13 +615,17 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 					UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
 							(complexityVectors.rows == 3 && complexityVectors.cols == 3));
-					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
+					secondEigenValue = complexityValuesFrom.at<float>(1,0)<complexityValuesTo.at<float>(1,0)?complexityValuesFrom.at<float>(1,0):complexityValuesTo.at<float>(1,0);
+					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). Second eigen value=%f. "
 						  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
 						  "direction of normals (%s: %s).",
 						  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
+						  secondEigenValue,
 						  fromComplexity<toComplexity?"From":"To",
 						  complexityVectors.rows==2?
 								  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
+								  secondEigenValue<_pointToPlaneMinComplexity?
+								  uFormat("n=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2)).c_str():
 								  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
 
 					if(ULogger::level() == ULogger::kDebug)
@@ -1140,7 +1145,10 @@ Transform RegistrationIcp::computeTransformationImpl(
 								float a = v.dot(n1);
 								float b = v.dot(n2);
 								Eigen::Vector3f vp = n1*a;
-								vp += n2*b;
+								if(secondEigenValue >= _pointToPlaneMinComplexity)
+								{
+									vp += n2*b;
+								}
 								UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
 										v[0], v[1], v[2], vp[0], vp[1], vp[2]);
 								v = vp;

guilib/src/DatabaseViewer.cpp

@@ -7007,7 +7007,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			}
 		}
 	}
-	if(scanPoses.size())
+	if(scanPoses.size()>1)
 	{
 		//optimize the path's poses locally
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
@@ -7065,6 +7065,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
 		pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
 		{
 			if(iter->first != currentLink.from())
@@ -7090,6 +7092,19 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 										toPoseInv * iter->second * scan.localTransform());
 							}
 						}
+						else if(scan.hasRGB())
+						{
+							if(scan.hasNormals())
+							{
+								*assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+							else
+							{
+								*assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+						}
 						else
 						{
 							if(scan.hasNormals())
@@ -7138,6 +7153,16 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
 		}
+		else if(assembledToNormalRGBClouds->size())
+		{
+			UASSERT(!fromScan.is2d());
+			assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
+		}
+		else if(assembledToRGBClouds->size())
+		{
+			UASSERT(!fromScan.is2d());
+			assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
+		}
 		else
 		{
 			UWARN("Assembled scan is empty!");
@@ -7787,16 +7812,7 @@ void DatabaseViewer::resetConstraint()
 		this->updateGraphView();
 	}
 
-	iter = rtabmap::graph::findLink(links_, from, to);
+	updateConstraintView();
-	if(iter != links_.end())
-	{
-		this->updateConstraintView(iter->second);
-	}
-	iter = rtabmap::graph::findLink(linksAdded_, from, to);
-	if(iter != linksAdded_.end())
-	{
-		this->updateConstraintView(iter->second);
-	}
 }
 
 void DatabaseViewer::rejectConstraint()