RGBD/SavedLocalizationIgnored: if true, it is now starting at the origin (0,0,0) instead of null (not linked to graph).

corelib/src/DBDriverSqlite3.cpp

@@ -4702,7 +4702,7 @@ cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
 
 void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
 {
-	UDEBUG("");
+	UDEBUG("poses=%d lastlocalizationPose=%s", (int)poses.size(), lastlocalizationPose.prettyPrint().c_str());
 	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
 	{
 		UTimer timer;

corelib/src/Memory.cpp

@@ -3066,6 +3066,7 @@ Transform Memory::computeIcpTransformMulti(
 		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>);
+		UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			if(iter->first != fromId)
@@ -3075,7 +3076,7 @@ Transform Memory::computeIcpTransformMulti(
 				{
 					LaserScan scan;
 					s->sensorData().uncompressData(0, 0, &scan);
-					if(!scan.isEmpty() && scan.format() == fromScan.format())
+					if(!scan.isEmpty() && scan.format() == toScan.format())
 					{
 						if(scan.hasIntensity())
 						{
@@ -3106,12 +3107,13 @@ Transform Memory::computeIcpTransformMulti(
 
 						if(scan.size() > maxPoints)
 						{
+							UDEBUG("maxPoints scan(%d) = %d", iter->first, (int)scan.size());
 							maxPoints = scan.size();
 						}
 					}
 					else if(!scan.isEmpty())
 					{
-						UWARN("Incompatible scan format %d vs %d", (int)fromScan.format(), (int)scan.format());
+						UWARN("Incompatible scan format %s vs %s", toScan.formatName().c_str(), scan.formatName().c_str());
 					}
 				}
 				else
@@ -3138,13 +3140,14 @@ Transform Memory::computeIcpTransformMulti(
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
 		}
+		UDEBUG("assembledScan=%d points", assembledScan.cols);
 
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
 		assembledData.setLaserScan(
 				LaserScan(assembledScan,
 					fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
 					fromScan.rangeMax(),
-					fromScan.format(),
+					toScan.format(),
 					fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
 		t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);

corelib/src/Rtabmap.cpp

@@ -318,17 +318,25 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 
 	Transform lastPose;
 	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
-	UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), lastPose.prettyPrint().c_str());
 	if(!_optimizedPoses.empty())
 	{
-		if(!_savedLocalizationIgnored)
+		if(_savedLocalizationIgnored)
 		{
-			_lastLocalizationPose = lastPose;
+			UDEBUG("lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
+			lastPose.setIdentity();
 		}
+		_lastLocalizationPose = lastPose;
+
+		UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
+
 		std::map<int, Transform> tmp;
 		// Get just the links
 		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
 	}
+	else
+	{
+		UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
+	}
 
 	if(_databasePath.empty())
 	{
@@ -5015,7 +5023,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
 		{
 			if(_lastLocalizationPose.isNull() || _optimizedPoses.empty())
 			{
-				UWARN("Last localization pose is null... cannot compute a path");
+				UWARN("Last localization pose is null or optimized graph is empty... cannot compute a path");
 				return false;
 			}
 			if(_optimizedPoses.begin()->first < 0)

guilib/src/DatabaseViewer.cpp

@@ -3019,6 +3019,11 @@ void DatabaseViewer::regenerateSavedMap()
 		dbDriver_->save2DMap(map, xMin, yMin, grid.getCellSize());
 		Transform lastlocalizationPose;
 		dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
+		if(lastlocalizationPose.isNull() && !graphes_.back().empty())
+		{
+			// use last pose by default
+			lastlocalizationPose = graphes_.back().rbegin()->second;
+		}
 		dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
 		// reset optimized mesh as poses have changed
 		dbDriver_->saveOptimizedMesh(cv::Mat());
@@ -4345,7 +4350,7 @@ void DatabaseViewer::update(int value,
 				if(data.laserScanRaw().size())
 				{
 					labelScan->setText(tr("Format=%1 Points=%2 [max=%3] Range=[%4->%5 m] Angle=[%6->%7 rad inc=%8] Has [Color=%9 2D=%10 Normals=%11 Intensity=%12]")
-							.arg(data.laserScanRaw().format())
+							.arg(data.laserScanRaw().formatName().c_str())
 							.arg(data.laserScanRaw().size())
 							.arg(data.laserScanRaw().maxPoints())
 							.arg(data.laserScanRaw().rangeMin())

guilib/src/MainWindow.cpp

@@ -1344,7 +1344,6 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		{
 			_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
 		}
-		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 		if(_preferencesDialog->isFramesShown())
 		{
@@ -1355,7 +1354,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			_cloudViewer->removeLine("odom_to_base_link");
 		}
 #endif
-
+		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
 	}
 	_cloudViewer->update();
 
@@ -2041,6 +2040,21 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			UDEBUG("time= %d ms", time.restart());
 
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+			if(_preferencesDialog->isFramesShown())
+			{
+				_cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
+				_cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
+				_cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false);
+			}
+			else
+			{
+				_cloudViewer->removeLine("map_to_odom");
+				_cloudViewer->removeCoordinate("odom_frame");
+				_cloudViewer->removeCoordinate("map_frame");
+			}
+#endif
+
 			if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
 			{
 				if(poses.rbegin()->first == stat.getLastSignatureData().id())
@@ -2063,21 +2077,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				}
 			}
 
-#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-			if(_preferencesDialog->isFramesShown())
-			{
-				_cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false);
-				_cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false);
-				_cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false);
-			}
-			else
-			{
-				_cloudViewer->removeLine("map_to_odom");
-				_cloudViewer->removeCoordinate("odom_frame");
-				_cloudViewer->removeCoordinate("map_frame");
-			}
-#endif
-
 			if(_cachedSignatures.contains(0) && stat.refImageId()>0)
 			{
 				if(poses.find(stat.refImageId())!=poses.end())