Rtabmap::detectMoreLoopClosures: added clusterRadiusMin parameter and update optimized poses after each accepted loop closure (also in MainWindow) like in DbViewer. Added graph::computeMinMax(poses). OdometryInfo: added guess transform. Export: added min/max axis ranges to filter nodes before expoting clouds.

corelib/include/rtabmap/core/Graph.h

@@ -327,6 +327,9 @@ std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
 		std::map<int, Transform> poses,
 		const std::multimap<int, Link> & links);
 
+void RTABMAP_EXP computeMinMax(const std::map<int, Transform> & poses,
+		cv::Vec3f & min,
+		cv::Vec3f & max);
 
 } /* namespace graph */
 

corelib/include/rtabmap/core/OdometryInfo.h

@@ -84,6 +84,7 @@ public:
 		output.transformFiltered = transformFiltered;
 		output.transformGroundTruth = transformGroundTruth;
 		output.guessVelocity = guessVelocity;
+		output.guess = guess;
 		output.distanceTravelled = distanceTravelled;
 		output.memoryUsage = memoryUsage;
 		output.gravityRollError = gravityRollError;
@@ -111,7 +112,8 @@ public:
 	Transform transform;
 	Transform transformFiltered;
 	Transform transformGroundTruth;
-	Transform guessVelocity;
+	Transform guessVelocity; // deprecated, will be removed. Use guess and interval instead.
+	Transform guess;
 	float distanceTravelled;
 	int memoryUsage; //MB
 	double gravityRollError;

corelib/include/rtabmap/core/Rtabmap.h

@@ -199,12 +199,13 @@ public:
 	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
 	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
-			float clusterRadius = 0.5f,
+			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,
 			int iterations = 1,
 			bool intraSession = true,
 			bool interSession = true,
-			const ProgressState * state = 0);
+			const ProgressState * state = 0,
+			float clusterRadiusMin = 0.0f);
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;

corelib/src/Graph.cpp

@@ -2334,6 +2334,33 @@ std::list<std::map<int, Transform> > getPaths(
 	return paths;
 }
 
+void computeMinMax(const std::map<int, Transform> & poses,
+		cv::Vec3f & min,
+		cv::Vec3f & max)
+{
+	if(!poses.empty())
+	{
+		min[0] = max[0] = poses.begin()->second.x();
+		min[1] = max[1] = poses.begin()->second.y();
+		min[2] = max[2] = poses.begin()->second.z();
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(min[0] > iter->second.x())
+				min[0] = iter->second.x();
+			if(max[0] < iter->second.x())
+				max[0] = iter->second.x();
+			if(min[1] > iter->second.y())
+				min[1] = iter->second.y();
+			if(max[1] < iter->second.y())
+				max[1] = iter->second.y();
+			if(min[2] > iter->second.z())
+				min[2] = iter->second.z();
+			if(max[2] < iter->second.z())
+				max[2] = iter->second.z();
+		}
+	}
+}
+
 } /* namespace graph */
 
 } /* namespace rtabmap */

corelib/src/Odometry.cpp

@@ -481,6 +481,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				guess = guess.to3DoF();
 			}
 		}
+		else if(!imuLastTransform_.isNull())
+		{
+			UWARN("Could not find imu transform at %f", data.stamp());
+		}
 	}
 
 	UTimer time;
@@ -562,6 +566,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		info->stamp = data.stamp();
 		info->interval = dt;
 		info->transform = t;
+		info->guess = guess;
 		if(_publishRAMUsage)
 		{
 			info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);

corelib/src/RegistrationIcp.cpp

@@ -547,7 +547,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 #else
 	if(_libpointmatcher)
 	{
-		UINFO("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
+		UDEBUG("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
 		if(_libpointmatcherICP!=0)
 		{
 			delete (PM::ICP*)_libpointmatcherICP;

corelib/src/Rtabmap.cpp

@@ -4650,12 +4650,13 @@ std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
 }
 
 int Rtabmap::detectMoreLoopClosures(
-		float clusterRadius,
+		float clusterRadiusMax,
 		float clusterAngle,
 		int iterations,
 		bool intraSession,
 		bool interSession,
-		const ProgressState * processState)
+		const ProgressState * processState,
+		float clusterRadiusMin)
 {
 	UASSERT(iterations>0);
 
@@ -4698,11 +4699,11 @@ int Rtabmap::detectMoreLoopClosures(
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
-				n+1, iterations, clusterRadius, clusterAngle);
+				n+1, iterations, clusterRadiusMax, clusterAngle);
 
 		std::multimap<int, int> clusters = graph::radiusPosesClustering(
 				posesToCheckLoopClosures,
-				clusterRadius,
+				clusterRadiusMax,
 				clusterAngle);
 
 		UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
@@ -4750,138 +4751,148 @@ int Rtabmap::detectMoreLoopClosures(
 					   addedLinks.find(to) == addedLinks.end() &&
 					   rtabmap::graph::findLink(links, from, to) == links.end())
 					{
-						checkedLoopClosures.insert(std::make_pair(from, to));
-
-						UASSERT(signatures.find(from) != signatures.end());
-						UASSERT(signatures.find(to) != signatures.end());
-
-						Transform guess;
-						if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
+						// Reverify if in the bounds with the current optimized graph
+						Transform delta = poses.at(from).inverse() * poses.at(to);
+						if(delta.getNorm() < clusterRadiusMax &&
+						   delta.getNorm() >= clusterRadiusMin)
 						{
-							guess = poses.at(from).inverse() * poses.at(to);
-						}
+							checkedLoopClosures.insert(std::make_pair(from, to));
 
-						RegistrationInfo info;
-						// use signatures instead of IDs because some signatures may not be in WM
-						Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
+							UASSERT(signatures.find(from) != signatures.end());
+							UASSERT(signatures.find(to) != signatures.end());
 
-						if(!t.isNull())
-						{
-							bool updateConstraints = true;
-							if(_optimizationMaxError > 0.0f)
+							Transform guess;
+							if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
 							{
-								//optimize the graph to see if the new constraint is globally valid
-
-								int fromId = from;
-								int mapId = signatures.at(from).mapId();
-								// use first node of the map containing from
-								for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
-								{
-									if(ster->second.mapId() == mapId)
-									{
-										fromId = ster->first;
-										break;
-									}
-								}
-								std::multimap<int, Link> linksIn = links;
-								linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
-								const Link * maxLinearLink = 0;
-								const Link * maxAngularLink = 0;
-								float maxLinearError = 0.0f;
-								float maxAngularError = 0.0f;
-								float maxLinearErrorRatio = 0.0f;
-								float maxAngularErrorRatio = 0.0f;
-								std::map<int, Transform> optimizedPoses;
-								std::multimap<int, Link> links;
-								UASSERT(poses.find(fromId) != poses.end());
-								UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-								_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
-								UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
-								UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
-								UASSERT(graph::findLink(links, from, to) != links.end());
-								optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
-								std::string msg;
-								if(optimizedPoses.size())
-								{
-									graph::computeMaxGraphErrors(
-											optimizedPoses,
-											links,
-											maxLinearErrorRatio,
-											maxAngularErrorRatio,
-											maxLinearError,
-											maxAngularError,
-											&maxLinearLink,
-											&maxAngularLink);
-									if(maxLinearLink)
-									{
-										UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-										if(maxLinearErrorRatio > _optimizationMaxError)
-										{
-											msg = uFormat("Rejecting edge %d->%d because "
-													  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-													  "\"%s\" is %f.",
-													  from,
-													  to,
-													  maxLinearError,
-													  maxLinearLink->from(),
-													  maxLinearLink->to(),
-													  maxLinearErrorRatio,
-													  sqrt(maxLinearLink->transVariance()),
-													  Parameters::kRGBDOptimizeMaxError().c_str(),
-													  _optimizationMaxError);
-										}
-									}
-									else if(maxAngularLink)
-									{
-										UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-										if(maxAngularErrorRatio > _optimizationMaxError)
-										{
-											msg = uFormat("Rejecting edge %d->%d because "
-													  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
-													  "\"%s\" is %f m.",
-													  from,
-													  to,
-													  maxAngularError*180.0f/M_PI,
-													  maxAngularLink->from(),
-													  maxAngularLink->to(),
-													  maxAngularErrorRatio,
-													  sqrt(maxAngularLink->rotVariance()),
-													  Parameters::kRGBDOptimizeMaxError().c_str(),
-													  _optimizationMaxError);
-										}
-									}
-								}
-								else
-								{
-									msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
-											  from,
-											  to);
-								}
-								if(!msg.empty())
-								{
-									UWARN("%s", msg.c_str());
-									updateConstraints = false;
-								}
+								guess = poses.at(from).inverse() * poses.at(to);
 							}
 
-							if(updateConstraints)
-							{
-								addedLinks.insert(from);
-								addedLinks.insert(to);
-								cv::Mat inf = getInformation(info.covariance);
-								links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
-								loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
-								std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
-								UINFO(msg.c_str());
+							RegistrationInfo info;
+							// use signatures instead of IDs because some signatures may not be in WM
+							Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
 
-								if(processState)
+							if(!t.isNull())
+							{
+								bool updateConstraints = true;
+								if(_optimizationMaxError > 0.0f)
 								{
-									UINFO(msg.c_str());
-									if(!processState->callback(msg))
+									//optimize the graph to see if the new constraint is globally valid
+
+									int fromId = from;
+									int mapId = signatures.at(from).mapId();
+									// use first node of the map containing from
+									for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
 									{
-										return -1;
+										if(ster->second.mapId() == mapId)
+										{
+											fromId = ster->first;
+											break;
+										}
+									}
+									std::multimap<int, Link> linksIn = links;
+									linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
+									const Link * maxLinearLink = 0;
+									const Link * maxAngularLink = 0;
+									float maxLinearError = 0.0f;
+									float maxAngularError = 0.0f;
+									float maxLinearErrorRatio = 0.0f;
+									float maxAngularErrorRatio = 0.0f;
+									std::map<int, Transform> optimizedPoses;
+									std::multimap<int, Link> links;
+									UASSERT(poses.find(fromId) != poses.end());
+									UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
+									UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
+									_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
+									UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
+									UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
+									UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
+									UASSERT(graph::findLink(links, from, to) != links.end());
+									optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
+									std::string msg;
+									if(optimizedPoses.size())
+									{
+										graph::computeMaxGraphErrors(
+												optimizedPoses,
+												links,
+												maxLinearErrorRatio,
+												maxAngularErrorRatio,
+												maxLinearError,
+												maxAngularError,
+												&maxLinearLink,
+												&maxAngularLink);
+										if(maxLinearLink)
+										{
+											UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+											if(maxLinearErrorRatio > _optimizationMaxError)
+											{
+												msg = uFormat("Rejecting edge %d->%d because "
+														  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+														  "\"%s\" is %f.",
+														  from,
+														  to,
+														  maxLinearError,
+														  maxLinearLink->from(),
+														  maxLinearLink->to(),
+														  maxLinearErrorRatio,
+														  sqrt(maxLinearLink->transVariance()),
+														  Parameters::kRGBDOptimizeMaxError().c_str(),
+														  _optimizationMaxError);
+											}
+										}
+										else if(maxAngularLink)
+										{
+											UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+											if(maxAngularErrorRatio > _optimizationMaxError)
+											{
+												msg = uFormat("Rejecting edge %d->%d because "
+														  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+														  "\"%s\" is %f m.",
+														  from,
+														  to,
+														  maxAngularError*180.0f/M_PI,
+														  maxAngularLink->from(),
+														  maxAngularLink->to(),
+														  maxAngularErrorRatio,
+														  sqrt(maxAngularLink->rotVariance()),
+														  Parameters::kRGBDOptimizeMaxError().c_str(),
+														  _optimizationMaxError);
+											}
+										}
+									}
+									else
+									{
+										msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+												  from,
+												  to);
+									}
+									if(!msg.empty())
+									{
+										UWARN("%s", msg.c_str());
+										updateConstraints = false;
+									}
+									else
+									{
+										poses = optimizedPoses;
+									}
+								}
+
+								if(updateConstraints)
+								{
+									addedLinks.insert(from);
+									addedLinks.insert(to);
+									cv::Mat inf = getInformation(info.covariance);
+									links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
+									loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
+									std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
+									UINFO(msg.c_str());
+
+									if(processState)
+									{
+										UINFO(msg.c_str());
+										if(!processState->callback(msg))
+										{
+											return -1;
+										}
 									}
 								}
 							}

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -131,6 +131,7 @@ private Q_SLOTS:
 	void cancel();
 
 private:
+	std::map<int, Transform> filterNodes(const std::map<int, Transform> & poses);
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
 			const std::map<int, Transform> & poses,
 			const QMap<int, Signature> & cachedSignatures,

guilib/src/ExportCloudsDialog.cpp

@@ -110,6 +110,15 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
 
+	connect(_ui->checkBox_nodes_filtering, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_nodes_filtering, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
+	connect(_ui->doubleSpinBox_nodes_filtering_xmin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_nodes_filtering_xmax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_nodes_filtering_ymin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_nodes_filtering_ymax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_nodes_filtering_zmin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_nodes_filtering_zmax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+
 	connect(_ui->checkBox_regenerate, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_regenerate, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
@@ -325,6 +334,14 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value());
 	settings.setValue("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex());
 
+	settings.setValue("nodes_filtering", _ui->checkBox_nodes_filtering->isChecked());
+	settings.setValue("nodes_filtering_xmin", _ui->doubleSpinBox_nodes_filtering_xmin->value());
+	settings.setValue("nodes_filtering_xmax", _ui->doubleSpinBox_nodes_filtering_xmax->value());
+	settings.setValue("nodes_filtering_ymin", _ui->doubleSpinBox_nodes_filtering_ymin->value());
+	settings.setValue("nodes_filtering_ymax", _ui->doubleSpinBox_nodes_filtering_ymax->value());
+	settings.setValue("nodes_filtering_zmin", _ui->doubleSpinBox_nodes_filtering_zmin->value());
+	settings.setValue("nodes_filtering_zmax", _ui->doubleSpinBox_nodes_filtering_zmax->value());
+
 	settings.setValue("regenerate", _ui->checkBox_regenerate->isChecked());
 	settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
 	settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value());
@@ -474,6 +491,14 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
 	_ui->comboBox_intensityColormap->setCurrentIndex(settings.value("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex()).toInt());
 
+	_ui->checkBox_nodes_filtering->setChecked(settings.value("nodes_filtering", _ui->checkBox_nodes_filtering->isChecked()).toBool());
+	_ui->doubleSpinBox_nodes_filtering_xmin->setValue(settings.value("nodes_filtering_xmin", _ui->doubleSpinBox_nodes_filtering_xmin->value()).toInt());
+	_ui->doubleSpinBox_nodes_filtering_xmax->setValue(settings.value("nodes_filtering_xmax", _ui->doubleSpinBox_nodes_filtering_xmax->value()).toInt());
+	_ui->doubleSpinBox_nodes_filtering_ymin->setValue(settings.value("nodes_filtering_ymin", _ui->doubleSpinBox_nodes_filtering_ymin->value()).toInt());
+	_ui->doubleSpinBox_nodes_filtering_ymax->setValue(settings.value("nodes_filtering_ymax", _ui->doubleSpinBox_nodes_filtering_ymax->value()).toInt());
+	_ui->doubleSpinBox_nodes_filtering_zmin->setValue(settings.value("nodes_filtering_zmin", _ui->doubleSpinBox_nodes_filtering_zmin->value()).toInt());
+	_ui->doubleSpinBox_nodes_filtering_zmax->setValue(settings.value("nodes_filtering_zmax", _ui->doubleSpinBox_nodes_filtering_zmax->value()).toInt());
+
 	_ui->checkBox_regenerate->setChecked(settings.value("regenerate", _ui->checkBox_regenerate->isChecked()).toBool());
 	_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
 	_ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
@@ -626,6 +651,14 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
 	_ui->comboBox_intensityColormap->setCurrentIndex(0);
 
+	_ui->checkBox_nodes_filtering->setChecked(false);
+	_ui->doubleSpinBox_nodes_filtering_xmin->setValue(0);
+	_ui->doubleSpinBox_nodes_filtering_xmax->setValue(0);
+	_ui->doubleSpinBox_nodes_filtering_ymin->setValue(0);
+	_ui->doubleSpinBox_nodes_filtering_ymax->setValue(0);
+	_ui->doubleSpinBox_nodes_filtering_zmin->setValue(0);
+	_ui->doubleSpinBox_nodes_filtering_zmax->setValue(0);
+
 	_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
 	_ui->spinBox_decimation->setValue(1);
 	_ui->doubleSpinBox_maxDepth->setValue(4);
@@ -830,6 +863,7 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 	_ui->checkBox_cameraProjection->setEnabled(_ui->checkBox_assemble->isChecked() && !_ui->checkBox_meshing->isChecked());
 	_ui->label_cameraProjection->setEnabled(_ui->checkBox_cameraProjection->isEnabled());
 
+	_ui->groupBox_nodes_filtering->setVisible(_ui->checkBox_nodes_filtering->isChecked());
 	_ui->groupBox_regenerate->setVisible(_ui->checkBox_regenerate->isChecked() && _ui->checkBox_fromDepth->isChecked());
 	_ui->groupBox_regenerateScans->setVisible(_ui->checkBox_regenerate->isChecked() && !_ui->checkBox_fromDepth->isChecked());
 	_ui->groupBox_bilateral->setVisible(_ui->checkBox_bilateral->isChecked());
@@ -953,6 +987,42 @@ void ExportCloudsDialog::setOkButton()
 	updateReconstructionFlavor();
 }
 
+std::map<int, Transform> ExportCloudsDialog::filterNodes(const std::map<int, Transform> & poses)
+{
+	if(_ui->checkBox_nodes_filtering->isChecked())
+	{
+		std::map<int, Transform> posesFiltered;
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			bool ignore = false;
+			if(_ui->doubleSpinBox_nodes_filtering_xmin->value() != _ui->doubleSpinBox_nodes_filtering_xmax->value() &&
+				(iter->second.x() < _ui->doubleSpinBox_nodes_filtering_xmin->value() ||
+				 iter->second.x() > _ui->doubleSpinBox_nodes_filtering_xmax->value()))
+			{
+				ignore = true;
+			}
+			if(_ui->doubleSpinBox_nodes_filtering_ymin->value() != _ui->doubleSpinBox_nodes_filtering_ymax->value() &&
+				(iter->second.y() < _ui->doubleSpinBox_nodes_filtering_ymin->value() ||
+				 iter->second.y() > _ui->doubleSpinBox_nodes_filtering_ymax->value()))
+			{
+				ignore = true;
+			}
+			if(_ui->doubleSpinBox_nodes_filtering_zmin->value() != _ui->doubleSpinBox_nodes_filtering_zmax->value() &&
+				(iter->second.z() < _ui->doubleSpinBox_nodes_filtering_zmin->value() ||
+				 iter->second.z() > _ui->doubleSpinBox_nodes_filtering_zmax->value()))
+			{
+				ignore = true;
+			}
+			if(!ignore)
+			{
+				posesFiltered.insert(*iter);
+			}
+		}
+		return posesFiltered;
+	}
+	return poses;
+}
+
 void ExportCloudsDialog::exportClouds(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
@@ -1432,7 +1502,7 @@ bool ExportCloudsDialog::removeDirRecursively(const QString & dirName)
 }
 
 bool ExportCloudsDialog::getExportedClouds(
-		const std::map<int, Transform> & poses,
+		const std::map<int, Transform> & posesIn,
 		const std::multimap<int, Link> & links,
 		const std::map<int, int> & mapIds,
 		const QMap<int, Signature> & cachedSignatures,
@@ -1460,9 +1530,11 @@ bool ExportCloudsDialog::getExportedClouds(
 	}
 	if(this->exec() == QDialog::Accepted)
 	{
+		std::map<int, Transform> poses = filterNodes(posesIn);
+
 		if(poses.empty())
 		{
-			QMessageBox::critical(this, tr("Creating clouds..."), tr("Poses are null! Cannot export/view clouds."));
+			QMessageBox::critical(this, tr("Creating clouds..."), tr("Poses are empty! Cannot export/view clouds."));
 			return false;
 		}
 		_progressDialog->resetProgress();

guilib/src/MainWindow.cpp

@@ -1671,15 +1671,15 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		_ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
-	}
 
-	if(!odom.info().guessVelocity.isNull())
-	{
-		odom.info().guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-		dist = odom.info().guessVelocity.getNorm();
-		_ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*3.6f, _preferencesDialog->isCacheSavedInFigures());
-		_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*2.237f, _preferencesDialog->isCacheSavedInFigures());
-		_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures());
+		if(!odom.info().guess.isNull())
+		{
+			odom.info().guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+			dist = odom.info().guess.getNorm();
+			_ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
+			_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
+			_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
+		}
 	}
 
 	if(!odom.info().transformGroundTruth.isNull())
@@ -6029,186 +6029,195 @@ void MainWindow::postProcessing()
 						   addedLinks.find(to) == addedLinks.end() &&
 						   rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
 						{
-							checkedLoopClosures.insert(std::make_pair(from, to));
+							// Reverify if in the bounds with the current optimized graph
+							Transform delta = _currentPosesMap.at(from).inverse() * _currentPosesMap.at(to);
+							if(delta.getNorm() < clusterRadius)
+							{
+								checkedLoopClosures.insert(std::make_pair(from, to));
 
-							if(!_cachedSignatures.contains(from))
-							{
-								UERROR("Didn't find signature %d", from);
-							}
-							else if(!_cachedSignatures.contains(to))
-							{
-								UERROR("Didn't find signature %d", to);
-							}
-							else
-							{
-								Signature signatureFrom = _cachedSignatures[from];
-								Signature signatureTo = _cachedSignatures[to];
-
-								if(signatureFrom.getWeight() >= 0 &&
-								   signatureTo.getWeight() >= 0) // ignore intermediate nodes
+								if(!_cachedSignatures.contains(from))
 								{
-									Transform transform;
-									RegistrationInfo info;
-									if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
-										parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
-									{
-										uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
-									}
-									Registration * registration = Registration::create(parameters);
+									UERROR("Didn't find signature %d", from);
+								}
+								else if(!_cachedSignatures.contains(to))
+								{
+									UERROR("Didn't find signature %d", to);
+								}
+								else
+								{
+									Signature signatureFrom = _cachedSignatures[from];
+									Signature signatureTo = _cachedSignatures[to];
 
-									if(reextractFeatures)
+									if(signatureFrom.getWeight() >= 0 &&
+									   signatureTo.getWeight() >= 0) // ignore intermediate nodes
 									{
-										signatureFrom.sensorData().uncompressData();
-										signatureTo.sensorData().uncompressData();
+										Transform transform;
+										RegistrationInfo info;
+										if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
+											parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
+										{
+											uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
+										}
+										Registration * registration = Registration::create(parameters);
 
-										if(signatureFrom.sensorData().imageRaw().empty() &&
-										   signatureTo.sensorData().imageRaw().empty())
+										if(reextractFeatures)
 										{
-											UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
-													"images. Update the cache.",
-												Parameters::kRGBDLoopClosureReextractFeatures().c_str());
-										}
-										else
-										{
-											signatureFrom.removeAllWords();
-											signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-											signatureTo.removeAllWords();
-											signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-										}
-									}
-									else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
-									{
-										UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
-												"registration will not be possible. Set \"%s\" to true.",
-												Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
-												signatureFrom.id(),
-												signatureTo.id(),
-												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
-										bool updateConstraint = true;
-										cv::Mat information = info.covariance.inv();
-										if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
-										{
-											for(int i=0; i<6; ++i)
+											signatureFrom.sensorData().uncompressData();
+											signatureTo.sensorData().uncompressData();
+
+											if(signatureFrom.sensorData().imageRaw().empty() &&
+											   signatureTo.sensorData().imageRaw().empty())
 											{
-												if(information.at<double>(i,i) > odomMaxInf[i])
-												{
-													information.at<double>(i,i) = odomMaxInf[i];
-												}
-											}
-										}
-										if(optimizeMaxError > 0.0f && optimizeIterations > 0)
-										{
-											int fromId = from;
-											int mapId = _currentMapIds.at(from);
-											// use first node of the map containing from
-											for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
-											{
-												if(iter->second == mapId && _currentPosesMap.find(iter->first)!=_currentPosesMap.end())
-												{
-													fromId = iter->first;
-													break;
-												}
-											}
-											std::multimap<int, Link> linksIn = _currentLinksMap;
-											linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
-											const Link * maxLinearLink = 0;
-											const Link * maxAngularLink = 0;
-											float maxLinearError = 0.0f;
-											float maxAngularError = 0.0f;
-											std::map<int, Transform> poses;
-											std::multimap<int, Link> links;
-											UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
-											UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-											UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-											optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
-											UASSERT(poses.find(fromId) != poses.end());
-											UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-											UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-											UASSERT(graph::findLink(links, from, to) != links.end());
-											poses = optimizer->optimize(fromId, poses, links);
-											std::string msg;
-											if(poses.size())
-											{
-												float maxLinearErrorRatio = 0.0f;
-												float maxAngularErrorRatio = 0.0f;
-												graph::computeMaxGraphErrors(
-														poses,
-														links,
-														maxLinearErrorRatio,
-														maxAngularErrorRatio,
-														maxLinearError,
-														maxAngularError,
-														&maxLinearLink,
-														&maxAngularLink);
-												if(maxLinearLink)
-												{
-													UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-													if(maxLinearErrorRatio > optimizeMaxError)
-													{
-														msg = uFormat("Rejecting edge %d->%d because "
-																  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-																  "\"%s\" is %f.",
-																  from,
-																  to,
-																  maxLinearError,
-																  maxLinearLink->from(),
-																  maxLinearLink->to(),
-																  maxLinearErrorRatio,
-																  sqrt(maxLinearLink->transVariance()),
-																  Parameters::kRGBDOptimizeMaxError().c_str(),
-																  optimizeMaxError);
-													}
-												}
-												else if(maxAngularLink)
-												{
-													UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-													if(maxAngularErrorRatio > optimizeMaxError)
-													{
-														msg = uFormat("Rejecting edge %d->%d because "
-																  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
-																  "\"%s\" is %f m.",
-																  from,
-																  to,
-																  maxAngularError*180.0f/M_PI,
-																  maxAngularLink->from(),
-																  maxAngularLink->to(),
-																  maxAngularErrorRatio,
-																  sqrt(maxAngularLink->rotVariance()),
-																  Parameters::kRGBDOptimizeMaxError().c_str(),
-																  optimizeMaxError);
-													}
-												}
+												UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
+														"images. Update the cache.",
+													Parameters::kRGBDLoopClosureReextractFeatures().c_str());
 											}
 											else
 											{
-												msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
-														  from,
-														  to);
-											}
-											if(!msg.empty())
-											{
-												UWARN("%s", msg.c_str());
-												_progressDialog->appendText(tr("%1").arg(msg.c_str()));
-												QApplication::processEvents();
-												updateConstraint = false;
+												signatureFrom.removeAllWords();
+												signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+												signatureTo.removeAllWords();
+												signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 											}
 										}
-
-										if(updateConstraint)
+										else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
 										{
-											UINFO("Added new loop closure between %d and %d.", from, to);
-											addedLinks.insert(from);
-											addedLinks.insert(to);
+											UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
+													"registration will not be possible. Set \"%s\" to true.",
+													Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
+													signatureFrom.id(),
+													signatureTo.id(),
+													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
+											bool updateConstraint = true;
+											cv::Mat information = info.covariance.inv();
+											if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
+											{
+												for(int i=0; i<6; ++i)
+												{
+													if(information.at<double>(i,i) > odomMaxInf[i])
+													{
+														information.at<double>(i,i) = odomMaxInf[i];
+													}
+												}
+											}
+											if(optimizeMaxError > 0.0f && optimizeIterations > 0)
+											{
+												int fromId = from;
+												int mapId = _currentMapIds.at(from);
+												// use first node of the map containing from
+												for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
+												{
+													if(iter->second == mapId && _currentPosesMap.find(iter->first)!=_currentPosesMap.end())
+													{
+														fromId = iter->first;
+														break;
+													}
+												}
+												std::multimap<int, Link> linksIn = _currentLinksMap;
+												linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
+												const Link * maxLinearLink = 0;
+												const Link * maxAngularLink = 0;
+												float maxLinearError = 0.0f;
+												float maxAngularError = 0.0f;
+												std::map<int, Transform> poses;
+												std::multimap<int, Link> links;
+												UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
+												UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
+												UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
+												optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
+												UASSERT(poses.find(fromId) != poses.end());
+												UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
+												UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
+												UASSERT(graph::findLink(links, from, to) != links.end());
+												poses = optimizer->optimize(fromId, poses, links);
+												std::string msg;
+												if(poses.size())
+												{
+													float maxLinearErrorRatio = 0.0f;
+													float maxAngularErrorRatio = 0.0f;
+													graph::computeMaxGraphErrors(
+															poses,
+															links,
+															maxLinearErrorRatio,
+															maxAngularErrorRatio,
+															maxLinearError,
+															maxAngularError,
+															&maxLinearLink,
+															&maxAngularLink);
+													if(maxLinearLink)
+													{
+														UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+														if(maxLinearErrorRatio > optimizeMaxError)
+														{
+															msg = uFormat("Rejecting edge %d->%d because "
+																	  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+																	  "\"%s\" is %f.",
+																	  from,
+																	  to,
+																	  maxLinearError,
+																	  maxLinearLink->from(),
+																	  maxLinearLink->to(),
+																	  maxLinearErrorRatio,
+																	  sqrt(maxLinearLink->transVariance()),
+																	  Parameters::kRGBDOptimizeMaxError().c_str(),
+																	  optimizeMaxError);
+														}
+													}
+													else if(maxAngularLink)
+													{
+														UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+														if(maxAngularErrorRatio > optimizeMaxError)
+														{
+															msg = uFormat("Rejecting edge %d->%d because "
+																	  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+																	  "\"%s\" is %f m.",
+																	  from,
+																	  to,
+																	  maxAngularError*180.0f/M_PI,
+																	  maxAngularLink->from(),
+																	  maxAngularLink->to(),
+																	  maxAngularErrorRatio,
+																	  sqrt(maxAngularLink->rotVariance()),
+																	  Parameters::kRGBDOptimizeMaxError().c_str(),
+																	  optimizeMaxError);
+														}
+													}
+												}
+												else
+												{
+													msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+															  from,
+															  to);
+												}
+												if(!msg.empty())
+												{
+													UWARN("%s", msg.c_str());
+													_progressDialog->appendText(tr("%1").arg(msg.c_str()));
+													QApplication::processEvents();
+													updateConstraint = false;
+												}
+												else
+												{
+													_currentPosesMap = poses;
+												}
+											}
 
-											_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
-											++loopClosuresAdded;
-											_progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
+											if(updateConstraint)
+											{
+												UINFO("Added new loop closure between %d and %d.", from, to);
+												addedLinks.insert(from);
+												addedLinks.insert(to);
+
+												_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
+												++loopClosuresAdded;
+												_progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
+											}
 										}
 									}
 								}

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-584</y>
+        <y>-148</y>
         <width>780</width>
-        <height>5347</height>
+        <height>5569</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -52,21 +52,21 @@
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
+         <item row="11" column="0">
           <widget class="QCheckBox" name="checkBox_regenerate">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="11" column="0">
+         <item row="12" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="10" column="1">
+         <item row="11" column="1">
           <widget class="QLabel" name="label_regenerate">
            <property name="text">
             <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
@@ -116,14 +116,14 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="0">
+         <item row="13" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="11" column="1">
+         <item row="12" column="1">
           <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
             <string>Cloud filtering. Remove sparse points that are far from surfaces.</string>
@@ -143,7 +143,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="1">
+         <item row="13" column="1">
           <widget class="QLabel" name="label_smoothing">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -289,7 +289,7 @@
            </item>
           </widget>
          </item>
-         <item row="13" column="1">
+         <item row="14" column="1">
           <widget class="QLabel" name="label_gainCompensation">
            <property name="text">
             <string>Gain compensation. Normalize brightness of images.</string>
@@ -299,7 +299,7 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="1">
+         <item row="15" column="1">
           <widget class="QLabel" name="label_cameraProjection">
            <property name="text">
             <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
@@ -309,21 +309,21 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="0">
+         <item row="14" column="0">
           <widget class="QCheckBox" name="checkBox_gainCompensation">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="14" column="0">
+         <item row="15" column="0">
           <widget class="QCheckBox" name="checkBox_cameraProjection">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="15" column="1">
+         <item row="16" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -333,15 +333,236 @@
            </property>
           </widget>
          </item>
-         <item row="15" column="0">
+         <item row="16" column="0">
           <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
+         <item row="10" column="1">
+          <widget class="QLabel" name="label_binaryFile_10">
+           <property name="text">
+            <string>Nodes filtering. Filter nodes to be exported in a specified region .</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="10" column="0">
+          <widget class="QCheckBox" name="checkBox_nodes_filtering">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
         </layout>
        </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_nodes_filtering">
+         <property name="title">
+          <string>Nodes Filtering</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_19">
+          <item>
+           <widget class="QLabel" name="label_114">
+            <property name="text">
+             <string>Set min and max range values on each axis. Nodes inside those ranges will be exported. If the min and max are equals, there is no filtering on that axis. For example for 2D map, set Z min and Z max to 0. To filter just in altitude, set only Z ranges.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <layout class="QGridLayout" name="gridLayout_21">
+            <item row="0" column="2">
+             <widget class="QLabel" name="label_113">
+              <property name="text">
+               <string>X max</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <widget class="QLabel" name="label_142">
+              <property name="toolTip">
+               <string>Length along X-Axis</string>
+              </property>
+              <property name="text">
+               <string>Y min</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_ymin">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>0</number>
+              </property>
+              <property name="minimum">
+               <double>-9999999.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>9999999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="2">
+             <widget class="QLabel" name="label_115">
+              <property name="toolTip">
+               <string>Width along Y-Axis</string>
+              </property>
+              <property name="text">
+               <string>Y max</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QLabel" name="label_116">
+              <property name="toolTip">
+               <string>Height along Z-Axis</string>
+              </property>
+              <property name="text">
+               <string>Z min</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="2">
+             <widget class="QLabel" name="label_119">
+              <property name="toolTip">
+               <string>Width along Y-Axis</string>
+              </property>
+              <property name="text">
+               <string>Z max</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="0">
+             <widget class="QLabel" name="label_111">
+              <property name="text">
+               <string>X min</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="4">
+             <spacer name="horizontalSpacer">
+              <property name="orientation">
+               <enum>Qt::Horizontal</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>40</width>
+                <height>20</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+            <item row="0" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_xmin">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>0</number>
+              </property>
+              <property name="minimum">
+               <double>-9999999.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>9999999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="3">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_xmax">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>0</number>
+              </property>
+              <property name="minimum">
+               <double>-9999999.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>9999999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="1">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_zmin">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>0</number>
+              </property>
+              <property name="minimum">
+               <double>-9999999.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>9999999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="3">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_ymax">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>0</number>
+              </property>
+              <property name="minimum">
+               <double>-9999999.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>9999999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="3">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_zmax">
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>0</number>
+              </property>
+              <property name="minimum">
+               <double>-9999999.000000000000000</double>
+              </property>
+              <property name="maximum">
+               <double>9999999.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
+         </layout>
+        </widget>
+       </item>
        <item>
         <widget class="QGroupBox" name="groupBox_regenerateScans">
          <property name="title">

tools/DetectMoreLoopClosures/main.cpp

@@ -51,6 +51,7 @@ void showUsage()
 			"rtabmap-detectMoreLoopClosures [options] database.db\n"
 			"Options:\n"
 			"    -r #          Cluster radius (default 1 m).\n"
+			"    -rx #         Cluster radius min (default 0 m).\n"
 			"    -a #          Cluster angle (default 30 deg).\n"
 			"    -i #          Iterations (default 1).\n"
 			"    --intra       Add only intra-session loop closures.\n"
@@ -92,7 +93,8 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
-	float clusterRadius = 1.0f;
+	float clusterRadiusMin = 0.0f;
+	float clusterRadiusMax = 1.0f;
 	float clusterAngle = CV_PI/6.0f;
 	int iterations = 1;
 	bool intraSession = false;
@@ -124,7 +126,19 @@ int main(int argc, char * argv[])
 			++i;
 			if(i<argc-1)
 			{
-				clusterRadius = uStr2Float(argv[i]);
+				clusterRadiusMax = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "-rx") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				clusterRadiusMin = uStr2Float(argv[i]);
 			}
 			else
 			{
@@ -165,7 +179,8 @@ int main(int argc, char * argv[])
 	}
 
 	printf("\nDatabase: %s\n", dbPath.c_str());
-	printf("Cluster radius = %f m\n", clusterRadius);
+	printf("Cluster radius min = %f m\n", clusterRadiusMin);
+	printf("Cluster radius max = %f m\n", clusterRadiusMax);
 	printf("Cluster angle = %f deg\n", clusterAngle*180.0f/CV_PI);
 	if(intraSession)
 	{
@@ -204,7 +219,7 @@ int main(int argc, char * argv[])
 
 	PrintProgressState progress;
 	printf("Detecting...\n");
-	int detected = rtabmap.detectMoreLoopClosures(clusterRadius, clusterAngle, iterations, intraSession, interSession, &progress);
+	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin);
 	if(detected < 0)
 	{
 		if(!g_loopForever)

tools/Export/main.cpp

@@ -83,6 +83,12 @@ void showUsage()
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
+			"    --xmin #              Minimum range on X axis to keep nodes to export.\n"
+			"    --xmax #              Maximum range on X axis to keep nodes to export.\n"
+			"    --ymin #              Minimum range on Y axis to keep nodes to export.\n"
+			"    --ymax #              Maximum range on Y axis to keep nodes to export.\n"
+			"    --zmin #              Minimum range on Z axis to keep nodes to export.\n"
+			"    --zmax #              Maximum range on Z axis to keep nodes to export.\n"
 			"\n%s", Parameters::showUsage());
 	;
 	exit(1);
@@ -125,6 +131,7 @@ int main(int argc, char * argv[])
 	bool camProjection = false;
 	bool exportPoses = false;
 	bool exportImages = false;
+	cv::Vec3f min, max;
 	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -346,6 +353,78 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--xmin") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				min[0] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--xmax") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				max[0] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--ymin") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				min[1] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--ymax") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				max[1] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--zmin") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				min[2] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--zmax") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				max[2] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 	}
 
 	if(decimation < 1)
@@ -432,6 +511,43 @@ int main(int argc, char * argv[])
 		return -1;
 	}
 
+	if(min[0] != max[0] || min[1] != max[1] || min[2] != max[2])
+	{
+		cv::Vec3f minP,maxP;
+		graph::computeMinMax(optimizedPoses, minP, maxP);
+		printf("Filtering poses (range: x=%f<->%f, y=%f<->%f, z=%f<->%f, map size=%f x %f x %f)...\n",
+				min[0],max[0],min[1],max[1],min[2],max[2],
+				maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
+		std::map<int, Transform> posesFiltered;
+		for(std::map<int, Transform>::const_iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+		{
+			bool ignore = false;
+			if(min[0] != max[0] && (iter->second.x() < min[0] || iter->second.x() > max[0]))
+			{
+				ignore = true;
+			}
+			if(min[1] != max[1] && (iter->second.y() < min[1] || iter->second.y() > max[1]))
+			{
+				ignore = true;
+			}
+			if(min[2] != max[2] && (iter->second.z() < min[2] || iter->second.z() > max[2]))
+			{
+				ignore = true;
+			}
+			if(!ignore)
+			{
+				posesFiltered.insert(*iter);
+			}
+		}
+		graph::computeMinMax(posesFiltered, minP, maxP);
+		printf("Filtering poses... done! %d/%d remaining (new map size=%f x %f x %f).\n", (int)posesFiltered.size(), (int)optimizedPoses.size(), maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
+		optimizedPoses = posesFiltered;
+		if(optimizedPoses.empty())
+		{
+			return -1;
+		}
+	}
+
 	std::string outputDirectory = UDirectory::getDir(dbPath);
 	std::string baseName = uSplit(UFile::getName(dbPath), '.').front();
 

tools/Info/main.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/VisualWord.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UStl.h"
@@ -231,6 +232,11 @@ int main(int argc, char * argv[])
 		driver->getAllNodeIds(ids);
 		Transform lastLocalization;
 		std::map<int, Transform> optimizedPoses = driver->loadOptimizedPoses(&lastLocalization);
+		cv::Vec3f min, max;
+		if(!optimizedPoses.empty())
+		{
+			graph::computeMinMax(optimizedPoses, min, max);
+		}
 		std::multimap<int, int> mapIdsLinkedToLastGraph;
 		int lastMapId=0;
 		double previousStamp = 0.0f;
@@ -353,7 +359,7 @@ int main(int argc, char * argv[])
 		std::cout << (uFormat("%s%d nodes and %d words (dim=%d type=%s)\n", pad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize(), wordsDim, wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
 		std::cout << (uFormat("%s%d nodes and %d words\n", pad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
 		std::cout << (uFormat("%s%d poses and %d links\n", pad("Global graph:").c_str(), odomPoses, links.size()));
-		std::cout << (uFormat("%s%d poses\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size()));
+		std::cout << (uFormat("%s%d poses (x=%d->%d, y=%d->%d, z=%d->%d)\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size(), (int)min[0], (int)max[0], (int)min[1], (int)max[1], min[2], (int)max[2]));
 		std::cout << (uFormat("%s%d/%d [%s]\n", pad("Maps in graph:").c_str(), (int)mapsLinkedToLastGraph.size(), sessions, sessionsInOptGraphStr.str().c_str()));
 		std::cout << (uFormat("%s%d poses\n", pad("Ground truth:").c_str(), gtPoses));
 		std::cout << (uFormat("%s%d poses\n", pad("GPS:").c_str(), gpsValues));