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

guilib/include/rtabmap/gui/CloudViewer.h

@@ -231,7 +231,8 @@ public:
 				const Transform & to,
 				const QColor & color,
 				bool arrow = false,
-				bool foreground = false);
+				bool foreground = false,
+				double width = 1.0);
 	void removeLine(const std::string & id);
 	void removeAllLines();
 	const std::set<std::string> & getAddedLines() const {return _lines;}

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -371,6 +371,7 @@ private Q_SLOTS:
 	void changeOdometryORBSLAMVocabulary();
 	void changeOdometryOKVISConfigPath();
 	void changeOdometryVINSFusionConfigPath();
+	void changeOdometryLIOSAMConfigPath();
 	void changeOdometryOpenVINSLeftMask();
 	void changeOdometryOpenVINSRightMask();
 	void changeIcpPMConfigPath();

guilib/src/CloudViewer.cpp

@@ -1732,7 +1732,8 @@ void CloudViewer::addOrUpdateLine(
 			const Transform & to,
 			const QColor & color,
 			bool arrow,
-			bool foreground)
+			bool foreground,
+			double width)
 {
 	if(id.empty())
 	{
@@ -1764,6 +1765,7 @@ void CloudViewer::addOrUpdateLine(
 			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?3:2);
 		}
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, width, id);
 	}
 }
 
@@ -2969,18 +2971,19 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 				Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 				Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
 				Eigen::Vector3f xAxis = yAxis.cross(zAxis);
-				Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
-							yAxis[0], yAxis[1], yAxis[2],0,
-							zAxis[0], zAxis[1], zAxis[2],0);
 
-				PR.normalizeRotation();
+				Eigen::Matrix3f m;
+				m << xAxis[0], xAxis[1], xAxis[2],
+					yAxis[0], yAxis[1], yAxis[2],
+					zAxis[0], zAxis[1], zAxis[2];
 
-				Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
-							PR[4], PR[5], PR[6], cameras.front().pos[1],
-							PR[8], PR[9], PR[10], cameras.front().pos[2]);
-				Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
-							PR[4], PR[5], PR[6], cameras.front().focal[1],
-							PR[8], PR[9], PR[10], cameras.front().focal[2]);
+				// Make sure it is normalized
+				Eigen::Quaternionf q = Eigen::Quaternionf(m).normalized();
+
+				Transform P(cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+							q.x(), q.y(), q.z(), q.w());
+				Transform F(cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+							q.x(), q.y(), q.z(), q.w());
 				Transform N = pose;
 				Transform O = _lastPose;
 				Transform O2N = O.inverse()*N;

guilib/src/DataRecorder.cpp

@@ -140,7 +140,7 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
 		//save to database
 		UTimer time;
 		memory_->update(data, pose, covariance);
-		const Signature * s = memory_->getLastWorkingSignature();
+		const Signature * s = memory_->getLastWorkingSignature(false);
 		totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
 		totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
 		totalSizeKB_ += (int)s->sensorData().laserScanCompressed().data().total()/1000;

guilib/src/DatabaseViewer.cpp

@@ -7930,8 +7930,7 @@ void DatabaseViewer::updateGraphView()
 
 		// remove intermediate nodes?
 		if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
-		   ui_->checkBox_ignoreIntermediateNodes->isEnabled() &&
-		   ui_->checkBox_ignoreIntermediateNodes->isChecked())
+		   (ui_->checkBox_ignoreIntermediateNodes->isChecked() || ui_->comboBox_optimizationFlavor->currentIndex() == 2))
 		{
 			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
@@ -9129,10 +9128,6 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
 		int fromId = newLink.from();
 		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
 		linksIn.insert(std::make_pair(newLink.from(), newLink));
-		const Link * maxLinearLink = 0;
-		const Link * maxAngularLink = 0;
-		float maxLinearErrorRatio = 0.0f;
-		float maxAngularErrorRatio = 0.0f;
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
@@ -9165,51 +9160,43 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
 		std::string msg;
 		if(poses.size())
 		{
-			float maxLinearError = 0.0f;
-			float maxAngularError = 0.0f;
-			graph::computeMaxGraphErrors(
+			graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors(
 					poses,
-					links,
-					maxLinearErrorRatio,
-					maxAngularErrorRatio,
-					maxLinearError,
-					maxAngularError,
-					&maxLinearLink,
-					&maxAngularLink);
-			if(maxLinearLink)
+					links);
+			if(maxGraphErrors.linearLink.isValid())
 			{
-				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-				if(maxLinearErrorRatio > maxOptimizationError)
+				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()));
+				if(maxGraphErrors.linearRatio > maxOptimizationError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),
-						  maxLinearError,
-						  maxLinearErrorRatio,
-						  maxLinearLink->from(),
-						  maxLinearLink->to(),
-						  sqrt(maxLinearLink->transVariance()),
+						  maxGraphErrors.linear,
+						  maxGraphErrors.linearRatio,
+						  maxGraphErrors.linearLink.from(),
+						  maxGraphErrors.linearLink.to(),
+						  sqrt(maxGraphErrors.linearLink.transVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
 				}
 			}
-			if(maxAngularLink)
+			if(maxGraphErrors.angularLink.isValid())
 			{
-				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
-				if(maxAngularErrorRatio > maxOptimizationError)
+				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()));
+				if(maxGraphErrors.angularRatio > maxOptimizationError)
 				{
 					msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),
-						  maxAngularError*180.0f/CV_PI,
-						  maxAngularErrorRatio,
-						  maxAngularLink->from(),
-						  maxAngularLink->to(),
-						  sqrt(maxAngularLink->rotVariance()),
+						  maxGraphErrors.angular*180.0f/CV_PI,
+						  maxGraphErrors.angularRatio,
+						  maxGraphErrors.angularLink.from(),
+						  maxGraphErrors.angularLink.to(),
+						  sqrt(maxGraphErrors.angularLink.rotVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
 				}
@@ -9451,7 +9438,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 		findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
 		if(findIter != linksRemoved_.end())
 		{
-			UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
+			//UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
 			continue; // don't add this link
 		}
 
@@ -9468,7 +9455,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			{
 				links.insert(*findIter);
 			}
-			UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
+			//UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
 			continue;
 		}
 
@@ -9487,11 +9474,11 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
 			if(findIter->second.from() != findIter->second.to()) {
 				links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways 
 			}
-			UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
+			//UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
 			continue;
 		}
 
-		UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
+		//UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
 		links.insert(*iter);
 		if(iter->second.from() != iter->second.to()) {
 			links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways 

guilib/src/GraphViewer.cpp

@@ -2270,11 +2270,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aMouseTracking->setChecked(_mouseTracking);
 	aMouseTracking->setEnabled(_viewPlane == XY);
 	aShowHideGraph->setEnabled(_viewPlane == XY);
-	aShowHideGraphNodes->setEnabled(_graphRoot->isVisible());
 	aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
 	aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
-	aShowHideGtGraph->setEnabled(_gtGraphRoot->isVisible());
-	aShowHideGPSGraph->setEnabled(_gpsGraphRoot->isVisible());
 	aShowHideOdomCacheOverlay->setEnabled(_odomCacheOverlay->rect().width()>0);
 
 	QMenu * viewPlaneMenu = menu.addMenu("View Plane...");

guilib/src/MainWindow.cpp

@@ -585,6 +585,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	// Apply state
 	this->changeState(kIdle);
 	this->applyPrefSettings(PreferencesDialog::kPanelAll);
+	applyPrefSettings(parameters, false);
 
 	_ui->statsToolBox->setNewFigureMaxItems(50);
 	_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
@@ -709,10 +710,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	this->loadFigures();
 	connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
 
-	// update loop closure viewer parameters
-	_loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
-	_loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
-
 	if (splash)
 	{
 		splash->close();
@@ -2110,6 +2107,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		}
 
 		// For intermediate empty nodes, keep latest image shown
+		bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 		if(signature.getWeight() >= 0)
 		{
 			_ui->imageView_source->clear();
@@ -2134,7 +2132,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			_ui->label_matchId->clear();
 
-			bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 			int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
 			bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
 			_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
@@ -2458,6 +2455,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				UDEBUG("time= %d ms (update loop closure viewer)", time.restart());
 			}
 		}
+		else if(rehearsedSimilarity)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
+		}
+		else if(smallMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::gray);
+		}
+		else if(fastMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::magenta);
+		}
 
 		// PDF AND LIKELIHOOD
 		if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -2715,7 +2724,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			Signature & s = *_cachedSignatures.find(stat.refImageId());
 			_cachedMemoryUsage -= s.sensorData().getMemoryUsed();
 			s.sensorData().clearRawData();
-			s.sensorData().clearOccupancyGridRaw();
 			_cachedMemoryUsage += s.sensorData().getMemoryUsed();
 		}
 
@@ -3254,14 +3262,17 @@ void MainWindow::updateMapCloud(
 	}
 
 	std::map<int, Transform> posesWithOdomCache;
-
+	std::set<int> odomCachePosesIds;
 	if(_ui->graphicsView_graphView->isVisible() ||
 	   ((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size()))
 	{
 		posesWithOdomCache = posesIn;
 		for(std::map<int, Transform>::const_iterator iter=odomCachePoses.begin(); iter!=odomCachePoses.end(); ++iter)
 		{
-			posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second));
+			if(posesWithOdomCache.insert(std::make_pair(iter->first, _odometryCorrection*iter->second)).second)
+			{
+				odomCachePosesIds.insert(iter->first);
+			}
 		}
 	}
 
@@ -3520,7 +3531,7 @@ void MainWindow::updateMapCloud(
 		std::multimap<int, Link> constraintsWithOdomCache;
 		constraintsWithOdomCache = constraints;
 		constraintsWithOdomCache.insert(odomCacheConstraints.begin(), odomCacheConstraints.end());
-		_ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map<int, int>(), uKeysSet(odomCachePoses));
+		_ui->graphicsView_graphView->updateGraph(posesWithOdomCache, constraintsWithOdomCache, mapIdsIn, std::map<int, int>(), odomCachePosesIds);
 		if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
 		{
 			std::map<int, Transform> gtPoses = _currentGTPosesMap;
@@ -6862,10 +6873,6 @@ void MainWindow::postProcessing(
 												}
 												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());
@@ -6880,51 +6887,43 @@ void MainWindow::postProcessing(
 												std::string msg;
 												if(poses.size())
 												{
-													float maxLinearErrorRatio = 0.0f;
-													float maxAngularErrorRatio = 0.0f;
-													graph::computeMaxGraphErrors(
+													graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors(
 															poses,
-															links,
-															maxLinearErrorRatio,
-															maxAngularErrorRatio,
-															maxLinearError,
-															maxAngularError,
-															&maxLinearLink,
-															&maxAngularLink);
-													if(maxLinearLink)
+															links);
+													if(maxGraphErrors.linearLink.isValid())
 													{
-														UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-														if(maxLinearErrorRatio > optimizeMaxError)
+														UINFO("Max optimization linear error = %f m (link %d->%d)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to());
+														if(maxGraphErrors.linearRatio > 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()),
+																	  maxGraphErrors.linear,
+																	  maxGraphErrors.linearLink.from(),
+																	  maxGraphErrors.linearLink.to(),
+																	  maxGraphErrors.linearRatio,
+																	  sqrt(maxGraphErrors.linearLink.transVariance()),
 																	  Parameters::kRGBDOptimizeMaxError().c_str(),
 																	  optimizeMaxError);
 														}
 													}
-													else if(maxAngularLink)
+													else if(maxGraphErrors.angularLink.isValid())
 													{
-														UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
-														if(maxAngularErrorRatio > optimizeMaxError)
+														UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to());
+														if(maxGraphErrors.angularRatio > 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()),
+																	  maxGraphErrors.angular*180.0f/M_PI,
+																	  maxGraphErrors.angularLink.from(),
+																	  maxGraphErrors.angularLink.to(),
+																	  maxGraphErrors.angularRatio,
+																	  sqrt(maxGraphErrors.angularLink.rotVariance()),
 																	  Parameters::kRGBDOptimizeMaxError().c_str(),
 																	  optimizeMaxError);
 														}

guilib/src/PreferencesDialog.cpp

@@ -243,6 +243,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_CUVSLAM
 	_ui->odom_strategy->setItemData(13, 0, Qt::UserRole - 1);
 #endif
+#ifndef RTABMAP_LIOSAM
+	_ui->odom_strategy->setItemData(14, 0, Qt::UserRole - 1);
+#endif
 
 #if CV_MAJOR_VERSION < 3
 	_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
@@ -758,6 +761,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_checkBox_ignoreFeatures, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignorePriors, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_intermediateNodesAreNormalNodes, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1269,6 +1273,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str());
 	_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 	_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
+	_ui->graphOptimization_maxErrorRepairRadius->setObjectName(Parameters::kRGBDOptimizeMaxErrorRepairRadius().c_str());
 	_ui->graphOptimization_gravitySigma->setObjectName(Parameters::kOptimizerGravitySigma().c_str());
 	_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
 	_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
@@ -1698,6 +1703,22 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Odometry CuVSLAM
 	_ui->odom_cuvslam_multicam_mode->setObjectName(Parameters::kOdomCuVSLAMMulticamMode().c_str());
 
+	// Odometry LIO-SAM
+	_ui->lineEdit_OdomLIOSAMPath->setObjectName(Parameters::kOdomLIOSAMConfigPath().c_str());
+	connect(_ui->toolButton_OdomLIOSAMPath, SIGNAL(clicked()), this, SLOT(changeOdometryLIOSAMConfigPath()));
+	_ui->odom_liosam_sensor->setObjectName(Parameters::kOdomLIOSAMSensor().c_str());
+	_ui->odom_liosam_nscan->setObjectName(Parameters::kOdomLIOSAMNScan().c_str());
+	_ui->odom_liosam_horizon_scan->setObjectName(Parameters::kOdomLIOSAMHorizonScan().c_str());
+	_ui->odom_liosam_imu_acc_noise->setObjectName(Parameters::kOdomLIOSAMImuAccNoise().c_str());
+	_ui->odom_liosam_imu_gyr_noise->setObjectName(Parameters::kOdomLIOSAMImuGyrNoise().c_str());
+	_ui->odom_liosam_imu_acc_bias_n->setObjectName(Parameters::kOdomLIOSAMImuAccBiasN().c_str());
+	_ui->odom_liosam_imu_gyr_bias_n->setObjectName(Parameters::kOdomLIOSAMImuGyrBiasN().c_str());
+	_ui->odom_liosam_imu_gravity->setObjectName(Parameters::kOdomLIOSAMImuGravity().c_str());
+	_ui->odom_liosam_edge_threshold->setObjectName(Parameters::kOdomLIOSAMEdgeThreshold().c_str());
+	_ui->odom_liosam_surf_threshold->setObjectName(Parameters::kOdomLIOSAMSurfThreshold().c_str());
+	_ui->odom_liosam_linvar->setObjectName(Parameters::kOdomLIOSAMLinVar().c_str());
+	_ui->odom_liosam_angvar->setObjectName(Parameters::kOdomLIOSAMAngVar().c_str());
+
 	//StereoDense
 	_ui->comboBox_stereoDense_strategy->setObjectName(Parameters::kStereoDenseStrategy().c_str());
 	connect(_ui->comboBox_stereoDense_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereoDense, SLOT(setCurrentIndex(int)));
@@ -2209,6 +2230,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_ignoreFeatures->setChecked(true);
 		_ui->source_checkBox_ignorePriors->setChecked(false);
 		_ui->source_checkBox_ignoreIMU->setChecked(false);
+		_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(false);
 		_ui->source_spinBox_databaseStartId->setValue(0);
 		_ui->source_spinBox_databaseStopId->setValue(0);
 		_ui->source_lineEdit_databaseCameraIndex->setText("");
@@ -2971,6 +2993,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_checkBox_ignoreFeatures->setChecked(settings.value("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()).toBool());
 	_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
 	_ui->source_checkBox_ignoreIMU->setChecked(settings.value("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()).toBool());
+	_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(settings.value("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked()).toBool());
 	
 	_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
 	_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
@@ -3591,6 +3614,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("ignoreFeatures",  _ui->source_checkBox_ignoreFeatures->isChecked());
 	settings.setValue("ignorePriors",  _ui->source_checkBox_ignorePriors->isChecked());
 	settings.setValue("ignoreImu",  _ui->source_checkBox_ignoreIMU->isChecked());
+	settings.setValue("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked());
 	settings.setValue("startId",          _ui->source_spinBox_databaseStartId->value());
 	settings.setValue("stopId",          _ui->source_spinBox_databaseStopId->value());
 	settings.setValue("cameraIndices",       _ui->source_lineEdit_databaseCameraIndex->text());
@@ -5572,6 +5596,7 @@ void PreferencesDialog::updateOdometryStackedIndex(int index)
 	_ui->groupBox_odomOpenVINS->setVisible(index==10);
 	_ui->groupBox_odomOpen3D->setVisible(index==12);
 	_ui->groupBox_odomCuvslam->setVisible(index==13);
+	_ui->groupBox_odomLIOSAM->setVisible(index==14);
 }
 
 void PreferencesDialog::useOdomFeatures()
@@ -5677,6 +5702,23 @@ void PreferencesDialog::changeOdometryVINSFusionConfigPath()
 	}
 }
 
+void PreferencesDialog::changeOdometryLIOSAMConfigPath()
+{
+	QString path;
+	if(_ui->lineEdit_OdomLIOSAMPath->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), this->getWorkingDirectory(), tr("LIO-SAM config (*.yaml)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), _ui->lineEdit_OdomLIOSAMPath->text(), tr("LIO-SAM config (*.yaml)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_OdomLIOSAMPath->setText(path);
+	}
+}
+
 void PreferencesDialog::changeOdometryOpenVINSLeftMask()
 {
 	QString path;
@@ -7265,13 +7307,14 @@ Camera * PreferencesDialog::createCamera(
 				_ui->source_spinBox_databaseStartId->value(),
 				cameraIndices,
 				_ui->source_spinBox_databaseStopId->value(),
-				!_ui->general_checkBox_createIntermediateNodes->isChecked(),
+				!_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked() && !_ui->general_checkBox_createIntermediateNodes->isChecked(),
 				_ui->source_checkBox_ignoreLandmarks->isChecked(),
 				_ui->source_checkBox_ignoreFeatures->isChecked(),
 				0,
 				-1,
 				_ui->source_checkBox_ignorePriors->isChecked(),
 				_ui->source_checkBox_ignoreIMU->isChecked(),
+				_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked(),
 				localTransformOverrides);
 	}
 	else