Removed parameter "LccIcp/HighTransitionalVariance" (Identity covariance is set directly on pose correction and local loop closure detection in space)

Covariance of virtual links added on the path is set to Identity. For those added to keep the path linked to current map, their covariance is set to 100.
DatabaseViewer: set fixed colors on constraints view

corelib/include/rtabmap/core/Parameters.h

@@ -332,7 +332,6 @@ class RTABMAP_EXP Parameters
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
 	RTABMAP_PARAM(LccIcp, MaxDistance,     float, 0.2,     "Maximum ICP correction distance accepted (m).");
-	RTABMAP_PARAM(LccIcp, HighTransitionalVariance,  bool, true, 	"Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder.");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
@@ -358,7 +357,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
 	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.05, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity
 	RTABMAP_PARAM(Stereo, WinSize,               int, 16,        "See cv::calcOpticalFlowPyrLK().");

corelib/include/rtabmap/core/Rtabmap.h

@@ -183,7 +183,6 @@ private:
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _planWithNearNodesLinked;
-	bool _icpHighTransVariance;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/src/Rtabmap.cpp

@@ -58,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LOG_I "LogI.txt"
 
 #define GRAPH_FILE_NAME "Graph.dot"
-#define HIGH_VARIANCE 10000
+
 
 //
 //
@@ -111,7 +111,6 @@ Rtabmap::Rtabmap() :
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
-	_icpHighTransVariance(Parameters::defaultLccIcpHighTransitionalVariance()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -378,7 +377,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
-	Parameters::parse(parameters, Parameters::kLccIcpHighTransitionalVariance(), _icpHighTransVariance);
 
 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -873,7 +871,7 @@ bool Rtabmap::process(const SensorData & data)
 						oldId,
 						signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 						t.prettyPrint().c_str());
-				_memory->updateLink(signature->id(), oldId, t, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
+				_memory->updateLink(signature->id(), oldId, t, 1, 1); // set Identify covariance
 			}
 			else
 			{
@@ -1486,7 +1484,7 @@ bool Rtabmap::process(const SensorData & data)
 							signature->id(),
 							localSpaceNearestId,
 							t.prettyPrint().c_str());
-					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
+					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, 1, 1); // set Identify covariance
 
 					// Old map -> new map, used for localization correction on loop closure
 					const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1568,7 +1566,7 @@ bool Rtabmap::process(const SensorData & data)
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
 			if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
 			{
-				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
+				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100); // set high variance
 			}
 		}
 	}
@@ -2608,7 +2606,7 @@ void Rtabmap::updateGoalIndex()
 						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 						{
 							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
+							_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1); // on the optimized path, set Identity variance
 							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 						}
 					}

corelib/src/util3d.cpp

@@ -589,6 +589,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		int decimation)
 {
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
+	UASSERT(imageDepth.rows % decimation == 0);
+	UASSERT(imageDepth.cols % decimation == 0);
+
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(decimation < 1)
 	{
@@ -630,6 +633,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 {
 	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
+	UASSERT(imageDepth.rows % decimation == 0);
+	UASSERT(imageDepth.cols % decimation == 0);
+
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
 	{
@@ -691,6 +697,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 		int decimation)
 {
 	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
+	UASSERT(imageDisparity.rows % decimation == 0);
+	UASSERT(imageDisparity.cols % decimation == 0);
+
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(decimation < 1)
 	{
@@ -738,6 +747,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 	UASSERT(imageRgb.rows == imageDisparity.rows &&
 			imageRgb.cols == imageDisparity.cols &&
 			(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
+	UASSERT(imageDisparity.rows % decimation == 0);
+	UASSERT(imageDisparity.cols % decimation == 0);
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
 	{

guilib/include/rtabmap/gui/CloudViewer.h

@@ -70,12 +70,14 @@ public:
 	bool updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = Qt::gray);
 
 	bool updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = Qt::gray);
 
 	bool addOrUpdateCloud(
 		const std::string & id,

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -108,10 +108,13 @@ private:
 				bool updateConstraintView = true);
 	void updateStereo(const Signature * data);
 	void updateWordsMatching();
-	void updateConstraintView(const rtabmap::Link & link,
+	void updateConstraintView(
+			const rtabmap::Link & link,
+			bool updateImageSliders = true,
 			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>),
-			bool updateImageSliders = true);
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
 	void updateConstraintButtons();
 	Link findActiveLink(int from, int to);
 	bool containsLink(

guilib/src/CloudViewer.cpp

@@ -179,14 +179,15 @@ bool CloudViewer::updateCloudPose(
 bool CloudViewer::updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose)
+		const Transform & pose,
+		const QColor & color)
 {
 	if(_addedClouds.contains(id))
 	{
 		UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
 		int index = _visualizer->getColorHandlerIndex(id);
 		this->removeCloud(id);
-		if(this->addCloud(id, cloud, pose))
+		if(this->addCloud(id, cloud, pose, color))
 		{
 			_visualizer->updateColorHandlerIndex(id, index);
 			return true;
@@ -198,14 +199,15 @@ bool CloudViewer::updateCloud(
 bool CloudViewer::updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose)
+		const Transform & pose,
+		const QColor & color)
 {
 	if(_addedClouds.contains(id))
 	{
 		UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
 		int index = _visualizer->getColorHandlerIndex(id);
 		this->removeCloud(id);
-		if(this->addCloud(id, cloud, pose))
+		if(this->addCloud(id, cloud, pose, color))
 		{
 			_visualizer->updateColorHandlerIndex(id, index);
 			return true;
@@ -220,7 +222,7 @@ bool CloudViewer::addOrUpdateCloud(
 		const Transform & pose,
 		const QColor & color)
 {
-	if(!updateCloud(id, cloud, pose))
+	if(!updateCloud(id, cloud, pose, color))
 	{
 		return addCloud(id, cloud, pose, color);
 	}
@@ -233,7 +235,7 @@ bool CloudViewer::addOrUpdateCloud(
 		const Transform & pose,
 		const QColor & color)
 {
-	if(!updateCloud(id, cloud, pose))
+	if(!updateCloud(id, cloud, pose, color))
 	{
 		return addCloud(id, cloud, pose, color);
 	}

guilib/src/DatabaseViewer.cpp

@@ -700,6 +700,7 @@ void DatabaseViewer::view3DMap()
 						{
 							color = (Qt::GlobalColor)(mapId % 12 + 7 );
 						}
+
 						viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
 
 						UINFO("Generated %d (%d points)", iter->first, cloud->size());
@@ -1109,10 +1110,7 @@ void DatabaseViewer::update(int value,
 						ui_->horizontalSlider_loops->blockSignals(true);
 						ui_->horizontalSlider_loops->setValue(i);
 						ui_->horizontalSlider_loops->blockSignals(false);
-						this->updateConstraintView(loopLinks_.at(i),
+						this->updateConstraintView(loopLinks_.at(i), false);
-								pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-								pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-								false);
 					}
 					ui_->horizontalSlider_neighbors->blockSignals(true);
 					ui_->horizontalSlider_neighbors->setValue(0);
@@ -1131,10 +1129,7 @@ void DatabaseViewer::update(int value,
 						ui_->horizontalSlider_neighbors->blockSignals(true);
 						ui_->horizontalSlider_neighbors->setValue(i);
 						ui_->horizontalSlider_neighbors->blockSignals(false);
-						this->updateConstraintView(neighborLinks_.at(i),
+						this->updateConstraintView(neighborLinks_.at(i), false);
-								pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-								pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-								false);
 					}
 					ui_->horizontalSlider_loops->blockSignals(true);
 					ui_->horizontalSlider_loops->setValue(0);
@@ -1424,16 +1419,16 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
 // only called when ui_->checkBox_showOptimized state changed
 void DatabaseViewer::updateConstraintView()
 {
-	this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()),
+	this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-			pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-			false);
 }
 
-void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
+void DatabaseViewer::updateConstraintView(
+		const rtabmap::Link & linkIn,
+		bool updateImageSliders,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
-		bool updateImageSliders)
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo)
 {
 	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
@@ -1448,7 +1443,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 	ui_->checkBox_showOptimized->setEnabled(false);
 	UASSERT(!t.isNull() && memory_);
 
-	ui_->label_constraint->setText(QString("%1 (%2r=%3 %4t=%5)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.rotVariance())).arg(QChar(0xc3, 0x03)).arg(sqrt(link.transVariance())));
+	ui_->label_type->setNum(link.type());
+	ui_->label_variance->setText(QString("%1, %2").arg(sqrt(link.rotVariance())).arg(sqrt(link.transVariance())));
+	ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()));
 	if(link.type() == Link::kNeighbor &&
 	   graphes_.size() &&
 	   (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
@@ -1462,12 +1459,12 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 			{
 				ui_->checkBox_showOptimized->setEnabled(true);
 				Transform topt = iterFrom->second.inverse()*iterTo->second;
-				Transform delta = t.inverse()*topt;
+				float diff = topt.getDistance(t);
 				Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
 				Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
 				float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
 				a = (a *180.0f) / CV_PI;
-				ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((delta.getNorm()/t.getNorm())*100.0f).arg(a));
+				ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((diff/t.getNorm())*100.0f).arg(a));
 
 				if(ui_->checkBox_showOptimized->isChecked())
 				{
@@ -1520,19 +1517,19 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 
 	if(ui_->constraintsViewer->isVisible())
 	{
+		Signature dataFrom, dataTo;
+
+		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);
+
+
 		if(cloudFrom->size() == 0 && cloudTo->size() == 0)
 		{
-			Signature dataFrom, dataTo;
-
-			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
 			if(!ui_->checkBox_show3DWords->isChecked())
 			{
@@ -1584,11 +1581,11 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 
 				if(cloudFrom->size())
 				{
-					ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
+					ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
 				}
 				if(cloudTo->size())
 				{
-					ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+					ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
 				}
 			}
 			else
@@ -1628,7 +1625,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 
 					if(cloudFrom->size())
 					{
-						ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
+						ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
 					}
 					else
 					{
@@ -1636,7 +1633,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 					}
 					if(cloudTo->size())
 					{
-						ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+						ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
 					}
 					else
 					{
@@ -1648,31 +1645,44 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 					UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
 				}
 			}
-
-			//cloud 2d
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-			scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
-			scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
-			scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
-
-			if(scanA->size())
-			{
-				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
-			}
-			if(scanB->size())
-			{
-				ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
-			}
 		}
 		else
 		{
 			if(cloudFrom->size())
 			{
-				ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
+				ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
 			}
 			if(cloudTo->size())
 			{
-				ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
+				ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
+			}
+		}
+
+		if(scanFrom->size() == 0 && scanTo->size() == 0)
+		{
+			//cloud 2d
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
+			scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
+			scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
+			if(scanA->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
+			}
+			if(scanB->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
+			}
+		}
+		else
+		{
+			if(scanFrom->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
+			}
+			if(scanTo->size())
+			{
+				ui_->constraintsViewer->addOrUpdateCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
 			}
 		}
 
@@ -1916,6 +1926,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
@@ -1925,20 +1937,20 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 		if(!oldLaserScan.empty() && !newLaserScan.empty())
 		{
 			// 2D
-			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldLaserScan);
+			scanA = util3d::cvMat2Cloud(oldLaserScan);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newLaserScan, t);
+			scanB = util3d::cvMat2Cloud(newLaserScan, t);
 
 			//voxelize
 			if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
 			{
-				oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
+				scanA = util3d::voxelize<pcl::PointXYZ>(scanA, ui_->doubleSpinBox_icp_voxel->value());
-				newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, ui_->doubleSpinBox_icp_voxel->value());
+				scanB = util3d::voxelize<pcl::PointXYZ>(scanB, ui_->doubleSpinBox_icp_voxel->value());
 			}
 
-			if(newCloud->size() && oldCloud->size())
+			if(scanB->size() && scanA->size())
 			{
-				transform = util3d::icp2D(newCloud,
+				transform = util3d::icp2D(scanB,
-						oldCloud,
+						scanA,
 						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 						ui_->spinBox_icp_iteration->value(),
 					   &hasConverged,
@@ -2087,7 +2099,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 		if(ui_->dockWidget_constraints->isVisible())
 		{
 			cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
-			this->updateConstraintView(newLink, cloudA, cloudB);
+			scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, transform);
+			this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
 		}
 	}
 }

guilib/src/PreferencesDialog.cpp

@@ -495,7 +495,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
 	_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
 	_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
-	_ui->loopClosure_highICPTransitionalVariance->setObjectName(Parameters::kLccIcpHighTransitionalVariance().c_str());
 
 
 	//Odometry

guilib/src/ui/DatabaseViewer.ui

@@ -40,7 +40,7 @@
            <property name="geometry">
             <rect>
              <x>0</x>
-             <y>0</y>
+             <y>-16</y>
              <width>344</width>
              <height>81</height>
             </rect>
@@ -56,7 +56,7 @@
             <item row="0" column="1">
              <widget class="QLabel" name="label_parentsA">
               <property name="text">
-               <string>Parents</string>
+               <string/>
               </property>
              </widget>
             </item>
@@ -70,7 +70,21 @@
             <item row="1" column="1">
              <widget class="QLabel" name="label_childrenA">
               <property name="text">
-               <string>Children</string>
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QLabel" name="label_childrenA_4">
+              <property name="text">
+               <string>Label</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="1">
+             <widget class="QLabel" name="label_labelA">
+              <property name="text">
+               <string/>
               </property>
              </widget>
             </item>
@@ -150,7 +164,7 @@
            <property name="geometry">
             <rect>
              <x>0</x>
-             <y>0</y>
+             <y>-16</y>
              <width>344</width>
              <height>81</height>
             </rect>
@@ -166,7 +180,7 @@
             <item row="0" column="1">
              <widget class="QLabel" name="label_parentsB">
               <property name="text">
-               <string>Parents</string>
+               <string/>
               </property>
              </widget>
             </item>
@@ -180,7 +194,21 @@
             <item row="1" column="1">
              <widget class="QLabel" name="label_childrenB">
               <property name="text">
-               <string>Children</string>
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QLabel" name="label_childrenA_5">
+              <property name="text">
+               <string>Label</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="1">
+             <widget class="QLabel" name="label_labelB">
+              <property name="text">
+               <string/>
               </property>
              </widget>
             </item>
@@ -354,48 +382,76 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
+       <item row="3" column="0">
         <widget class="QLabel" name="label_16">
          <property name="text">
           <string>Transform</string>
          </property>
         </widget>
        </item>
-       <item row="2" column="1">
+       <item row="3" column="1">
         <widget class="QLabel" name="label_constraint">
          <property name="text">
           <string/>
          </property>
         </widget>
        </item>
-       <item row="3" column="0">
+       <item row="5" column="0">
         <widget class="QCheckBox" name="checkBox_showOptimized">
          <property name="text">
           <string>Optimized</string>
          </property>
         </widget>
        </item>
-       <item row="3" column="1">
+       <item row="5" column="1">
         <widget class="QLabel" name="label_constraint_opt">
          <property name="text">
           <string/>
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
+       <item row="6" column="0">
         <widget class="QLabel" name="label_32">
          <property name="text">
           <string>Show 3D words</string>
          </property>
         </widget>
        </item>
-       <item row="4" column="1">
+       <item row="6" column="1">
         <widget class="QCheckBox" name="checkBox_show3DWords">
          <property name="text">
           <string/>
          </property>
         </widget>
        </item>
+       <item row="4" column="0">
+        <widget class="QLabel" name="label_18">
+         <property name="text">
+          <string>σ (rot, trans)</string>
+         </property>
+        </widget>
+       </item>
+       <item row="4" column="1">
+        <widget class="QLabel" name="label_variance">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="0">
+        <widget class="QLabel" name="label_33">
+         <property name="text">
+          <string>Type</string>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="1">
+        <widget class="QLabel" name="label_type">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-203</y>
             <width>744</width>
             <height>1101</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>20</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5662,13 +5662,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
-                      <item row="1" column="0">
-                       <widget class="QCheckBox" name="loopClosure_highICPTransitionalVariance">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="0">
                        <widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
                         <property name="suffix">
@@ -5698,16 +5691,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_80">
-                        <property name="text">
-                         <string>Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder. Only used with ICP 2D.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                      </layout>
                     </item>
                     <item>