bump 0.20.23

.github/workflows/cmake-ros.yml

@@ -22,7 +22,7 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        ros_distribution: [melodic, noetic, foxy, galactic, humble, rolling]
+        ros_distribution: [melodic, noetic, foxy, humble, rolling]
         include:
         - ros_distribution: 'melodic' 
           os: ubuntu-18.04
@@ -30,8 +30,6 @@ jobs:
           os: ubuntu-20.04
         - ros_distribution: 'foxy'
           os: ubuntu-20.04
-        - ros_distribution: 'galactic'
-          os: ubuntu-20.04
         - ros_distribution: 'humble'
           os: ubuntu-22.04
         - ros_distribution: 'rolling'

.github/workflows/cmake.yml

@@ -24,7 +24,7 @@ jobs:
       run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev
 
     - uses: actions/checkout@v2
 

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 22)
+SET(RTABMAP_PATCH_VERSION 23)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -106,6 +106,7 @@ if(MSVC)
       SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP${N}")
     endif()
   endif()
+  add_compile_options("/bigobj")
 endif()
 
 # [Eclipse] Automatic Discovery of Include directories (Optional, but handy) 
@@ -315,7 +316,11 @@ IF(WITH_QT)
 			IF(value EQUAL -1)
                 list(FIND PCL_LIBRARIES vtkGUISupportQt value)
                 IF(value EQUAL -1)
-                    SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    IF("${VTK_MAJOR_VERSION}" GREATER 8)
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};VTK::GUISupportQt")
+                    ELSE()
+                        SET(PCL_LIBRARIES "${PCL_LIBRARIES};vtkGUISupportQt")
+                    ENDIF()
                     SET(ADD_VTK_GUI_SUPPORT_QT_TO_CONF TRUE)
                 ENDIF(value EQUAL -1)
 		    ENDIF(value EQUAL -1)
@@ -1019,7 +1024,11 @@ IF(NOT WITH_PYTHON OR NOT Python3_FOUND)
 ENDIF()
 IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF)
    SET(CONF_VTK_QT true)
-   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+   IF("${VTK_MAJOR_VERSION}" GREATER 8)
+      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} VTK::GUISupportQt)
+   ELSE()
+      SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} vtkGUISupportQt)
+   ENDIF()
 ELSE()
    SET(CONF_VTK_QT false)
 ENDIF()

README.md

@@ -59,14 +59,10 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
             <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
         </tr>
         <tr>
-            <td rowspan="4">ROS 2</td>
+            <td rowspan="3">ROS 2</td>
             <td>Foxy</td>
             <td><a href="http://build.ros2.org/job/Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Fbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
         </tr>
-        <tr>
-            <td>Galactic</td>
-            <td><a href="http://build.ros2.org/job/Gbin_uF64__rtabmap__ubuntu_focal_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Gbin_uF64__rtabmap__ubuntu_focal_amd64__binary" alt="Build Status"/></td>
-        </tr>
         <tr>
             <td>Humble</td>
             <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>

corelib/include/rtabmap/core/Statistics.h

@@ -241,7 +241,7 @@ public:
 	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
 	void setStamp(double stamp) {_stamp = stamp;}
 
-	RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}, "Use addSignatureData() instead.");
+	RTABMAP_DEPRECATED(void setLastSignatureData(const Signature & data), "Use addSignatureData() instead.");
 	void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}
 	void setSignaturesData(const std::map<int, Signature> & data) {_signaturesData = data;}
 

corelib/src/MarkerDetector.cpp

@@ -39,12 +39,16 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
 	maxRange_ = Parameters::defaultMarkerMaxRange();
 	minRange_ = Parameters::defaultMarkerMinRange();
 	dictionaryId_ = Parameters::defaultMarkerDictionary();
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    detectorParams_.reset(new cv::aruco::DetectorParameters());
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	detectorParams_ = cv::aruco::DetectorParameters::create();
 #else
 	detectorParams_.reset(new cv::aruco::DetectorParameters());
 #endif
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod();
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
 	detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
 #else
 	detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
@@ -70,7 +74,11 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 	detectorParams_->minCornerDistanceRate = 0.05;
 	detectorParams_->minDistanceToBorder = 3;
 	detectorParams_->minMarkerDistanceRate = 0.05;
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    int cornerRefinementMethod;
+    Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod);
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod)cornerRefinementMethod;
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
 	Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
 #else
 	int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
@@ -103,7 +111,10 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 		dictionaryId_ = Parameters::defaultMarkerDictionary();
 	}
 #endif
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+    dictionary_.reset(new cv::aruco::Dictionary());
+    *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
+#elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
 	dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
 #else
 	dictionary_.reset(new cv::aruco::Dictionary());

corelib/src/RegistrationVis.cpp

@@ -490,9 +490,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
 				if(guessSet)
 				{
-					if(fromSignature.sensorData().cameraModels().size() == 1 || fromSignature.sensorData().cameraModels().size() == 1)
+					if(toSignature.sensorData().cameraModels().size() == 1 || toSignature.sensorData().stereoCameraModels().size() == 1)
 					{
-						Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModels()[0].left().localTransform();
+						Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModels()[0].left().localTransform();
 						Transform guessCameraRef = (guess * localTransform).inverse();
 						cv::Mat R = (cv::Mat_<double>(3,3) <<
 								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
@@ -501,7 +501,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cv::Mat rvec(1,3, CV_64FC1);
 						cv::Rodrigues(R, rvec);
 						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-						cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModels()[0].left().K();
+						cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModels()[0].left().K();
 						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
 					}
 					else

corelib/src/Statistics.cpp

@@ -95,4 +95,10 @@ void Statistics::addStatistic(const std::string & name, float value)
 	uInsert(_data, std::pair<std::string, float>(name, value));
 }
 
+//deprecated
+void Statistics::setLastSignatureData(const Signature & data)
+{
+	_signaturesData.insert(std::make_pair(data.id(), data));
+}
+
 }

corelib/src/odometry/OdometryVINS.cpp

@@ -503,7 +503,7 @@ Transform OdometryVINS::computeTransform(
 	{
 		UERROR("VINS-Fusion requires stereo images!");
 	}
-	else
+	else if(data.imu().empty())
 	{
 		UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
 	}

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -303,8 +303,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 							mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
 							mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
-							mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
+							mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
-							mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+							mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 
 							graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
@@ -383,8 +383,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 							mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
 							mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
-							mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
+							mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
-							mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+							mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 							gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 							graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
 						}
@@ -471,8 +471,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 					mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
 					mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
-					mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
+					mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
-					mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+					mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
 					gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
 
 #ifdef RTABMAP_VERTIGO
@@ -708,8 +708,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
 				mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
 				mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
-				mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
+				mgtsam.block(0,3,3,3) = info.block(3,0,3,3); // off diagonal
-				mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
+				mgtsam.block(3,0,3,3) = info.block(0,3,3,3); // off diagonal
 				memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
 			}
 			else

guilib/src/DatabaseViewer.cpp

@@ -4364,9 +4364,15 @@ void DatabaseViewer::refineAllLinks(const QList<Link> & links)
 		{
 			int from = links[i].from();
 			int to = links[i].to();
-			this->refineConstraint(links[i].from(), links[i].to(), true);
+			if(from > 0 && to > 0)
-
+			{
-			progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
+				this->refineConstraint(links[i].from(), links[i].to(), true);
+				progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
+			}
+			else
+			{
+				progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to));
+			}
 			progressDialog->incrementStep();
 			QApplication::processEvents();
 			if(progressDialog->isCanceled())
@@ -7217,6 +7223,49 @@ void DatabaseViewer::updateGraphView()
 			}
 		}
 
+		// Marker priors parameters
+		double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
+		double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
+		std::map<int, Transform> markerPriors;
+		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+		Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
+		UASSERT(markerPriorsLinearVariance>0.0f);
+		Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
+		UASSERT(markerPriorsAngularVariance>0.0f);
+		std::string markerPriorsStr;
+		if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
+		{
+			std::list<std::string> strList = uSplit(markerPriorsStr, '|');
+			for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
+			{
+				std::string markerStr = *iter;
+				while(!markerStr.empty() && !uIsDigit(markerStr[0]))
+				{
+					markerStr.erase(markerStr.begin());
+				}
+				if(!markerStr.empty())
+				{
+					std::string idStr = uSplitNumChar(markerStr).front();
+					int id = uStr2Int(idStr);
+					Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
+					if(!prior.isNull() && id>0)
+					{
+						markerPriors.insert(std::make_pair(-id, prior));
+						UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
+					}
+					else
+					{
+						UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
+					}
+				}
+				else if(!iter->empty())
+				{
+					UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
+				}
+			}
+		}
+
+
 		// filter links
 		int totalNeighbor = 0;
 		int totalNeighborMerged = 0;
@@ -7291,6 +7340,19 @@ void DatabaseViewer::updateGraphView()
 				}
 				loopLinks_.push_back(iter->second);
 				++totalLandmarks;
+
+				// add landmark priors if there are some
+				int markerId = iter->second.to();
+				if(markerPriors.find(markerId) != markerPriors.end())
+				{
+					cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
+					infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
+					infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
+					links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
+					UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
+							markerPriorsLinearVariance, markerPriorsAngularVariance);
+					++totalPriors;
+				}
 			}
 			else if(iter->second.type() == Link::kPosePrior)
 			{
@@ -7352,7 +7414,7 @@ void DatabaseViewer::updateGraphView()
 
 		graphes_.push_back(poses);
 
-		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
+		Optimizer * optimizer = Optimizer::create(parameters);
 
 		std::map<int, rtabmap::Transform> posesOut;
 		std::multimap<int, rtabmap::Link> linksOut;

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.20.22</version>
+  <version>0.20.23</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/EurocDataset/CMakeLists.txt

@@ -45,11 +45,11 @@ SET(LIBRARIES
 	${YAML_CPP_LIBRARIES}
 )
 
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS} yaml-cpp)
 
 ADD_EXECUTABLE(euroc_dataset main.cpp)
   
-TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES})
+TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES} yaml-cpp)
 
 SET_TARGET_PROPERTIES( euroc_dataset 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-euroc_dataset)

tools/Reprocess/main.cpp

@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Graph.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/OdometryInfo.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -67,6 +69,11 @@ void showUsage()
 			"     -c \"path.ini\"   Configuration file, overwriting parameters read \n"
 			"                       from the database. If custom parameters are also set as \n"
 			"                       arguments, they overwrite those in config file and the database.\n"
+			"     -default    Input database's parameters are ignored, using default ones instead.\n"
+			"     -odom       Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n"
+			"                 is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
+			"                 cannot be detected in this mode (assuming the database contains continuous frames\n"
+			"                 of a single session).\n"
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
 			"     -start_s #  Start from this map session ID.\n"
@@ -229,6 +236,8 @@ int main(int argc, char * argv[])
 	bool assemble2dOctoMap = false;
 	bool assemble3dOctoMap = false;
 	bool useDatabaseRate = false;
+	bool useDefaultParameters = false;
+	bool recomputeOdometry = false;
 	int startId = 0;
 	int stopId = 0;
 	int startMapId = 0;
@@ -274,6 +283,15 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "-default") == 0 || strcmp(argv[i], "--default") == 0)
+		{
+			useDefaultParameters = true;
+			printf("Using default parameters.\n");
+		}
+		else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0)
+		{
+			recomputeOdometry = true;
+		}
 		else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
 		{
 			++i;
@@ -556,13 +574,19 @@ int main(int argc, char * argv[])
 		return -1;
 	}
 
-	ParametersMap parameters = dbDriver->getLastParameters();
+	ParametersMap parameters;
-	std::string targetVersion = dbDriver->getDatabaseVersion();
+	std::string targetVersion;
-	parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
+	if(!useDefaultParameters)
-	if(parameters.empty())
 	{
-		printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
+		parameters = dbDriver->getLastParameters();
+		targetVersion = dbDriver->getDatabaseVersion();
+		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
+		if(parameters.empty())
+		{
+			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
+		}
 	}
+
 	if(customParameters.size())
 	{
 		printf("Custom parameters:\n");
@@ -757,6 +781,28 @@ int main(int argc, char * argv[])
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate);
 
+	Odometry * odometry = 0;
+	float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
+	double lastUpdateStamp = 0;
+	if(recomputeOdometry)
+	{
+		if(odometryIgnored)
+		{
+			printf("odom option is set but %s parameter is false, odometry won't be recomputed...\n", Parameters::kRGBDEnabled().c_str());
+			recomputeOdometry = false;
+		}
+		else
+		{
+			printf("Odometry will be recomputed (odom option is set)\n");
+			Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
+			if(rtabmapUpdateRate!=0)
+			{
+				rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
+			}
+			odometry = Odometry::create(parameters);
+		}
+	}
+
 	printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
 	std::map<std::string, float> globalMapStats;
 	int processed = 0;
@@ -773,6 +819,44 @@ int main(int argc, char * argv[])
 	bool inMotion = true;
 	while(data.isValid() && g_loopForever)
 	{
+		if(recomputeOdometry)
+		{
+			OdometryInfo odomInfo;
+			Transform pose = odometry->process(data, &odomInfo);
+			printf("Processed %d/%d frames (visual=%d/%d lidar=%f lost=%s)... odometry = %dms\n",
+					processed+1,
+					totalIds,
+					odomInfo.reg.inliers,
+					odomInfo.reg.matches,
+					odomInfo.reg.icpInliersRatio,
+					odomInfo.lost?"true":"false",
+					int(odomInfo.timeEstimation * 1000));
+			if(lastUpdateStamp > 0.0 && data.stamp() < lastUpdateStamp + rtabmapUpdateRate)
+			{
+				if(framesToSkip>0)
+				{
+					int skippedFrames = framesToSkip;
+					while(skippedFrames-- > 0)
+					{
+						++processed;
+						data = dbReader->takeImage();
+					}
+				}
+
+				data = dbReader->takeImage(&info);
+				if(scanFromDepth)
+				{
+					data.setLaserScan(LaserScan());
+				}
+				camThread.postUpdate(&data, &info);
+				++processed;
+				continue;
+			}
+			info.odomPose = pose;
+			info.odomCovariance = odomInfo.reg.covariance;
+			lastUpdateStamp = data.stamp();
+		}
+
 		UTimer iterationTime;
 		std::string status;
 		if(!odometryIgnored && info.odomPose.isNull())
@@ -986,11 +1070,12 @@ int main(int argc, char * argv[])
 
 		Transform odomPose = info.odomPose;
 
-		if(framesToSkip>0)
+		if(framesToSkip>0 && !recomputeOdometry)
 		{
 			int skippedFrames = framesToSkip;
 			while(skippedFrames-- > 0)
 			{
+				processed++;
 				data = dbReader->takeImage(&info);
 				if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
 				{
@@ -1058,6 +1143,8 @@ int main(int argc, char * argv[])
 	rtabmap.close(true);
 	printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
 
+	delete odometry;
+
 	if(assemble2dMap)
 	{
 		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";