Merge branch 'master' of github.com:introlab/rtabmap into gtest

corelib/include/rtabmap/core/Optimizer.h

@@ -149,14 +149,16 @@ public:
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
 			std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
-			bool rematchFeatures = false);
+			bool rematchFeatures = false,
+			const ParametersMap & registrationParameters = ParametersMap());
 
 	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
-			bool rematchFeatures = false);
+			bool rematchFeatures = false,
+			const ParametersMap & registrationParameters = ParametersMap());
 
 	Transform optimizeBA(
 			const Link & link,
@@ -172,7 +174,8 @@ public:
 			std::map<int, cv::Point3f> & points3DMap,
 			std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
 			bool rematchFeatures = false,
-			bool useLinkTransformAsGuess = false);
+			bool useLinkTransformAsGuess = false,
+			ParametersMap registrationParameters = ParametersMap());
 
 protected:
 	Optimizer(

corelib/include/rtabmap/core/util3d.h

@@ -1060,6 +1060,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
+		float maxDepthError = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
 		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
@@ -1090,6 +1091,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
+		float maxDepthError = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
 		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,

corelib/src/Optimizer.cpp

@@ -447,7 +447,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		const ParametersMap & registrationParameters)
 {
 	UDEBUG("");
 	std::map<int, std::vector<CameraModel> > multiModels;
@@ -497,7 +498,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 	}
 
 	// compute correspondences
-	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
+	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures, false, registrationParameters);
 
 	return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
 }
@@ -507,11 +508,12 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
-		bool rematchFeatures)
+		bool rematchFeatures,
+		const ParametersMap & registrationParameters)
 {
 	std::map<int, cv::Point3f> points3DMap;
 	std::map<int, std::map<int, FeatureBA> > wordReferences;
-	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
+	return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures, registrationParameters);
 }
 
 Transform Optimizer::optimizeBA(
@@ -557,11 +559,20 @@ void Optimizer::computeBACorrespondences(
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		bool rematchFeatures,
-		bool useLinkTransformAsGuess)
+		bool useLinkTransformAsGuess,
+		ParametersMap registrationParameters)
 {
 	UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
+
+	// Some defaults if not provided
+	registrationParameters.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
+	registrationParameters.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+	RegistrationVis reg(registrationParameters);
+
 	std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
 	for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
 	{
@@ -601,13 +612,6 @@ void Optimizer::computeBACorrespondences(
 					sTo.getWords().size() &&
 					sFrom.getWords3().size())
 				{
-					ParametersMap regParam;
-					regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
-					regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
-					regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
-					regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
-					RegistrationVis reg(regParam);
-
 					if(!rematchFeatures)
 					{
 						sFrom.setWordsDescriptors(cv::Mat());

corelib/src/python/rtabmap_superpoint.py

@@ -2,7 +2,7 @@
 #
 # Drop this file in the root folder of SuperPoint git: https://github.com/magicleap/SuperPointPretrainedNetwork
 # To use with rtabmap:
-#   --Vis/FeatureType 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py" --PyDetector/Model "~/SuperPointPretrainedNetwork/superpoint_v1.pth"
+#   --Vis/FeatureType 15 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py"
 #
 
 import random

corelib/src/util3d.cpp

@@ -3078,6 +3078,18 @@ public:
 	float distance;
 };
 
+class RegisteredPoints {
+	public:
+		class Point {
+			public:
+				Point(float distance_, int index_) : distance(distance_), index(index_) {}
+				float distance;
+				int index;
+		};
+		float minDistance;
+		std::vector<Point> points;
+};
+
 /**
  * For each point, return pixel of the best camera (NodeID->CameraIndex)
  * looking at it based on the policy and parameters
@@ -3089,6 +3101,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance,
 		float maxAngle,
+		float maxDepthError,
 		const std::vector<float> & roiRatios,
 		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
@@ -3099,6 +3112,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	UINFO("cameraModels=%d", (int)cameraModels.size());
 	UINFO("maxDistance=%f", maxDistance);
 	UINFO("maxAngle=%f", maxAngle);
+	UINFO("maxDepthError=%f", maxDepthError);
 	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
 	UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]).c_str():"");
 	UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
@@ -3164,8 +3178,9 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				float cx = cameraMatrixK.at<double>(0,2);
 				float cy = cameraMatrixK.at<double>(1,2);
 
-				// depth: 2 channels UINT: [depthMM, indexPt]
+				// [rows][cols][depth, indexPt]
-				cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
+				std::vector<std::vector<RegisteredPoints> > registered(
+					imageSize.height, std::vector<RegisteredPoints>(imageSize.width));
 				Transform t = cameraTransform.inverse();
 
 				cv::Rect roi(0,0,imageSize.width, imageSize.height);
@@ -3187,34 +3202,44 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 					if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
 					{
 						float invZ = 1.0f/z;
-						float dx = (fx*ptScan.x)*invZ + cx;
+						float u = (fx*ptScan.x)*invZ + cx;
-						float dy = (fy*ptScan.y)*invZ + cy;
+						float v = (fy*ptScan.y)*invZ + cy;
-						int dx_low = dx;
+						int x = u + 0.5f;
-						int dy_low = dy;
+						int y = v + 0.5f;
-						int dx_high = dx + 0.5f;
+
-						int dy_high = dy + 0.5f;
+						if(uIsInBounds(x, roi.x, roi.x+roi.width) && uIsInBounds(y, roi.y, roi.y+roi.height) &&
-						int zMM = z * 1000;
+						   (validProjMask.empty() || validProjMask.at<unsigned char>(y, imageSize.width*camIndex+x) > 0)) {
-						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
+							RegisteredPoints &zReg = registered[y][x];
-						   (validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
+							if(zReg.points.empty()) {
-						{
+								zReg.minDistance = z;
-							set = true;
+								zReg.points.push_back(RegisteredPoints::Point(z, i));
-							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
+								set = true;
-							if(zReg[0] == 0 || zMM < zReg[0])
-							{
-								zReg[0] = zMM;
-								zReg[1] = i;
 							}
-						}
+							else if(z < zReg.minDistance) {
-						if((dx_low != dx_high || dy_low != dy_high) &&
+								zReg.minDistance = z;
-							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
+								if(maxDepthError<=0.0f) {
-							(validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
+									// keeping only closest point, just update it
-						{
+									zReg.points[0].distance = z;
-							set = true;
+									zReg.points[0].index = i;
-							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
+								}
-							if(zReg[0] == 0 || zMM < zReg[0])
+								else {
-							{
+									// update the points attached to same pixel based on new closest distance
-								zReg[0] = zMM;
+									std::vector<RegisteredPoints::Point> reOrderedPts;
-								zReg[1] = i;
+									reOrderedPts.push_back(RegisteredPoints::Point(z, i));
+									for(size_t p=0; p<zReg.points.size(); ++p) {
+										if(zReg.points[p].distance - z < maxDepthError) {
+											reOrderedPts.push_back(zReg.points[p]);
+										}
+									}
+									zReg.points = reOrderedPts;
+								}
+								set = true;
+							}
+							else if(maxDepthError>=0.0f && z - zReg.minDistance < maxDepthError) {
+								// The point is closer than current closest one to camera, 
+								// but still under max depth difference, just append
+								zReg.points.push_back(RegisteredPoints::Point(z, i));
+								set = true;
 							}
 						}
 					}
@@ -3225,19 +3250,19 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				}
 				if(count == 0)
 				{
-					registered = cv::Mat();
+					registered.clear();
 					UINFO("No points projected in camera %d/%d", pter->first, camIndex);
 				}
 				else
 				{
 					UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
 				}
-				for(int u=0; u<registered.cols; ++u)
+				for(int u=0; u<imageSize.width; ++u)
 				{
-					for(int v=0; v<registered.rows; ++v)
+					for(int v=0; v<imageSize.height; ++v)
 					{
-						cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
+						RegisteredPoints &zReg = registered[v][u];
-						if(zReg[0] > 0)
+						if(!zReg.points.empty())
 						{
 							ProjectionInfo info;
 							info.nodeID = pter->first;
@@ -3245,36 +3270,40 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 							info.uv.x = float(u)/float(imageSize.width);
 							info.uv.y = float(v)/float(imageSize.height);
 							const Transform & cam = cameraPoses.at(info.nodeID);
-							const PointT & pt = cloud.at(zReg[1]);
+							for(size_t p=0; p<zReg.points.size(); ++p)
-							Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
-							Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
-							float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
-							float distanceToCam = zReg[0]/1000.0f;
-							if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
-								(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
 							{
-								float vx = info.uv.x-0.5f;
+								int ptIdx = zReg.points[p].index;
-								float vy = info.uv.y-0.5f;
+								const PointT & pt = cloud.at(ptIdx);
-
+								Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
-								float distanceToCenter = vx*vx+vy*vy;
+								Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
-								float distance = distanceToCenter;
+								float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
-								if(distanceToCamPolicy)
+								float distanceToCam = zReg.points[p].distance;
+								if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
+									(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
 								{
-									distance = distanceToCam;
+									float vx = info.uv.x-0.5f;
-								}
+									float vy = info.uv.y-0.5f;
 
-								info.distance = distance;
+									float distanceToCenter = vx*vx+vy*vy;
-
+									float distance = distanceToCenter;
-								if(invertedIndex[zReg[1]].distance != -1.0f)
+									if(distanceToCamPolicy)
-								{
-									if(distance <= invertedIndex[zReg[1]].distance)
 									{
-										invertedIndex[zReg[1]] = info;
+										distance = distanceToCam;
+									}
+
+									info.distance = distance;
+
+									if(invertedIndex[ptIdx].distance != -1.0f)
+									{
+										if(distance <= invertedIndex[ptIdx].distance)
+										{
+											invertedIndex[ptIdx] = info;
+										}
+									}
+									else
+									{
+										invertedIndex[ptIdx] = info;
 									}
-								}
-								else
-								{
-									invertedIndex[zReg[1]] = info;
 								}
 							}
 						}
@@ -3344,6 +3373,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance,
 		float maxAngle,
+		float maxDepthError,
 		const std::vector<float> & roiRatios,
 		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
@@ -3354,6 +3384,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			cameraModels,
 			maxDistance,
 			maxAngle,
+			maxDepthError,
 			roiRatios,
 			projMask,
 			distanceToCamPolicy,
@@ -3366,6 +3397,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
 		float maxDistance,
 		float maxAngle,
+		float maxDepthError,
 		const std::vector<float> & roiRatios,
 		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
@@ -3376,6 +3408,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			cameraModels,
 			maxDistance,
 			maxAngle,
+			maxDepthError,
 			roiRatios,
 			projMask,
 			distanceToCamPolicy,

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -65,6 +65,8 @@ class ExportCloudsDialog;
 class EditDepthArea;
 class EditMapArea;
 class LinkRefiningDialog;
+class Registration;
+class RegistrationIcp;
 
 class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow
 {
@@ -202,8 +204,8 @@ private:
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
 	void updateCovariances(const QList<Link> & links);
 	void refineLinks(const QList<Link> & links);
-	void refineConstraint(int from, int to,  bool silent);
+	void refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regIcp, bool silent);
-	bool addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
+	bool addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
 	void exportPoses(int format);
 	void exportGPS(int format);
 

guilib/src/DataRecorder.cpp

@@ -156,8 +156,12 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
 void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
 {
 	processingImages_ = true;
-	imageView_->setImage(uCvMat2QImage(image));
+	if(!image.empty()) {
-	imageView_->setImageDepth(depth);
+		imageView_->setImage(uCvMat2QImage(image));
+	}
+	if(!depth.empty()) {
+		imageView_->setImageDepth(depth);
+	}
 	label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
 	processingImages_ = false;
 }

guilib/src/DatabaseViewer.cpp

@@ -4263,6 +4263,8 @@ void DatabaseViewer::detectMoreLoopClosures()
 		return;
 	}
 
+	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("iteration %d/%d", n+1, iterations);
@@ -4310,7 +4312,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 						   delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
 						{
 							checkedLoopClosures.insert(std::make_pair(from, to));
-							if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
+							if(addConstraint(from, to, reg.get(), true, useOptimizedGraphAsGuess))
 							{
 								UINFO("Added new loop closure between %d and %d.", from, to);
 								++added;
@@ -4568,13 +4570,16 @@ void DatabaseViewer::refineLinks(const QList<Link> & links)
 		progressDialog->setMinimumWidth(800);
 		progressDialog->show();
 
+		RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
+		std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+
 		for(int i=0; i<links.size(); ++i)
 		{
 			int from = links[i].from();
 			int to = links[i].to();
 			if(from > 0 && to > 0)
 			{
-				this->refineConstraint(links[i].from(), links[i].to(), true);
+				this->refineConstraint(links[i].from(), links[i].to(), reg.get(), &regProximity, true);
 				progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
 			}
 			else
@@ -8085,10 +8090,12 @@ void DatabaseViewer::refineConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	refineConstraint(from, to, false);
+	RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
+	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+	refineConstraint(from, to, reg.get(), &regProximity, false);
 }
 
-void DatabaseViewer::refineConstraint(int from, int to, bool silent)
+void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regProximity, bool silent)
 {
 	UDEBUG("%d -> %d", from, to);
 	bool switchedIds = false;
@@ -8361,8 +8368,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 				fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
 
 		toS = new Signature(assembledData);
-		RegistrationIcp registrationIcp(parameters);
+		transform = regProximity->computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
-		transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
 		if(!transform.isNull())
 		{
 			// local scan matching proximity detection should have higher variance (see Rtabmap::process())
@@ -8380,7 +8386,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		}
 
 		bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
-		Registration * reg = Registration::create(parameters);
 		if( reg->isScanRequired() ||
 			reg->isUserDataRequired() ||
 			reextractVisualFeatures ||
@@ -8477,8 +8482,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
 			switchedIds = true;
 		}
-
-		delete reg;
 	}
 	UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
 
@@ -8610,11 +8613,14 @@ void DatabaseViewer::addConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	addConstraint(from, to, false);
+	std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
+	addConstraint(from, to, reg.get(), false);
 }
 
-bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
+bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess)
 {
+	UASSERT(reg);
+
 	bool switchedIds = false;
 	if(from == to)
 	{
@@ -8641,7 +8647,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
 		UASSERT(!containsLink(linksRefined_, from, to));
 
 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
-		Registration * reg = Registration::create(parameters);
 
 		bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
 		Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
@@ -8823,7 +8828,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
 		{
 			t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
 		}
-		delete reg;
 		UDEBUG("");
 
 		if(!t.isNull())

guilib/src/ExportBundlerDialog.cpp

@@ -194,17 +194,37 @@ void ExportBundlerDialog::exportBundler(
 			ParametersMap parametersSBA = parameters;
 			uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
 			uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
-			Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
+			std::shared_ptr<Optimizer> sba(Optimizer::create(sbaType, parametersSBA));
 			sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
-			poses = sba->optimizeBA(
+			// set input poses as initial optimization guess
-					posesOut.begin()->first,
+			for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
-					posesOut,
+			{
-					linksOut,
+				iter->second = poses.at(iter->first);
+			}
+			if(_ui->sba_iterations->value() > 0)
+			{
+				poses = sba->optimizeBA(
+						posesOut.begin()->first,
+						posesOut,
+						linksOut,
+						signatures.toStdMap(),
+						points3DMap,
+						wordReferences,
+						_ui->sba_rematchFeatures->isChecked());
+			}
+			else
+			{
+				// do not optimize, just compute 3D features and word correspondences
+				poses = posesOut;
+				sba->computeBACorrespondences(poses,
+					linksOut, 
 					signatures.toStdMap(),
 					points3DMap,
 					wordReferences,
-					_ui->sba_rematchFeatures->isChecked());
+					_ui->sba_rematchFeatures->isChecked(),
-			delete sba;
+					false,
+					parametersSBA);
+			}
 
 			if(poses.empty())
 			{

guilib/src/ExportCloudsDialog.cpp

@@ -209,6 +209,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->spinBox_camProjDecimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_camProjMaxDepthError, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -443,6 +444,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("cam_proj_decimation", _ui->spinBox_camProjDecimation->value());
 	settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value());
 	settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value());
+	settings.setValue("cam_proj_max_depth_error", _ui->doubleSpinBox_camProjMaxDepthError->value());
 	settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked());
 	settings.setValue("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked());
 	settings.setValue("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked());
@@ -628,6 +630,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_camProjDecimation->setValue(settings.value("cam_proj_decimation", _ui->spinBox_camProjDecimation->value()).toInt());
 	_ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble());
 	_ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble());
+	_ui->doubleSpinBox_camProjMaxDepthError->setValue(settings.value("cam_proj_max_depth_error", _ui->doubleSpinBox_camProjMaxDepthError->value()).toDouble());
 	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool());
 	_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(settings.value("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()).toBool());
 	_ui->checkBox_camProjRecolorPoints->setChecked(settings.value("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()).toBool());
@@ -810,6 +813,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_camProjDecimation->setValue(1);
 	_ui->doubleSpinBox_camProjMaxDistance->setValue(0);
 	_ui->doubleSpinBox_camProjMaxAngle->setValue(0);
+	_ui->doubleSpinBox_camProjMaxDepthError->setValue(0);
 	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(true);
 	_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(false);
 	_ui->checkBox_camProjRecolorPoints->setChecked(true);
@@ -2911,6 +2915,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						cameraModelsProj,
 						_ui->doubleSpinBox_camProjMaxDistance->value(),
 						_ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0,
+						_ui->doubleSpinBox_camProjMaxDepthError->value(),
 						roiRatios,
 						projMask,
 						_ui->checkBox_camProjDistanceToCamPolicy->isChecked(),

guilib/src/MainWindow.cpp

@@ -6623,6 +6623,8 @@ void MainWindow::postProcessing(
 			odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
 		}
 
+		std::shared_ptr<Registration> registration(Registration::create(parameters));
+
 		UASSERT(iterations>0);
 		for(int n=0; n<iterations && !_progressCanceled; ++n)
 		{
@@ -6703,7 +6705,6 @@ void MainWindow::postProcessing(
 										{
 											uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
 										}
-										Registration * registration = Registration::create(parameters);
 
 										if(reextractFeatures)
 										{
@@ -6735,7 +6736,6 @@ void MainWindow::postProcessing(
 													Parameters::kRGBDLoopClosureReextractFeatures().c_str());
 										}
 										transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
-										delete registration;
 										if(!transform.isNull())
 										{
 											//optimize the graph to see if the new constraint is globally valid
@@ -7023,7 +7023,13 @@ void MainWindow::postProcessing(
 		uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
 		uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
 		Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA);
-		std::map<int, Transform>  newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
+		std::map<int, Transform>  newPoses = sbaOptimizer->optimizeBA(
+			optimizedPoses.begin()->first,
+			optimizedPoses,
+			linksOut,
+			_cachedSignatures.toStdMap(),
+			sbaRematchFeatures,
+			parametersSBA);
 		delete sbaOptimizer;
 		if(newPoses.size())
 		{

guilib/src/ui/exportBundlerDialog.ui

@@ -141,7 +141,7 @@
         <item row="1" column="0">
          <widget class="QSpinBox" name="sba_iterations">
           <property name="minimum">
-           <number>1</number>
+           <number>0</number>
           </property>
           <property name="maximum">
            <number>999999</number>

guilib/src/ui/exportCloudsDialog.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>919</width>
+    <width>895</width>
     <height>869</height>
    </rect>
   </property>
@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-3199</y>
+        <y>-2495</y>
-        <width>885</width>
+        <width>861</width>
-        <height>6183</height>
+        <height>6426</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2012,100 +2012,30 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
           <bool>false</bool>
          </property>
          <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1">
+          <item row="7" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_14">
+            <property name="text">
+             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="2">
+           <widget class="QLabel" name="label_meshingTextureSize_18">
+            <property name="text">
+             <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
           <item row="6" column="1">
-           <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_16">
-            <property name="text">
-             <string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="2" colspan="2">
-           <widget class="QLabel" name="label_camProjExportCamera">
-            <property name="text">
-             <string>ID format of the camera selected for each point.</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_13">
-            <property name="text">
-             <string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="1">
-           <widget class="QSpinBox" name="spinBox_camProjDecimation">
-            <property name="suffix">
-             <string/>
-            </property>
-            <property name="minimum">
-             <number>1</number>
-            </property>
-            <property name="maximum">
-             <number>9999</number>
-            </property>
-            <property name="singleStep">
-             <number>1</number>
-            </property>
-            <property name="value">
-             <number>1</number>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_17">
-            <property name="text">
-             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_12">
-            <property name="text">
-             <string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="8" column="1">
-           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="1">
            <widget class="QLineEdit" name="lineEdit_camProjMaskFilePath"/>
           </item>
-          <item row="7" column="1">
+          <item row="10" column="1">
-           <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="9" column="1">
            <widget class="QComboBox" name="comboBox_camProjExportCamera">
             <property name="toolTip">
              <string>By Node ID: cameras of same node have same ID
@@ -2134,18 +2064,56 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </item>
            </widget>
           </item>
-          <item row="6" column="2" colspan="2">
+          <item row="9" column="1">
-           <widget class="QLabel" name="label_meshingTextureSize_14">
+           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
             <property name="text">
-             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_13">
+            <property name="text">
+             <string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-          <item row="1" column="1">
+          <item row="8" column="1">
-           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+           <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_17">
+            <property name="text">
+             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="0">
+           <widget class="QToolButton" name="toolButton_camProjMaskFilePath">
+            <property name="text">
+             <string>...</string>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_16">
+            <property name="text">
+             <string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
           </item>
           <item row="3" column="1">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
@@ -2169,6 +2137,65 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
+          <item row="1" column="1">
+           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+          </item>
+          <item row="5" column="1">
+           <widget class="QSpinBox" name="spinBox_camProjDecimation">
+            <property name="suffix">
+             <string/>
+            </property>
+            <property name="minimum">
+             <number>1</number>
+            </property>
+            <property name="maximum">
+             <number>9999</number>
+            </property>
+            <property name="singleStep">
+             <number>1</number>
+            </property>
+            <property name="value">
+             <number>1</number>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_19">
+            <property name="text">
+             <string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_12">
+            <property name="text">
+             <string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="10" column="2" colspan="2">
+           <widget class="QLabel" name="label_camProjExportCamera">
+            <property name="text">
+             <string>ID format of the camera selected for each point.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
           <item row="2" column="1">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
             <property name="suffix">
@@ -2191,7 +2218,7 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
-          <item row="7" column="2" colspan="2">
+          <item row="8" column="2" colspan="2">
            <widget class="QLabel" name="label_meshingTextureSize_15">
             <property name="text">
              <string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
@@ -2201,33 +2228,38 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
-          <item row="8" column="2">
+          <item row="4" column="1">
-           <widget class="QLabel" name="label_meshingTextureSize_18">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDepthError">
-            <property name="text">
+            <property name="suffix">
-             <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
+             <string> m</string>
             </property>
-            <property name="wordWrap">
+            <property name="decimals">
-             <bool>true</bool>
+             <number>2</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>9.990000000000000</double>
+            </property>
+            <property name="singleStep">
+             <double>0.050000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.000000000000000</double>
             </property>
            </widget>
           </item>
-          <item row="5" column="2" colspan="2">
+          <item row="4" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_19">
+           <widget class="QLabel" name="label_meshingTextureSize_20">
             <property name="text">
-             <string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
+             <string>For all points registered to same pixel, only those close enough to the closest point will be colored with same color (0 means only the closest point of all points registered in same pixel is colored).</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-          <item row="5" column="0">
-           <widget class="QToolButton" name="toolButton_camProjMaskFilePath">
-            <property name="text">
-             <string>...</string>
-            </property>
-           </widget>
-          </item>
          </layout>
         </widget>
        </item>

tools/DataRecorder/main.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraRGBD.h>
 #include <rtabmap/core/CameraStereo.h>
 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/core/DBReader.h>
 #include <rtabmap/core/SensorCaptureThread.h>
 #include <rtabmap/core/SensorCaptureThread.h>
@@ -71,6 +72,31 @@ void sighandler(int sig)
 	}
 }
 
+// Detect when we reached end-of-files
+class StatusHandler: public UEventsHandler{
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeNoMoreImages)
+			{
+				printf("End of stream reached...\n");
+				if(cam)
+				{
+					cam->join(true);
+				}
+				if(app)
+				{
+					QMetaObject::invokeMethod(app, "quit");
+				}
+			}
+		}
+		return false;
+	}
+};
+
 int main (int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
@@ -138,6 +164,10 @@ int main (int argc, char * argv[])
 	signal(SIGTERM, &sighandler);
 	signal(SIGINT, &sighandler);
 
+	// Catch end of stream to close the gui
+	StatusHandler statusHandler;
+	statusHandler.registerToEventsManager();
+
 	rtabmap::Camera * camera = dialog.createCamera();
 	if(camera == 0)
 	{

tools/Export/main.cpp

@@ -106,9 +106,10 @@ void showUsage()
 			"                              2=Use optimized poses already computed in the database instead\n"
 			"                                of re-computing them (fallback to default if optimized poses don't exist).\n"
 			"                              3=No optimization, use odometry poses directly.\n"
-			"    --poses               Export optimized poses of the robot frame (e.g., base_link).\n"
+			"    --poses               Export optimized poses of the robot frame (e.g., base_link), including landmarks.\n"
 			"    --poses_camera        Export optimized poses of the camera frame (e.g., optical frame).\n"
 			"    --poses_scan          Export optimized poses of the scan frame.\n"
+			"    --poses_landmark      Export optimized poses of landmarks.\n"
 			"    --poses_gt            Export ground truth poses of the robot frame (e.g., base_link).\n"
 			"    --poses_gps           Export GPS poses of the GPS frame in local coordinates.\n"
 			"    --poses_format #      Format used for exported poses (default is 11):\n"
@@ -252,6 +253,7 @@ int main(int argc, char * argv[])
 	bool exportPoses = false;
 	bool exportPosesCamera = false;
 	bool exportPosesScan = false;
+	bool exportPosesLandmarks = false;
 	bool exportPosesGt = false;
 	bool exportPosesGps = false;
 	int exportPosesFormat = 11;
@@ -498,6 +500,10 @@ int main(int argc, char * argv[])
 		{
 			exportPosesScan = true;
 		}
+		else if(std::strcmp(argv[i], "--poses_landmark") == 0)
+		{
+			exportPosesLandmarks = true;
+		}
 		else if(std::strcmp(argv[i], "--poses_gt") == 0)
 		{
 			exportPosesGt = true;
@@ -1079,6 +1085,7 @@ int main(int argc, char * argv[])
 		 exportPoses ||
 		 exportPosesScan ||
 		 exportPosesCamera ||
+		 exportPosesLandmarks ||
 		 exportPosesGt ||
 		 exportPosesGps ||
 		 exportGps>=0 ||
@@ -1142,7 +1149,7 @@ int main(int argc, char * argv[])
 	std::multimap<int, Link> links;
 	dbDriver->getAllOdomPoses(odomPoses, true);
 	dbDriver->getAllLinks(links, true, true);
-	if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan))
+	if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan || exportPosesLandmarks))
 	{
 		// Just use odometry poses when exporting only images
 		optimizedPoses = odomPoses;
@@ -1346,13 +1353,18 @@ int main(int argc, char * argv[])
 	pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
 	std::map<int, rtabmap::Transform> robotPoses;
 	std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
+	std::vector<std::map<int, double> > cameraStamps;
 	std::map<int, rtabmap::Transform> scanPoses;
+	std::map<int, double> scanStamps;
+	std::map<int, rtabmap::Transform> landmarkPoses;
+	std::map<int, double> landmarkStamps;
 	std::map<int, rtabmap::Transform> gtPoses;
+	std::map<int, double> gtStamps;
 	std::map<int, rtabmap::Transform> gpsPoses;
 	std::map<int, double> gpsStamps;
 	GPS gpsOrigin;
 	std::map<int, rtabmap::GPS> gpsValues;
-	std::map<int, double> cameraStamps;
+	std::map<int, double> robotStamps;
 	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
 	std::map<int, cv::Mat> cameraDepths;
 	int imagesExported = 0;
@@ -1376,7 +1388,10 @@ int main(int argc, char * argv[])
 		{
 			// landmark, just add to list of poses
 			robotPoses.insert(*iter);
-			cameraStamps.insert(std::make_pair(iter->first, 0));
+			robotStamps.insert(std::make_pair(iter->first, 0));
+
+			landmarkPoses.insert(*iter);
+			landmarkStamps.insert(std::make_pair(iter->first, 0));
 			continue;
 		}
 		
@@ -1628,7 +1643,7 @@ int main(int argc, char * argv[])
 		}
 
 		robotPoses.insert(std::make_pair(iter->first, iter->second));
-		cameraStamps.insert(std::make_pair(iter->first, stamp));
+		robotStamps.insert(std::make_pair(iter->first, stamp));
 		if(models.empty() && weight == -1 && !cameraModels.empty())
 		{
 			// For intermediate nodes, use latest models
@@ -1645,17 +1660,20 @@ int main(int argc, char * argv[])
 				if(cameraPoses.empty())
 				{
 					cameraPoses.resize(models.size());
+					cameraStamps.resize(models.size());
 				}
 				UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
 				for(size_t i=0; i<models.size(); ++i)
 				{
 					cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
+					cameraStamps[i].insert(std::make_pair(iter->first, stamp));
 				}
 			}
 		}
 		if(exportPosesScan && !data.laserScanCompressed().empty())
 		{
 			scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
+			scanStamps.insert(std::make_pair(iter->first, stamp));
 		}
 
 		if(exportPosesGps || exportGps>=0)
@@ -1688,6 +1706,7 @@ int main(int argc, char * argv[])
 		if(exportPosesGt && !gt.isNull())
 		{
 			gtPoses.insert(std::make_pair(iter->first, gt));
+			gtStamps.insert(std::make_pair(iter->first, stamp));
 		}
 
 		if(optimizedPoses.size() >= 500)
@@ -1750,11 +1769,11 @@ int main(int argc, char * argv[])
 					exportPosesFormat,
 					std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()),
 					links,
-					std::map<int, double>(cameraStamps.lower_bound(1), cameraStamps.end()));
+					std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end()));
 			}
 			else
 			{
-				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps);
+				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps);
 			}
 			cv::Vec3f vmin, vmax;
 			graph::computeMinMax(robotPoses, vmin, vmax);
@@ -1773,7 +1792,7 @@ int main(int argc, char * argv[])
 					outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
 				else
 					outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
-				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
+				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i]);
 				cv::Vec3f vmin, vmax;
 				graph::computeMinMax(cameraPoses[i], vmin, vmax);
 				printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -1786,7 +1805,7 @@ int main(int argc, char * argv[])
 		if(exportPosesScan)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), cameraStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
 			cv::Vec3f min, max;
 			graph::computeMinMax(scanPoses, min, max);
 			printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -1795,6 +1814,30 @@ int main(int argc, char * argv[])
 					min[0], min[1], min[2],
 					max[0], max[1], max[2]);
 		}
+		if(exportPosesScan)
+		{
+			std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
+			cv::Vec3f min, max;
+			graph::computeMinMax(scanPoses, min, max);
+			printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
+					(int)scanPoses.size(),
+					outputPath.c_str(),
+					min[0], min[1], min[2],
+					max[0], max[1], max[2]);
+		}
+		if(exportPosesLandmarks)
+		{
+			std::string outputPath=outputDirectory+"/"+baseName+"_landmark_poses." + posesExt;
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps);
+			cv::Vec3f min, max;
+			graph::computeMinMax(landmarkPoses, min, max);
+			printf("%d landmark poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
+					(int)landmarkPoses.size(),
+					outputPath.c_str(),
+					min[0], min[1], min[2],
+					max[0], max[1], max[2]);
+		}
 		if(exportPosesGps)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt;
@@ -1806,7 +1849,7 @@ int main(int argc, char * argv[])
 		if(exportPosesGt)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), cameraStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps);
 			printf("%d scan poses exported to \"%s\".\n",
 					(int)gtPoses.size(),
 					outputPath.c_str());
@@ -1999,7 +2042,7 @@ int main(int argc, char * argv[])
 					}
 
 					depth = rtabmap::util2d::cvtDepthFromFloat(depth);
-					std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
+					std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",robotStamps.at(iter->first)))+".png";
 					cv::imwrite(outputPath, depth);
 				}
 			}
@@ -2027,6 +2070,7 @@ int main(int argc, char * argv[])
 						cameraModelsProj,
 						textureRange,
 						textureAngle,
+						textureDepthError,
 						textureRoiRatios,
 						projMask,
 						distanceToCamPolicy,
@@ -2040,6 +2084,7 @@ int main(int argc, char * argv[])
 						cameraModelsProj,
 						textureRange,
 						textureAngle,
+						textureDepthError,
 						textureRoiRatios,
 						projMask,
 						distanceToCamPolicy,

tools/GlobalBundleAdjustment/main.cpp

@@ -119,7 +119,7 @@ int main(int argc, char * argv[])
 
 	printf("Global bundle adjustment...\n");
 	Optimizer * optimizer = Optimizer::create(Optimizer::kTypeG2O, parameters);
-	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
+	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true, parameters);
 	delete optimizer;
 	printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
 

tools/Reprocess/main.cpp

@@ -1024,7 +1024,7 @@ int main(int argc, char * argv[])
 
 		UTimer iterationTime;
 		std::string status;
-		if(!odometryIgnored && info.odomPose.isNull())
+		if(!odometryIgnored && info.odomPose.isNull() && incrementalMemory)
 		{
 			printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
 		}
@@ -1235,15 +1235,22 @@ int main(int argc, char * argv[])
 				localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
 			}
 
-			if(exportPoses && !info.odomPose.isNull())
+			if(exportPoses)
 			{
-				if(!odomTrajectoryPoses.empty())
+				if(!info.odomPose.isNull())
 				{
-					int previousId = odomTrajectoryPoses.rbegin()->first;
+					if(!odomTrajectoryPoses.empty())
-					odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
+					{
+						int previousId = odomTrajectoryPoses.rbegin()->first;
+						odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
+					}
+					odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
+					localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
+				}
+				else
+				{
+					localizationPoses.insert(std::make_pair(refId, rtabmap.getLastLocalizationPose()));
 				}
-				odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
-				localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
 			}
 		}