Added Parameters::parseArguments() and Parameters::showUsage() functions for convenience. Updated OdometryViewer and Console tools to use parseArguments().

corelib/include/rtabmap/core/Parameters.h

@@ -419,7 +419,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
 	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
 	RTABMAP_PARAM(Icp, Epsilon,                   float, 0.0,   "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
-	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.3,   "Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.2,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
 
@@ -476,6 +476,9 @@ public:
 	static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
 	static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
 
+	static const char * showUsage();
+	static ParametersMap parseArguments(int argc, char * argv[]);
+
 	static std::string getVersion();
 	static std::string getDefaultDatabaseName();
 	

corelib/src/CameraThread.cpp

@@ -161,7 +161,7 @@ void CameraThread::mainLoop()
 			data.setDepthOrRightRaw(depth);
 			data.setStereoCameraModel(StereoCameraModel());
 			info.timeDisparity = timer.ticks();
-			UINFO("Computing disparity = %f s", info.timeDisparity);
+			UDEBUG("Computing disparity = %f s", info.timeDisparity);
 		}
 		if(_scanFromDepth &&
 			data.cameraModels().size() &&
@@ -200,7 +200,7 @@ void CameraThread::mainLoop()
 				}
 				data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
 				info.timeScanFromDepth = timer.ticks();
-				UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
+				UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
 			}
 			else
 			{

corelib/src/Memory.cpp

@@ -3052,6 +3052,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 
 		to->setPose(from->getPose());
 		to->setWords3(from->getWords3());
+		to->setWordsDescriptors(from->getWordsDescriptors());
 	}
 	else
 	{

corelib/src/OdometryF2M.cpp

@@ -253,7 +253,7 @@ Transform OdometryF2M::computeTransform(
 					if(regPipeline_->isImageRequired() &&
 						(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
 					{
-						UDEBUG("Update local map");
+						UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
 
 						// update local map
 						UASSERT(mapWords.size() == mapPoints.size());
@@ -326,7 +326,7 @@ Transform OdometryF2M::computeTransform(
 					if(regPipeline_->isScanRequired() &&
 						(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
 					{
-						UINFO("Update local scan map %d", lastFrame_->id());
+						UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
 						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
 						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
 
@@ -386,8 +386,6 @@ Transform OdometryF2M::computeTransform(
 						map_->setWords(mapWords);
 						map_->setWords3(mapPoints);
 						map_->setWordsDescriptors(mapDescriptors);
-
-						UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size());
 					}
 				}
 				else

corelib/src/Parameters.cpp

@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cmath>
 #include <stdlib.h>
 #include <sstream>
+#include <iostream>
+#include <iomanip>
 #include "SimpleIni.h"
 
 namespace rtabmap
@@ -384,6 +386,190 @@ void Parameters::parse(const ParametersMap & parameters, ParametersMap & paramet
 	}
 }
 
+const char * Parameters::showUsage()
+{
+	return  "Logger options:\n"
+			"   --nolog              Disable logger\n"
+			"   --logconsole         Set logger console type\n"
+			"   --logfile \"path\"     Set logger file type\n"
+			"   --logfilea \"path\"    Set logger file type with appending mode if the file already exists\n"
+			"   --udebug             Set logger level to debug\n"
+			"   --uinfo              Set logger level to info\n"
+			"   --uwarn              Set logger level to warn\n"
+			"   --uerror             Set logger level to error\n"
+			"   --logtime \"bool\"     Print time when logging\n"
+			"   --logwhere \"bool\"    Print where when logging\n"
+			"   --logthread \"bool\"   Print thread id when logging\n"
+			"RTAB-Map options:\n"
+			"   --params                       Show all parameters with their default value and description\n"
+			"   --\"parameter name\" \"value\"     Overwrite a specific RTAB-Map's parameter :\n"
+			"                                    --SURF/HessianThreshold 150\n"
+			"                                   For parameters in table format, add ',' between values :\n"
+			"                                    --Kp/RoiRatios 0,0,0.1,0\n"
+			;
+}
+
+ParametersMap Parameters::parseArguments(int argc, char * argv[])
+{
+	ParametersMap out;
+	const ParametersMap & parameters = getDefaultParameters();
+	const std::map<std::string, std::pair<bool, std::string> > & removedParams = getRemovedParameters();
+	for(int i=0;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--nolog") == 0)
+		{
+			ULogger::setType(ULogger::kTypeNoLog);
+		}
+		else if(strcmp(argv[i], "--logconsole") == 0)
+		{
+			ULogger::setType(ULogger::kTypeConsole);
+		}
+		else if(strcmp(argv[i], "--logfile") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				ULogger::setType(ULogger::kTypeFile, argv[i], false);
+			}
+			else
+			{
+				UERROR("\"--logfile\" argument requires following file path");
+			}
+		}
+		else if(strcmp(argv[i], "--logfilea") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				ULogger::setType(ULogger::kTypeFile, argv[i], true);
+			}
+			else
+			{
+				UERROR("\"--logfilea\" argument requires following file path");
+			}
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		else if(strcmp(argv[i], "--uwarn") == 0)
+		{
+			ULogger::setLevel(ULogger::kWarning);
+		}
+		else if(strcmp(argv[i], "--uerror") == 0)
+		{
+			ULogger::setLevel(ULogger::kError);
+		}
+		else if(strcmp(argv[i], "--ulogtime") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				ULogger::setPrintTime(uStr2Bool(argv[i]));
+			}
+			else
+			{
+				UERROR("\"--ulogtime\" argument requires a following boolean value");
+			}
+		}
+		else if(strcmp(argv[i], "--ulogwhere") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				ULogger::setPrintWhere(uStr2Bool(argv[i]));
+			}
+			else
+			{
+				UERROR("\"--ulogwhere\" argument requires a following boolean value");
+			}
+		}
+		else if(strcmp(argv[i], "--ulogthread") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				ULogger::setPrintThreadId(uStr2Bool(argv[i]));
+			}
+			else
+			{
+				UERROR("\"--ulogthread\" argument requires a following boolean value");
+			}
+		}
+		else if(strcmp(argv[i], "--params") == 0)
+		{
+			for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			UWARN("App will now exit after showing default RTAB-Map parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else // check for parameters
+		{
+			std::string key = uReplaceChar(argv[i], '-', "");
+			ParametersMap::const_iterator iter = parameters.find(key);
+			if(iter != parameters.end())
+			{
+				++i;
+				if(i < argc)
+				{
+					uInsert(out, ParametersPair(iter->first, argv[i]));
+				}
+			}
+			else
+			{
+				// backward compatibility
+				std::map<std::string, std::pair<bool, std::string> >::const_iterator jter = removedParams.find(key);
+				if(jter!=removedParams.end())
+				{
+					if(jter->second.first)
+					{
+						++i;
+						if(i < argc)
+						{
+							std::string value = argv[i];
+							if(!value.empty())
+							{
+								value = uReplaceChar(value, ',', ' '); // for table
+								key = jter->second.second;
+								UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
+										jter->first.c_str(), jter->second.second.c_str(), value.c_str());
+								uInsert(out, ParametersPair(key, value));
+							}
+						}
+						else
+						{
+							UERROR("Value missing for argument \"%s\"", argv[i-1]);
+						}
+					}
+					else if(jter->second.second.empty())
+					{
+						UERROR("Parameter \"%s\" doesn't exist anymore.", jter->first.c_str());
+					}
+					else
+					{
+						UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", jter->first.c_str(), jter->second.second.c_str());
+					}
+				}
+			}
+		}
+	}
+	return out;
+}
+
 
 void Parameters::readINI(const std::string & configFile, ParametersMap & parameters)
 {

corelib/src/RegistrationIcp.cpp

@@ -180,6 +180,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
 				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
 				UDEBUG("Conversion time = %f s", timer.ticks());
+
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
 				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
 				bool filtered = false;

corelib/src/Rtabmap.cpp

@@ -377,10 +377,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	ULOGGER_DEBUG("");
 	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
-	{
-		this->setWorkingDirectory(iter->second.c_str());
-	}
 
 	Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
@@ -488,6 +484,12 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	{
 		_bayesFilter->parseParameters(parameters);
 	}
+
+	// update working directory at the end
+	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
+	{
+		this->setWorkingDirectory(iter->second.c_str());
+	}
 }
 
 int Rtabmap::getLastLocationId() const

corelib/src/util3d.cpp

@@ -291,9 +291,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 			}
 		}
 	}
+
 	if(validIndices)
 	{
-		validIndices->reserve(oi);
+		validIndices->resize(oi);
 	}
 
 	return cloud;

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class QSpinBox;
 class QDoubleSpinBox;
 class QLabel;
+class QCheckBox;
 
 namespace rtabmap {
 
@@ -77,6 +78,9 @@ private:
 	QDoubleSpinBox * voxelSpin_;
 	QSpinBox * decimationSpin_;
 	QDoubleSpinBox * maxDepthSpin_;
+	QCheckBox * cloudShown_;
+	QCheckBox * scanShown_;
+	QCheckBox * featuresShown_;
 	QLabel * timeLabel_;
 	int validDecimationValue_;
 };

guilib/src/OdometryViewer.cpp

@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QPushButton>
 #include <QSpinBox>
 #include <QDoubleSpinBox>
+#include <QCheckBox>
 #include <QLabel>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
@@ -96,6 +97,15 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
 	decimationSpin_->setMinimum(1);
 	decimationSpin_->setMaximum(16);
 	decimationSpin_->setValue(decimation);
+	cloudShown_ = new QCheckBox(this);
+	cloudShown_->setText("Cloud");
+	cloudShown_->setChecked(true);
+	scanShown_ = new QCheckBox(this);
+	scanShown_->setText("Scan");
+	scanShown_->setChecked(true);
+	featuresShown_ = new QCheckBox(this);
+	featuresShown_->setText("Features");
+	featuresShown_->setChecked(true);
 	timeLabel_ = new QLabel(this);
 	QPushButton * resetButton = new QPushButton("reset", this);
 	QPushButton * clearButton = new QPushButton("clear", this);
@@ -121,6 +131,9 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
 	hlayout2->addWidget(maxDepthSpin_);
 	hlayout2->addWidget(decimationLabel);
 	hlayout2->addWidget(decimationSpin_);
+	hlayout2->addWidget(cloudShown_);
+	hlayout2->addWidget(scanShown_);
+	hlayout2->addWidget(featuresShown_);
 	hlayout2->addWidget(timeLabel_);
 	hlayout2->addStretch(1);
 	hlayout2->addWidget(resetButton);
@@ -190,7 +203,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 
 	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
 
-	if(!odom.data().imageRaw().empty() &&
+	if(cloudShown_->isChecked() &&
+		!odom.data().imageRaw().empty() &&
 		!odom.data().depthOrRightRaw().empty() &&
 		(odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
 	{
@@ -253,6 +267,14 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 			}
 		}
 	}
+	else if(!cloudShown_->isChecked())
+	{
+		while(!addedClouds_.empty())
+		{
+			UASSERT(cloudView_->removeCloud(addedClouds_.first()));
+			addedClouds_.pop_front();
+		}
+	}
 
 	if(!odom.pose().isNull())
 	{
@@ -260,18 +282,83 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 		cloudView_->updateCameraTargetPosition(odom.pose());
 	}
 
-	if(odom.info().localMap.size())
+	if(scanShown_->isChecked())
 	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		// scan local map
-		cloud->resize(odom.info().localMap.size());
+		if(!odom.info().localScanMap.empty())
-		int i=0;
-		for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
 		{
-			(*cloud)[i].x = iter->second.x;
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-			(*cloud)[i].y = iter->second.y;
+			cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
-			(*cloud)[i++].z = iter->second.z;
+			if(!cloudView_->addCloud("scanMapOdom", cloud, Transform::getIdentity(), Qt::blue))
+			{
+				UERROR("Adding scanMapOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("scanMapOdom", true);
+				cloudView_->setCloudOpacity("scanMapOdom", 0.5);
+			}
 		}
-		cloudView_->addCloud("localmap", cloud);
+		// scan cloud
+		if(!odom.data().laserScanRaw().empty())
+		{
+			cv::Mat scan = odom.data().laserScanRaw();
+
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose());
+
+			if(!cloudView_->addCloud("scanOdom", cloud, Transform::getIdentity(), Qt::magenta))
+			{
+				UERROR("Adding scanOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("scanOdom", true);
+				cloudView_->setCloudOpacity("scanOdom", 0.5);
+			}
+		}
+	}
+	else
+	{
+		cloudView_->removeCloud("scanMapOdom");
+		cloudView_->removeCloud("scanOdom");
+	}
+
+	// 3d features
+	if(featuresShown_->isChecked())
+	{
+		if(!odom.info().localMap.empty())
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			cloud->resize(odom.info().localMap.size());
+			int i=0;
+			for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
+			{
+				(*cloud)[i].x = iter->second.x;
+				(*cloud)[i].y = iter->second.y;
+				(*cloud)[i].z = iter->second.z;
+
+				// green = inlier, yellow = outliers
+				bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
+				(*cloud)[i].r = inlier?0:255;
+				(*cloud)[i].g = 255;
+				(*cloud)[i++].b = 0;
+			}
+
+			if(!cloudView_->addCloud("featuresOdom", cloud))
+			{
+				UERROR("Adding featuresOdom to viewer failed!");
+			}
+			else
+			{
+				cloudView_->setCloudVisibility("featuresOdom", true);
+				cloudView_->setCloudPointSize("featuresOdom", 3);
+			}
+		}
+	}
+	else
+	{
+		cloudView_->removeCloud("featuresOdom");
 	}
 
 	if(!odom.data().imageRaw().empty())

guilib/src/PreferencesDialog.cpp

@@ -1276,8 +1276,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->groupBox_scanFromDepth->setChecked(false);
 		_ui->spinBox_cameraScanFromDepth_decimation->setValue(8);
 		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
-		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
+		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.025f);
-		_ui->spinBox_cameraImages_scanNormalsK->setValue(0);
+		_ui->spinBox_cameraImages_scanNormalsK->setValue(20);
 
 		_ui->groupBox_depthFromScan->setChecked(false);
 		_ui->groupBox_depthFromScan_fillHoles->setChecked(true);

tools/ConsoleApp/main.cpp

@@ -55,17 +55,10 @@ void showUsage()
 			"  -createGT                       Generate a ground truth file\n"
 			"  -start_at #                     When \"path\" is a directory of images, set this parameter\n"
 			"                                   to start processing at image # (default 1).\n"
-			"  -\"parameter name\" \"value\"       Overwrite a specific RTAB-Map's parameter :\n"
-			"                                     -SURF/HessianThreshold 150\n"
-			"                                   For parameters in table format, add ',' between values :\n"
-			"                                     -Kp/RoiRatios 0,0,0.1,0\n"
-			"  -default_params                 Show default RTAB-Map's parameters\n"
-			"  -debug                          Set Log level to Debug (Default Error)\n"
-			"  -info                           Set Log level to Info (Default Error)\n"
-			"  -warn                           Set Log level to Warning (Default Error)\n"
-			"  -log_console                    Log to console\n"
 			"  -v                              Get version of RTAB-Map\n"
-			"  -input \"path\"                 Load previous database if it exists.\n");
+			"  -input \"path\"                 Load previous database if it exists.\n"
+			"%s\n",
+			rtabmap::Parameters::showUsage());
 	exit(1);
 }
 
@@ -83,11 +76,6 @@ int main(int argc, char * argv[])
 	signal(SIGTERM, &sighandler);
 	signal(SIGINT, &sighandler);
 
-	/*for(int i=0; i<argc; i++)
-	{
-		printf("argv[%d] = %s\n", i, argv[i]);
-	}*/
-	const ParametersMap & defaultParameters = Parameters::getDefaultParameters();
 	if(argc < 2)
 	{
 		showUsage();
@@ -97,14 +85,6 @@ int main(int argc, char * argv[])
 		printf("%s\n", Parameters::getVersion().c_str());
 		exit(0);
 	}
-	else if(argc == 2 && (strcmp(argv[1], "-default_params") == 0 || strcmp(argv[1], "--params") == 0))
-	{
-		for(ParametersMap::const_iterator iter = defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
-		{
-			printf("%s=%s\n", iter->first.c_str(), iter->second.c_str());
-		}
-		exit(0);
-	}
 	printf("\n");
 
 	std::string path;
@@ -114,9 +94,8 @@ int main(int argc, char * argv[])
 	bool createGT = false;
 	std::string inputDbPath;
 	int startAt = 1;
-	ParametersMap pm;
+	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::Level logLevel = ULogger::kError;
+	ULogger::setLevel(ULogger::kWarning);
-	bool logConsole = false;
 
 	for(int i=1; i<argc; ++i)
 	{
@@ -222,123 +201,21 @@ int main(int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-debug") == 0)
+		if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
 		{
-			logLevel = ULogger::kDebug;
+			showUsage();
-			continue;
 		}
-		if(strcmp(argv[i], "-info") == 0)
-		{
-			logLevel = ULogger::kInfo;
-			continue;
-		}
-		if(strcmp(argv[i], "-warn") == 0)
-		{
-			logLevel = ULogger::kWarning;
-			continue;
-		}
-		if(strcmp(argv[i], "-log_console") == 0)
-		{
-			logConsole = true;
-			continue;
-		}
-
-		// Check for RTAB-Map's parameters
-		std::string key = argv[i];
-		key = uSplit(key, '-').back();
-		if(defaultParameters.find(key) != defaultParameters.end())
-		{
-			++i;
-			if(i < argc)
-			{
-				std::string value = argv[i];
-				if(value.empty())
-				{
-					showUsage();
-				}
-				else
-				{
-					value = uReplaceChar(value, ',', ' ');
-				}
-				std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
-				if(inserted.second == false)
-				{
-					inserted.first->second = value;
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-
-		//backward compatibility
-		// look for old parameter name
-		std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
-		if(oldIter!=Parameters::getRemovedParameters().end())
-		{
-			++i;
-			if(i < argc)
-			{
-				std::string value = argv[i];
-				if(value.empty())
-				{
-					showUsage();
-				}
-				else
-				{
-					value = uReplaceChar(value, ',', ' ');
-				}
-
-				if(oldIter->second.first)
-				{
-					key = oldIter->second.second;
-					UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
-							oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
-				}
-				else if(oldIter->second.second.empty())
-				{
-					UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
-				}
-				else
-				{
-					UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
-				}
-				if(oldIter->second.first)
-				{
-					std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
-					if(inserted.second == false)
-					{
-						inserted.first->second = value;
-					}
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-
-		printf("Unrecognized option : %s\n", argv[i]);
-		showUsage();
 	}
 
+	ParametersMap pm = Parameters::parseArguments(argc, argv);
+	pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), "."));
+
 	if(repeat && createGT)
 	{
 		printf("Cannot create a Ground truth if repeat is on.\n");
 		showUsage();
 	}
 
-	if(logConsole)
-	{
-		ULogger::setType(ULogger::kTypeConsole);
-	}
-	//ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false);
-	//ULogger::setBuffered(true);
-	ULogger::setLevel(logLevel);
-
 	UTimer timer;
 	timer.start();
 	std::queue<double> iterationMeanTime;
@@ -376,13 +253,14 @@ int main(int argc, char * argv[])
 	{
 		printf("Loading database \"%s\".\n", inputDbPath.c_str());
 	}
+
 	// Disable statistics (we don't need them)
-	pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "false"));
+	uInsert(pm, ParametersPair(Parameters::kRtabmapPublishStats(), "false"));
-	pm.insert(ParametersPair(Parameters::kRGBDEnabled(), "false"));
+	uInsert(pm, ParametersPair(Parameters::kRGBDEnabled(), "false"));
 
 	rtabmap.init(pm, inputDbPath);
 
-	printf("Avpd init time = %fs\n", timer.ticks());
+	printf("rtabmap init time = %fs\n", timer.ticks());
 
 	// Start thread's task
 	int loopClosureId;

tools/OdometryViewer/main.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/OdometryF2F.h>
 #include <rtabmap/core/OdometryMono.h>
 #include <rtabmap/core/OdometryThread.h>
@@ -49,44 +50,12 @@ void showUsage()
 			"odometryViewer [options]\n"
 			"Options:\n"
 			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394, 7=FlyCapture2\n"
-			"  -o #                      Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
-			"  -nn #                     Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
-			"  -nndr #                   Nearest neighbor distance ratio (default 0.7)\n"
-			"  -flow                     Use optical flow odometry.\n"
-			"  -icp                      Use ICP odometry\n"
-			"  -mono                     Use Mono odometry\n"
-			"\n"
 			"  -hz #.#                   Camera rate (default 0, 0 means as fast as the camera can)\n"
 			"  -db \"input.db\"          Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
 			"  -clouds #                 Maximum clouds shown (default 10, zero means inf)\n"
 			"  -sec #.#                  Delay (seconds) before reading the database (if set)\n"
-			"\n"
+			"%s\n",
-			"  -in #.#                   Inliers maximum distance, features/ICP (default 0.01 m)\n"
+			rtabmap::Parameters::showUsage());
-			"  -max #                    Max features used for matching (default 0=inf)\n"
-			"  -min #                    Minimum inliers to accept the transform (default 20)\n"
-			"  -depth #.#                Maximum features depth (default 5.0 m)\n"
-			"  -i #                      RANSAC/ICP iterations (default 30)\n"
-			"  -reset #                  Reset countdown (default 0 = disabled)\n"
-			"  -gpu                      Use GPU\n"
-			"  -lh #                     Local history (default 1000)\n"
-			"\n"
-			"  -brief_bytes #        BRIEF bytes (default 32)\n"
-			"  -fast_thr #           FAST threshold (default 30)\n"
-			"\n"
-			"  -d #                      ICP decimation (default 4)\n"
-			"  -v #                      ICP voxel size (default 0.005)\n"
-			"  -cr #.#                   ICP correspondence ratio (default 0.7)\n"
-			"  -p2p                      ICP point to point (default point to plane)"
-			"\n"
-			"  -debug                    Log debug messages\n"
-			"\n"
-			"Examples:\n"
-			"  odometryViewer -odom 0 -lh 5000                      SURF example\n"
-			"  odometryViewer -odom 1 -lh 10000                     SIFT example\n"
-			"  odometryViewer -odom 4 -nn 2 -lh 1000                FAST/BRIEF example\n"
-			"  odometryViewer -odom 3 -nn 2 -lh 1000                FAST/FREAK example\n"
-			"  odometryViewer -icp -in 0.05 -i 30                   ICP example\n"
-			"  odometryViewer -flow -in 0.02                        Optical flow example\n");
 	exit(1);
 }
 
@@ -94,35 +63,13 @@ int main (int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
-	ULogger::setPrintTime(false);
-	ULogger::setPrintWhere(false);
 
 	// parse arguments
 	float rate = 0.0;
 	std::string inputDatabase;
 	int driver = 0;
-	int odomType = rtabmap::Parameters::defaultVisFeatureType();
-	bool icp = false;
-	bool flow = false;
-	bool mono = false;
-	int nnType = rtabmap::Parameters::defaultVisCorNNType();
-	float nndr = rtabmap::Parameters::defaultVisCorNNDR();
-	float distance = rtabmap::Parameters::defaultVisInlierDistance();
-	int maxWords = rtabmap::Parameters::defaultVisMaxFeatures();
-	int minInliers = rtabmap::Parameters::defaultVisMinInliers();
-	float maxDepth = rtabmap::Parameters::defaultVisMaxDepth();
-	int iterations = rtabmap::Parameters::defaultVisIterations();
-	int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
-	int decimation = 4;
-	float voxel = 0.005;
-	float ratio = 0.7f;
 	int maxClouds = 10;
-	int briefBytes = rtabmap::Parameters::defaultBRIEFBytes();
-	int fastThr = rtabmap::Parameters::defaultFASTThreshold();
 	float sec = 0.0f;
-	bool gpu = false;
-	int localHistory = rtabmap::Parameters::defaultOdomF2MMaxSize();
-	bool p2p = false;
 
 	for(int i=1; i<argc; ++i)
 	{
@@ -143,57 +90,6 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-o") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				odomType = std::atoi(argv[i]);
-				if(odomType < 0 || odomType > 6)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-nn") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				nnType = std::atoi(argv[i]);
-				if(nnType < 0 || nnType > 4)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-nndr") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				nndr = uStr2Float(argv[i]);
-				if(nndr < 0.0f)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
 		if(strcmp(argv[i], "-hz") == 0)
 		{
 			++i;
@@ -263,256 +159,10 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-in") == 0)
+		if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
 		{
-			++i;
+			showUsage();
-			if(i < argc)
-			{
-				distance = uStr2Float(argv[i]);
-				if(distance <= 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
 		}
-		if(strcmp(argv[i], "-max") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				maxWords = std::atoi(argv[i]);
-				if(maxWords < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-min") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				minInliers = std::atoi(argv[i]);
-				if(minInliers < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-depth") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				maxDepth = uStr2Float(argv[i]);
-				if(maxDepth < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-i") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				iterations = std::atoi(argv[i]);
-				if(iterations <= 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-reset") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				resetCountdown = std::atoi(argv[i]);
-				if(resetCountdown < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-d") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				decimation = std::atoi(argv[i]);
-				if(decimation < 1)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-v") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				voxel = uStr2Float(argv[i]);
-				if(voxel < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-cr") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				ratio = uStr2Float(argv[i]);
-				if(ratio < 0.0f)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-gpu") == 0)
-		{
-			gpu = true;
-			continue;
-		}
-		if(strcmp(argv[i], "-lh") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				localHistory = std::atoi(argv[i]);
-				if(localHistory < 0)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-brief_bytes") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				briefBytes = std::atoi(argv[i]);
-				if(briefBytes < 1)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-fast_thr") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				fastThr = std::atoi(argv[i]);
-				if(fastThr < 1)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-icp") == 0)
-		{
-			icp = true;
-			continue;
-		}
-		if(strcmp(argv[i], "-flow") == 0)
-		{
-			flow = true;
-			continue;
-		}
-		if(strcmp(argv[i], "-mono") == 0)
-		{
-			mono = true;
-			continue;
-		}
-		if(strcmp(argv[i], "-p2p") == 0)
-		{
-			p2p = true;
-			continue;
-		}
-		if(strcmp(argv[i], "-debug") == 0)
-		{
-			ULogger::setLevel(ULogger::kDebug);
-			ULogger::setPrintTime(true);
-			ULogger::setPrintWhere(true);
-			continue;
-		}
-
-		printf("Unrecognized option : %s\n", argv[i]);
-		showUsage();
-	}
-
-	if(odomType > 1 && nnType == rtabmap::VWDictionary::kNNFlannKdTree)
-	{
-		UERROR("You set \"-o %d\" (binary descriptor), you must use \"-nn 2\" (any \"-nn\" other than kNNFlannKdTree)", odomType);
-		showUsage();
-	}
-	else if(odomType <= 1 && nnType == rtabmap::VWDictionary::kNNFlannLSH)
-	{
-		UERROR("You set \"-o %d\" (float descriptor), you must use \"-nn 1\" (any \"-nn\" other than kNNFlannLSH)", odomType);
-		showUsage();
 	}
 
 	if(inputDatabase.size())
@@ -524,159 +174,46 @@ int main (int argc, char * argv[])
 		UINFO("Using OpenNI camera");
 	}
 
-	std::string odomName;
-	if(odomType == 0)
-	{
-		odomName = "SURF";
-	}
-	else if(odomType == 1)
-	{
-		odomName = "SIFT";
-	}
-	else if(odomType == 2)
-	{
-		odomName = "ORB";
-	}
-	else if(odomType == 3)
-	{
-		odomName = "FAST+FREAK";
-	}
-	else if(odomType == 4)
-	{
-		odomName = "FAST+BRIEF";
-	}
-	else if(odomType == 5)
-	{
-		odomName = "GFTT+FREAK";
-	}
-	else if(odomType == 6)
-	{
-		odomName = "GFTT+BRIEF";
-	}
-	else if(odomType == 7)
-	{
-		odomName = "BRISK";
-	}
-
-	if(icp)
-	{
-		odomName= "ICP";
-	}
-
-	if(flow)
-	{
-		odomName= "Optical Flow";
-	}
-
-	std::string nnName;
-	if(nnType == 0)
-	{
-		nnName = "kNNFlannLinear";
-	}
-	else if(nnType == 1)
-	{
-		nnName = "kNNFlannKdTree";
-	}
-	else if(nnType == 2)
-	{
-		nnName= "kNNFlannLSH";
-	}
-	else if(nnType == 3)
-	{
-		nnName= "kNNBruteForce";
-	}
-	else if(nnType == 4)
-	{
-		nnName= "kNNBruteForceGPU";
-	}
-
-	UINFO("Odometry used =           %s", odomName.c_str());
 	UINFO("Camera rate =             %f Hz", rate);
 	UINFO("Maximum clouds shown =    %d", maxClouds);
 	UINFO("Delay =                   %f s", sec);
-	UINFO("Max depth =               %f", maxDepth);
+
-	UINFO("Reset odometry coutdown = %d", resetCountdown);
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		UINFO(" Param \"%s\"=\"%s\"", iter->first.c_str(), iter->second.c_str());
+	}
+
+	bool icp = false;
+	int regStrategy = rtabmap::Parameters::defaultRegStrategy();
+	rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kRegStrategy(), regStrategy);
+	int decimation = 8;
+	float maxDepth = 4.0f;
+	float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
+	int normalsK = 0;
+	if(regStrategy == 1 || regStrategy == 2)
+	{
+		// icp requires scans
+		icp = true;
+
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpDownsamplingStep(), decimation);
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpVoxelSize(), voxelSize);
+
+		bool pointToPlane = rtabmap::Parameters::defaultIcpPointToPlane();
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
+		if(pointToPlane)
+		{
+			normalsK = rtabmap::Parameters::defaultIcpPointToPlaneNormalNeighbors();
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), normalsK);
+		}
+
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), "0"));
+	}
 
 	QApplication app(argc, argv);
 
-	rtabmap::Odometry * odom = 0;
+	rtabmap::Odometry * odom = rtabmap::Odometry::create(parameters);
-
-	rtabmap::ParametersMap parameters;
-
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMaxDepth(), uNumber2Str(maxDepth)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
-
-	if(!icp)
-	{
-		UINFO("Min inliers =             %d", minInliers);
-		UINFO("Inlier maximum correspondences distance = %f", distance);
-		UINFO("RANSAC iterations =       %d", iterations);
-		UINFO("Max features =            %d", maxWords);
-		UINFO("GPU =                     %s", gpu?"true":"false");
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisInlierDistance(), uNumber2Str(distance)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), uNumber2Str(minInliers)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisIterations(), uNumber2Str(iterations)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMaxFeatures(), uNumber2Str(maxWords)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisFeatureType(), uNumber2Str(odomType)));
-		if(odomType == 0)
-		{
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
-		}
-		if(odomType == 2)
-		{
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
-		}
-		if(odomType == 3 || odomType == 4)
-		{
-			UINFO("FAST threshold =          %d", fastThr);
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
-		}
-		if(odomType == 4 || odomType == 6)
-		{
-			UINFO("BRIEF bytes =             %d", briefBytes);
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
-		}
-
-		if(flow)
-		{
-			// Optical Flow
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorType(), "1"));
-			odom = new rtabmap::OdometryF2F(parameters);
-		}
-		else
-		{
-			//Frame to Map
-			UINFO("Nearest neighbor =         %s", nnName.c_str());
-			UINFO("Nearest neighbor ratio =  %f", nndr);
-			UINFO("Local history =           %d", localHistory);
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorNNType(), uNumber2Str(nnType)));
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorNNDR(), uNumber2Str(nndr)));
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomF2MMaxSize(), uNumber2Str(localHistory)));
-
-			if(mono)
-			{
-				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
-				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPReprojError(), "4.0"));
-				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisIterations(), "100"));
-				odom = new rtabmap::OdometryMono(parameters);
-			}
-			else
-			{
-				odom = new rtabmap::OdometryF2M(parameters);
-			}
-		}
-	}
-	else if(icp) // ICP
-	{
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(distance)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), uNumber2Str(iterations)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), uNumber2Str(voxel)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), uNumber2Str(ratio)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), p2p?"false":"true"));
-
-		odom = new rtabmap::OdometryF2F(parameters);
-	}
 
 	rtabmap::OdometryThread odomThread(odom);
 	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
@@ -788,11 +325,9 @@ int main (int argc, char * argv[])
 		{
 			if(camera->isCalibrated())
 			{
-				rtabmap::CameraThread cameraThread(camera);
+				rtabmap::CameraThread cameraThread(camera, parameters);
-				if(icp)
+
-				{
+				cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK);
-					cameraThread.setScanFromDepth(true, decimation, maxDepth);
-				}
 
 				odomThread.start();
 				cameraThread.start();