merged multicamera branch into devel branch

2026-09-02 17:40:23 +08:00 · 2015-06-16 17:41:51 -04:00
parent 6a7a9fb9b0 039ae45d57
commit 50eeda5b95
61 changed files with 3383 additions and 2918 deletions
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -86,7 +86,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_mapping.h"
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_registration.h"
@@ -438,9 +437,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
 	connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));

-	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
-	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
-	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
+	qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
+	connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent)), this, SLOT(processOdometry(rtabmap::OdometryEvent)));

 	connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));

@@ -672,12 +670,13 @@ void MainWindow::handleEvent(UEvent* anEvent)
 			if(!_processingOdometry && !_processingStatistics)
 			{
 				_processingOdometry = true; // if we receive too many odometry events!
-				emit odometryReceived(odomEvent->data(), odomEvent->info());
+				emit odometryReceived(*odomEvent);
 			}
 			else
 			{
 				// we receive too many odometry events! just send without data
-				emit odometryReceived(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->info());
+				OdometryEvent tmp(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
+				emit odometryReceived(tmp);
 			}
 		}
 	}
@@ -701,14 +700,14 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 }

-void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
+void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 {
 	_processingOdometry = true;
 	UTimer time;
 	// Process Data
-	if(data.isValid())
+	if(!odom.data().imageRaw().empty())
 	{
-		Transform pose = data.pose();
+		Transform pose = odom.pose();
 		bool lost = false;
 		bool lostStateChanged = false;

@@ -722,11 +721,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			pose = _lastOdomPose;
 			lost = true;
 		}
-		else if(info.inliers>0 &&
+		else if(odom.info().inliers>0 &&
 				_preferencesDialog->getOdomQualityWarnThr() &&
-				info.inliers < _preferencesDialog->getOdomQualityWarnThr())
+				odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
 		{
-			UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
+			UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
 			lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
 			_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
 			_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -750,84 +749,96 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			if(!pose.isNull())
 			{
 				// 3d cloud
-				if(data.depthOrRightImage().cols == data.image().cols &&
-				   data.depthOrRightImage().rows == data.image().rows &&
-				   !data.depthOrRightImage().empty() &&
-				   data.fx() > 0.0f &&
-				   data.fyOrBaseline() > 0.0f &&
+				if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
+				   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
+				   !odom.data().depthOrRightRaw().empty() &&
+				   (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValid()) &&
 				   _preferencesDialog->isCloudsShown(1))
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					cloud = createCloud(0,
-							data.image(),
-							data.depthOrRightImage(),
-							data.fx(),
-							data.fyOrBaseline(),
-							data.cx(),
-							data.cy(),
-							data.localTransform(),
-							pose,
-							_preferencesDialog->getCloudVoxelSize(1),
+					cloud = util3d::cloudRGBFromSensorData(odom.data(),
 							_preferencesDialog->getCloudDecimation(1),
-							_preferencesDialog->getCloudMaxDepth(1));
-
-					if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
+							_preferencesDialog->getCloudMaxDepth(1),
+							_preferencesDialog->getCloudVoxelSize(1));
+					if(cloud->size())
 					{
-						UERROR("Adding cloudOdom to viewer failed!");
+						cloud = util3d::transformPointCloud(cloud, pose);
+
+						if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
+						{
+							UERROR("Adding cloudOdom to viewer failed!");
+						}
+						_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
+						_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
+						_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
+					}
+					else
+					{
+						UWARN("Empty cloudOdom!");
+						_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false);
 					}
-					_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
-					_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
-					_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
 				}

 				// 2d cloud
-				if(!data.laserScan().empty() &&
+				if(!odom.data().laserScanRaw().empty() &&
 					_preferencesDialog->isScansShown(1))
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cloud = util3d::laserScanToPointCloud(data.laserScan());
+					cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
 					cloud = util3d::transformPointCloud(cloud, pose);
 					if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
 					{
-						UERROR("Adding scanOdom to viewer failed!");
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
+						cloud = util3d::transformPointCloud(cloud, pose);
+						if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
+						{
+							UERROR("Adding scanOdom to viewer failed!");
+						}
+						_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
+						_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
+						_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 					}
-					_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
-					_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
-					_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
-				}
-
-				if(!data.pose().isNull())
-				{
-					// update camera position
-					_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
 				}
 			}
-			_ui->widget_cloudViewer->update();
 		}

+		if(!odom.pose().isNull())
+		{
+			// update camera position
+			_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
+		}
+		_ui->widget_cloudViewer->update();
+
 		if(_ui->graphicsView_graphView->isVisible())
 		{
-			if(!pose.isNull() && !data.pose().isNull())
+			if(!pose.isNull() && !odom.pose().isNull())
 			{
-				_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
+				_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
 				_ui->graphicsView_graphView->update();
 			}
 		}

 		if(_ui->dockWidget_odometry->isVisible() &&
-		   !data.image().empty())
+		   !odom.data().imageRaw().empty())
 		{
 			if(_ui->imageView_odometry->isFeaturesShown())
 			{
-				if(info.type == 0)
+				if(odom.info().type == 0)
 				{
-					_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
+					_ui->imageView_odometry->setFeatures(
+							odom.info().words,
+							odom.data().depthRaw(),
+							Qt::yellow);
 				}
-				else if(info.type == 1)
+				else if(odom.info().type == 1)
 				{
 					std::vector<cv::KeyPoint> kpts;
-					cv::KeyPoint::convert(info.refCorners, kpts);
-					_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
+					cv::KeyPoint::convert(odom.info().refCorners, kpts);
+					_ui->imageView_odometry->setFeatures(
+							kpts,
+							odom.data().depthRaw(),
+							Qt::red);
 				}
 			}

@@ -840,7 +851,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 					_odomImageShow = _ui->imageView_odometry->isImageShown();
 					_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
 				}
-				_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
+				_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
 				_ui->imageView_odometry->setImageShown(true);
 				_ui->imageView_odometry->setImageDepthShown(true);
 			}
@@ -853,55 +864,55 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 					_ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
 				}

-				_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
+				_ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
 				if(_ui->imageView_odometry->isImageDepthShown())
 				{
-					_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+					_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
 				}

-				if(info.type == 0)
+				if(odom.info().type == 0)
 				{
 					if(_ui->imageView_odometry->isFeaturesShown())
 					{
-						for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+						for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
 						{
-							_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
+							_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
 						}
-						for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+						for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
 						{
-							_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
+							_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
 						}
 					}
 				}
-			}
-			if(info.type == 1 && info.cornerInliers.size())
-			{
-				if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
+				if(odom.info().type == 1 && odom.info().refCorners.size())
 				{
-					//draw lines
-					UASSERT(info.refCorners.size() == info.newCorners.size());
-					std::set<int> inliers(info.cornerInliers.begin(), info.cornerInliers.end());
-					for(unsigned int i=0; i<info.refCorners.size(); ++i)
+					if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
 					{
-						if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
+						//draw lines
+						UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
+						std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
+						for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
 						{
-							_ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
-						}
-						if(_ui->imageView_odometry->isLinesShown())
-						{
-							_ui->imageView_odometry->addLine(
-									info.refCorners[i].x,
-									info.refCorners[i].y,
-									info.newCorners[i].x,
-									info.newCorners[i].y,
-									inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
+							if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
+							{
+								_ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
+							}
+							if(_ui->imageView_odometry->isLinesShown())
+							{
+								_ui->imageView_odometry->addLine(
+										odom.info().refCorners[i].x,
+										odom.info().refCorners[i].y,
+										odom.info().newCorners[i].x,
+										odom.info().newCorners[i].y,
+										inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
+							}
 						}
 					}
 				}
 			}
-			if(!data.image().empty())
+			if(!odom.data().imageRaw().empty())
 			{
-				_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
+				_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
 			}

 			_ui->imageView_odometry->update();
@@ -914,74 +925,74 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 	}

 	//Process info
-	if(info.inliers >= 0)
+	if(odom.info().inliers >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
+		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
 	}
-	if(info.matches >= 0)
+	if(odom.info().matches >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches);
+		_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
 	}
-	if(info.variance >= 0)
+	if(odom.info().variance >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance));
+		_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance));
 	}
-	if(info.variance >= 0)
+	if(odom.info().variance >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
+		_ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance);
 	}
-	if(info.timeEstimation > 0)
+	if(odom.info().timeEstimation > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)data.id(), (float)info.timeEstimation*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
 	}
-	if(info.timeParticleFiltering > 0)
+	if(odom.info().timeParticleFiltering > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)data.id(), (float)info.timeParticleFiltering*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
 	}
-	if(info.features >=0)
+	if(odom.info().features >=0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features);
+		_ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features);
 	}
-	if(info.localMapSize >=0)
+	if(odom.info().localMapSize >=0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
+		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)odom.data().id(), (float)odom.info().localMapSize);
 	}
-	_ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
+	_ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id());

-	float x,y,z, roll,pitch,yaw;
-	if(!info.transform.isNull())
+	float x=0.0f,y,z, roll,pitch,yaw;
+	if(!odom.info().transform.isNull())
 	{
-		info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-		_ui->statsToolBox->updateStat("Odometry/Tx/m", (float)data.id(), x);
-		_ui->statsToolBox->updateStat("Odometry/Ty/m", (float)data.id(), y);
-		_ui->statsToolBox->updateStat("Odometry/Tz/m", (float)data.id(), z);
-		_ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)data.id(), roll*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)data.id(), pitch*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)data.id(), yaw*180.0/CV_PI);
+		odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		_ui->statsToolBox->updateStat("Odometry/Tx/m", (float)odom.data().id(), x);
+		_ui->statsToolBox->updateStat("Odometry/Ty/m", (float)odom.data().id(), y);
+		_ui->statsToolBox->updateStat("Odometry/Tz/m", (float)odom.data().id(), z);
+		_ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
 	}

-	if(!info.transformFiltered.isNull())
+	if(!odom.info().transformFiltered.isNull())
 	{
-		info.transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-		_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)data.id(), x);
-		_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)data.id(), y);
-		_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)data.id(), z);
-		_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)data.id(), roll*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)data.id(), pitch*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)data.id(), yaw*180.0/CV_PI);
+		odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)odom.data().id(), x);
+		_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)odom.data().id(), y);
+		_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)odom.data().id(), z);
+		_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
 	}

-	if(info.interval > 0)
+	if(odom.info().interval > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)data.id(), info.interval*1000.f);
-		_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f);
+		_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)odom.data().id(), odom.info().interval*1000.f);
+		_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)odom.data().id(), x/odom.info().interval*3.6f);
 	}
-	if(info.distanceTravelled > 0)
+	if(odom.info().distanceTravelled > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)data.id(), info.distanceTravelled);
+		_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)odom.data().id(), odom.info().distanceTravelled);
 	}

-	_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
+	_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)odom.data().id(), time.elapsed()*1000.0);
 	_processingOdometry = false;
 }

@@ -994,8 +1005,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	totalTime.start();
 	//Affichage des stats et images

-	int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1);
-	int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1));
+	int refMapId = -1, loopMapId = -1;
+	if(uContains(stat.getSignatures(), stat.refImageId()))
+	{
+		refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
+	}
+	if(uContains(stat.getSignatures(), stat.loopClosureId()))
+	{
+		loopMapId = stat.getSignatures().at(stat.loopClosureId()).mapId();
+	}

 	_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));

@@ -1012,12 +1030,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);

 		// update cache
-		Signature signature = stat.getSignature();
-		signature.uncompressData(); // make sure data are uncompressed
-		_cachedSignatures.insert(stat.getSignature().id(), signature);
+		Signature signature;
+		if(uContains(stat.getSignatures(), stat.refImageId()))
+		{
+			signature = stat.getSignatures().at(stat.refImageId());
+			signature.sensorData().uncompressData(); // make sure data are uncompressed
+			_cachedSignatures.insert(signature.id(), signature);
+		}

 		// For intermediate empty nodes, keep latest image shown
-		if(!signature.getImageRaw().empty() || signature.getWords().size())
+		if(!signature.sensorData().imageRaw().empty() || signature.getWords().size())
 		{
 			_ui->imageView_source->clear();
 			_ui->imageView_loopClosure->clear();
@@ -1098,7 +1120,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
 				if(iter != _cachedSignatures.end())
 				{
-					iter.value().uncompressData();
+					iter.value().sensorData().uncompressData();
 					loopSignature = iter.value();
 				}
 			}
@@ -1108,10 +1130,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)

 		//update image views
 		{
-			UCvMat2QImageThread qimageThread(signature.getImageRaw());
-			UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw());
-			UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
-			UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw());
+			UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
+			UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
+			UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
+			UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
 			qimageThread.start();
 			qdepthThread.start();
 			qimageLoopThread.start();
@@ -1198,10 +1220,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		if(stat.poses().size())
 		{
 			// update pose only if odometry is not received
+			std::map<int, int> mapIds;
+			for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
+			}
 			updateMapCloud(stat.poses(),
 					_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
 					stat.constraints(),
-					stat.getMapIds());
+					mapIds);

 			_odometryReceived = false;

@@ -1213,7 +1240,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			// loop closure view
 			if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0)  &&
 			   !stat.loopClosureTransform().isNull() &&
-			   !loopSignature.getImageRaw().empty())
+			   !loopSignature.sensorData().imageRaw().empty())
 			{
 				// the last loop closure data
 				Transform loopClosureTransform = stat.loopClosureTransform();
@@ -1294,7 +1321,7 @@ void MainWindow::updateMapCloud(
 		{
 			if(!_ui->actionSave_point_cloud->isEnabled() &&
 				_cachedSignatures.size() &&
-				(!(--_cachedSignatures.end())->getDepthCompressed().empty() ||
+				(!(--_cachedSignatures.end())->sensorData().depthOrRightCompressed().empty() ||
 				 !(--_cachedSignatures.end())->getWords3().empty()))
 			{
 				//enable save cloud action
@@ -1304,7 +1331,7 @@ void MainWindow::updateMapCloud(

 			if(!_ui->actionView_scans->isEnabled() &&
 				_cachedSignatures.size() &&
-				!(--_cachedSignatures.end())->getLaserScanCompressed().empty())
+				!(--_cachedSignatures.end())->sensorData().laserScanCompressed().empty())
 			{
 				_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1391,7 +1418,7 @@ void MainWindow::updateMapCloud(
 				else if(_cachedSignatures.contains(iter->first))
 				{
 					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
-					if((!jter->getImageCompressed().empty() && !jter->getDepthCompressed().empty()) || jter->getWords3().size())
+					if((!jter->sensorData().imageCompressed().empty() && !jter->sensorData().depthOrRightCompressed().empty()) || jter->getWords3().size())
 					{
 						this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
 					}
@@ -1427,7 +1454,7 @@ void MainWindow::updateMapCloud(
 				else if(_cachedSignatures.contains(iter->first))
 				{
 					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
-					if(!jter->getLaserScanCompressed().empty())
+					if(!jter->sensorData().laserScanCompressed().empty())
 					{
 						this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
 					}
@@ -1605,25 +1632,19 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		return;
 	}

-	if(!iter->getImageCompressed().empty() && !iter->getDepthCompressed().empty())
+	if(!iter->sensorData().imageCompressed().empty() && !iter->sensorData().depthOrRightCompressed().empty())
 	{

 		cv::Mat image, depth;
-		iter->uncompressData(&image, &depth, 0);
+		SensorData data = iter->sensorData();
+		data.uncompressData(&image, &depth, 0);

 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		cloud = createCloud(nodeId,
-				image,
-				depth,
-				iter->getFx(),
-				iter->getFy(),
-				iter->getCx(),
-				iter->getCy(),
-				iter->getLocalTransform(),
-				Transform::getIdentity(),
-				_preferencesDialog->getCloudVoxelSize(0),
+		UASSERT(nodeId == data.id());
+		cloud = util3d::cloudRGBFromSensorData(data,
 				_preferencesDialog->getCloudDecimation(0),
-				_preferencesDialog->getCloudMaxDepth(0));
+				_preferencesDialog->getCloudMaxDepth(0),
+				_preferencesDialog->getCloudVoxelSize(0));

 		if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
 		{
@@ -1758,10 +1779,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		return;
 	}

-	if(!iter->getLaserScanCompressed().empty())
+	if(!iter->sensorData().laserScanCompressed().empty())
 	{
 		cv::Mat depth2D;
-		iter->uncompressData(0, 0, &depth2D);
+		iter->sensorData().uncompressData(0, 0, &depth2D);

 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 		cloud = util3d::laserScanToPointCloud(depth2D);
@@ -1977,10 +1998,12 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		QApplication::processEvents();

 		int addedSignatures = 0;
+		std::map<int, int> mapIds;
 		for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
 			iter!=event.getSignatures().end();
 			++iter)
 		{
+			mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
 			if(!_cachedSignatures.contains(iter->first))
 			{
 				_cachedSignatures.insert(iter->first, iter->second);
@@ -2000,7 +2023,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			_initProgressDialog->appendText("Updating the 3D map cloud...");
 			_initProgressDialog->incrementStep();
 			QApplication::processEvents();
-			this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), event.getMapIds(), true);
+			this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), mapIds, true);
 			_initProgressDialog->appendText("Updating the 3D map cloud... done.");
 		}
 		else
@@ -3288,15 +3311,15 @@ void MainWindow::postProcessing()
 			{
 				odomPoses.insert(*iter); // fill raw poses
 			}
-			if(jter->getLocalTransform().isNull())
+			if(jter->sensorData().cameraModels().size() == 0 && !jter->sensorData().stereoCameraModel().isValid())
 			{
-				UWARN("Local transform of %d is null.", iter->first);
+				UWARN("Calibration of %d is null.", iter->first);
 				allDataAvailable = false;
 			}
 			if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
 			{
 				// depth data required
-				if(jter->getDepthCompressed().empty() || jter->getFx() <= 0.0f || jter->getFy() <= 0.0f)
+				if(jter->sensorData().depthOrRightCompressed().empty())
 				{
 					UWARN("Depth data of %d missing.", iter->first);
 					allDataAvailable = false;
@@ -3305,7 +3328,7 @@ void MainWindow::postProcessing()
 				if(reextractFeatures)
 				{
 					// rgb required
-					if(jter->getImageCompressed().empty())
+					if(jter->sensorData().imageCompressed().empty())
 					{
 						UWARN("Rgb of %d missing.", iter->first);
 						allDataAvailable = false;
@@ -3354,6 +3377,7 @@ void MainWindow::postProcessing()
 	int loopClosuresAdded = 0;
 	if(detectMoreLoopClosures)
 	{
+		UDEBUG("");
 		Memory memory(parameters);
 		if(reextractFeatures)
 		{
@@ -3426,13 +3450,15 @@ void MainWindow::postProcessing()
 							memory.init("", true); // clear previously added signatures

 							// Add signatures
-							SensorData dataFrom = signatureFrom.toSensorData();
-							SensorData dataTo = signatureTo.toSensorData();
+							SensorData dataFrom = signatureFrom.sensorData();
+							SensorData dataTo = signatureTo.sensorData();
+
+							cv::Mat image, depth;
+							dataFrom.uncompressData(&image, &depth, 0);
+							dataTo.uncompressData(&image, &depth, 0);

 							if(dataFrom.isValid() &&
-							   dataFrom.isMetric() &&
 							   dataTo.isValid() &&
-							   dataTo.isMetric() &&
 							   dataFrom.id() != Memory::kIdInvalid &&
 							   signatureFrom.id() != Memory::kIdInvalid)
 							{
@@ -3502,6 +3528,7 @@ void MainWindow::postProcessing()

 	if(refineNeighborLinks || refineLoopClosureLinks)
 	{
+		UDEBUG("");
 		if(refineLoopClosureLinks)
 		{
 			_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
@@ -3556,83 +3583,96 @@ void MainWindow::postProcessing()
 					Signature & signatureTo = _cachedSignatures[to];

 					//3D
+					UDEBUG("");
 					cv::Mat depthA, depthB;
-					signatureFrom.uncompressData(0, &depthA, 0);
-					signatureTo.uncompressData(0, &depthB, 0);
-
-					if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
+					if(signatureFrom.sensorData().stereoCameraModel().isValid())
 					{
-						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.getFx(), signatureFrom.getFy(), signatureFrom.getCx(), signatureFrom.getCy(),
-							decimation,
-							maxDepth,
-							voxelSize,
-							samples,
-							signatureFrom.getLocalTransform());
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
-							signatureTo.getFx(), signatureTo.getFy(), signatureTo.getCx(), signatureTo.getCy(),
-							decimation,
-							maxDepth,
-							voxelSize,
-							samples,
-							iter->second.transform() * signatureTo.getLocalTransform());
-
-					bool hasConverged = false;
-					double variance = -1;
-					int correspondences = 0;
-					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(cloudANormals);
-						if(cloudA->size() != cloudANormals->size())
-						{
-							UWARN("removed nan normals...");
-						}
-
-						cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
-						if(cloudB->size() != cloudBNormals->size())
-						{
-							UWARN("removed nan normals...");
-						}
-
-						transform = util3d::icpPointToPlane(cloudBNormals,
-								cloudANormals,
-								maxCorrespondences,
-								icpIterations,
-								&hasConverged,
-								&variance,
-								&correspondences);
+						cv::Mat leftA, leftB;
+						signatureFrom.sensorData().uncompressData(&leftA, &depthA, 0);
+						signatureTo.sensorData().uncompressData(&leftB, &depthB, 0);
 					}
 					else
 					{
-						transform = util3d::icp(cloudB,
-								cloudA,
-								maxCorrespondences,
-								icpIterations,
-								&hasConverged,
-								&variance,
-								&correspondences);
+						signatureFrom.sensorData().uncompressData(0, &depthA, 0);
+						signatureTo.sensorData().uncompressData(0, &depthB, 0);
 					}

-					float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
-
-					if(!transform.isNull() && hasConverged &&
-					   correspondencesRatio >= correspondenceRatio)
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::cloudFromSensorData(
+							signatureFrom.sensorData(),
+							decimation,
+							maxDepth,
+							voxelSize,
+							samples);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::cloudFromSensorData(
+							signatureTo.sensorData(),
+							decimation,
+							maxDepth,
+							voxelSize,
+							samples);
+					if(cloudA->size() && cloudB->size())
 					{
-						Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
-						iter->second = newLink;
+						cloudB = util3d::transformPointCloud(cloudB, iter->second.transform());
+
+						bool hasConverged = false;
+						double variance = -1;
+						int correspondences = 0;
+						Transform transform;
+						if(pointToPlane)
+						{
+							UDEBUG("");
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
+
+							cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
+							if(cloudA->size() != cloudANormals->size())
+							{
+								UWARN("removed nan normals...");
+							}
+
+							cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
+							if(cloudB->size() != cloudBNormals->size())
+							{
+								UWARN("removed nan normals...");
+							}
+
+							transform = util3d::icpPointToPlane(cloudBNormals,
+									cloudANormals,
+									maxCorrespondences,
+									icpIterations,
+									&hasConverged,
+									&variance,
+									&correspondences);
+						}
+						else
+						{
+							UDEBUG("");
+							transform = util3d::icp(cloudB,
+									cloudA,
+									maxCorrespondences,
+									icpIterations,
+									&hasConverged,
+									&variance,
+									&correspondences);
+						}
+
+						float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
+
+						if(!transform.isNull() && hasConverged &&
+						   correspondencesRatio >= correspondenceRatio)
+						{
+							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
+							iter->second = newLink;
+						}
+						else
+						{
+							QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
+							_initProgressDialog->appendText(str, Qt::darkYellow);
+							UWARN("%s", str.toStdString().c_str());
+						}
 					}
 					else
 					{
-						QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
+						QString str = tr("Cannot refine link %1->%2 (clouds empty!)").arg(from).arg(to);
 						_initProgressDialog->appendText(str, Qt::darkYellow);
 						UWARN("%s", str.toStdString().c_str());
 					}
@@ -4945,70 +4985,6 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
 	}
 }

-pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
-		int id,
-		const cv::Mat & rgb,
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
-		const Transform & pose,
-		float voxelSize,
-		int decimation,
-		float maxDepth) const
-{
-	UTimer timer;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-	if(depth.type() == CV_8UC1)
-	{
-		cloud = util3d::cloudFromStereoImages(
-				rgb,
-				depth,
-				cx, cy,
-				fx, fy,
-				decimation);
-	}
-	else
-	{
-		cloud = util3d::cloudFromDepthRGB(
-				rgb,
-				depth,
-				cx, cy,
-				fx, fy,
-				decimation);
-	}
-
-	if(cloud->size())
-	{
-		bool filtered = false;
-		if(cloud->size() && maxDepth)
-		{
-			cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
-			filtered = true;
-		}
-
-		if(cloud->size() && voxelSize)
-		{
-			cloud = util3d::voxelize(cloud, voxelSize);
-			filtered = true;
-		}
-
-		if(cloud->size() && !filtered)
-		{
-			cloud = util3d::removeNaNFromPointCloud(cloud);
-		}
-
-		if(cloud->size())
-		{
-			cloud = util3d::transformPointCloud(cloud, pose * localTransform);
-		}
-	}
-	UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks());
-	return cloud;
-}
-
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 		const std::map<int, Transform> & poses,
 		float assembledVoxelSize,
@@ -5031,23 +5007,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 				if(_cachedSignatures.contains(iter->first))
 				{
 					const Signature & s = _cachedSignatures.find(iter->first).value();
+					SensorData d = s.sensorData();
 					cv::Mat image, depth;
-					s.uncompressDataConst(&image, &depth, 0);
+					d.uncompressData(&image, &depth, 0);

 					if(!image.empty() && !depth.empty())
 					{
-						cloud = createCloud(iter->first,
-								image,
-								depth,
-								s.getFx(),
-								s.getFy(),
-								s.getCx(),
-								s.getCy(),
-								s.getLocalTransform(),
-								iter->second,
-								regenerateVoxelSize,
+						UASSERT(iter->first == d.id());
+						cloud = util3d::cloudRGBFromSensorData(
+								d,
 								regenerateDecimation,
-								regenerateMaxDepth);
+								regenerateMaxDepth,
+								regenerateVoxelSize);
+						if(cloud->size())
+						{
+							cloud = util3d::transformPointCloud(cloud, iter->second);
+						}
 					}
 					else if(s.getWords3().size())
 					{
@@ -5135,22 +5110,17 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
 				if(_cachedSignatures.contains(iter->first))
 				{
 					const Signature & s = _cachedSignatures.find(iter->first).value();
+					SensorData d = s.sensorData();
 					cv::Mat image, depth;
-					s.uncompressDataConst(&image, &depth, 0);
+					d.uncompressData(&image, &depth, 0);
 					if(!image.empty() && !depth.empty())
 					{
-						cloud =	 createCloud(iter->first,
-							image,
-							depth,
-							s.getFx(),
-							s.getFy(),
-							s.getCx(),
-							s.getCy(),
-							s.getLocalTransform(),
-							Transform::getIdentity(),
-							regenerateVoxelSize,
+						UASSERT(iter->first == d.id());
+						cloud =	 util3d::cloudRGBFromSensorData(
+							d,
 							regenerateDecimation,
-							regenerateMaxDepth);
+							regenerateMaxDepth,
+							regenerateVoxelSize);
 					}
 					else if(s.getWords3().size())
 					{