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

corelib/src/CMakeLists.txt

@@ -98,6 +98,7 @@ SET(SRC_FILES
     odometry/OdometryORBSLAM3.cpp
     odometry/OdometryLOAM.cpp
     odometry/OdometryFLOAM.cpp
+    odometry/OdometryLIOSAM.cpp
     odometry/OdometryMSCKF.cpp
     odometry/OdometryVINSFusion.cpp
     odometry/OdometryOpenVINS.cpp
@@ -604,6 +605,18 @@ IF(floam_FOUND)
     )
 ENDIF(floam_FOUND)
 
+IF(lio_sam_FOUND)
+    SET(INCLUDE_DIRS
+        ${INCLUDE_DIRS}
+        ${lio_sam_INCLUDE_DIRS}
+    )
+    link_directories(${lio_sam_LIBRARY_DIRS})
+    SET(LIBRARIES
+        ${LIBRARIES}
+        lio_sam_core
+    )
+ENDIF(lio_sam_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -791,10 +804,6 @@ IF(CUVSLAM_FOUND)
 ENDIF(CUVSLAM_FOUND)
 
 IF(GTSAM_FOUND)
-	SET(SRC_FILES
-    	${SRC_FILES}
-		optimizer/gtsam/GravityFactor.cpp
-	)
 	SET(LIBRARIES
 		${LIBRARIES}
 		gtsam

corelib/src/DBReader.cpp

@@ -57,6 +57,7 @@ DBReader::DBReader(const std::string & databasePath,
 				   int stopMapId,
 				   bool priorsIgnored,
 				   bool imuIgnored,
+				   bool intermediateNodesAreNormalNodes,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
@@ -67,6 +68,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_stopId(stopId),
 	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
@@ -99,6 +101,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int stopMapId,
 				   bool priorsIgnored,
 				   bool imuIgnored,
+				   bool intermediateNodesAreNormalNodes,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
    _paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_stopId(stopId),
 	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
@@ -749,7 +753,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					data.setStereoCameraModels(combinedStereoModels);
 				}
 			}
-			data.setId(seq);
+			data.setId(!_intermediateNodesAreNormalNodes && s->getWeight()==-1 ? -1 : seq);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
 			if(!globalPose.isNull())

corelib/src/Graph.cpp

@@ -927,21 +927,12 @@ Transform calcRMSE (
 	return t;
 }
 
-void computeMaxGraphErrors(
+MaxGraphErrors computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		float & maxLinearErrorRatio,
-		float & maxAngularErrorRatio,
-		float & maxLinearError,
-		float & maxAngularError,
-		const Link ** maxLinearErrorLink,
-		const Link ** maxAngularErrorLink,
 		bool force3DoF)
 {
-	maxLinearErrorRatio = -1;
-	maxAngularErrorRatio = -1;
-	maxLinearError = -1;
-	maxAngularError = -1;
+	MaxGraphErrors maxError;
 
 	UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -963,19 +954,7 @@ void computeMaxGraphErrors(
 					iter->second.to(),
 					t2.prettyPrint().c_str());
 
-				if(maxLinearErrorLink)
-				{
-					*maxLinearErrorLink = 0;
-				}
-				if(maxAngularErrorLink)
-				{
-					*maxAngularErrorLink = 0;
-				}
-				maxLinearErrorRatio = -1;
-				maxAngularErrorRatio = -1;
-				maxLinearError = -1;
-				maxAngularError = -1;
-				return;
+				return MaxGraphErrors();
 			}
 
 			Transform t;
@@ -999,14 +978,11 @@ void computeMaxGraphErrors(
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
-			if(linearErrorRatio > maxLinearErrorRatio)
+			if(linearErrorRatio > maxError.linearRatio)
 			{
-				maxLinearError = linearError;
-				maxLinearErrorRatio = linearErrorRatio;
-				if(maxLinearErrorLink)
-				{
-					*maxLinearErrorLink = &iter->second;
-				}
+				maxError.linear = linearError;
+				maxError.linearRatio = linearErrorRatio;
+				maxError.linearLink = iter->second;
 			}
 
 			// For landmark links, don't compute angular error if it doesn't estimate orientation
@@ -1031,18 +1007,16 @@ void computeMaxGraphErrors(
 				UASSERT(iter->second.rotVariance(false)>0.0);
 				float stddevAngular = sqrt(iter->second.rotVariance(false));
 				float angularErrorRatio = angularError/stddevAngular;
-				if(angularErrorRatio > maxAngularErrorRatio)
+				if(angularErrorRatio > maxError.angularRatio)
 				{
-					maxAngularError = angularError;
-					maxAngularErrorRatio = angularErrorRatio;
-					if(maxAngularErrorLink)
-					{
-						*maxAngularErrorLink = &iter->second;
-					}
+					maxError.angular = angularError;
+					maxError.angularRatio = angularErrorRatio;
+					maxError.angularLink = iter->second;
 				}
 			}
 		}
 	}
+	return maxError;
 }
 
 std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)

corelib/src/LaserScan.cpp

@@ -68,6 +68,9 @@ std::string LaserScan::formatName(const Format & format)
 		case kXYZIT:
 			name = "XYZIT";
 			break;
+		case kXYZIRT:
+			name = "XYZIRT";
+			break;
 		default:
 			name = "Unknown";
 			break;
@@ -96,6 +99,7 @@ int LaserScan::channels(const Format & format)
 			break;
 		case kXYZNormal:
 		case kXYINormal:
+		case kXYZIRT:
 			channels = 6;
 			break;
 		case kXYZINormal:
@@ -123,11 +127,15 @@ bool LaserScan::isScanHasRGB(const Format & format)
 }
 bool LaserScan::isScanHasIntensity(const Format & format)
 {
-	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
+	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT || format==kXYZIRT;
 }
 bool LaserScan::isScanHasTime(const Format & format)
 {
-	return format==kXYZIT;
+	return format==kXYZIT || format==kXYZIRT;
+}
+bool LaserScan::isScanHasRing(const Format & format)
+{
+	return format==kXYZIRT;
 }
 
 float LaserScan::packRGB(unsigned char r, unsigned char g, unsigned char b)
@@ -420,7 +428,7 @@ void LaserScan::init(
 			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
-			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal || format == kXYZIRT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 		}
 	}

corelib/src/Odometry.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryORBSLAM3.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/odometry/OdometryFLOAM.h"
+#include "rtabmap/core/odometry/OdometryLIOSAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINSFusion.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
@@ -101,6 +102,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeFLOAM:
 		odometry = new OdometryFLOAM(parameters);
 		break;
+	case Odometry::kTypeLIOSAM:
+		odometry = new OdometryLIOSAM(parameters);
+		break;
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;
@@ -653,7 +657,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
-	else if(!guess.isNull()) {
+	else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) {
 		UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
 	}
 

corelib/src/Optimizer.cpp

@@ -240,7 +240,8 @@ void Optimizer::getConnectedGraph(
 {
 	UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
 	UASSERT(fromId>0);
-	UASSERT(uContains(posesIn, fromId));
+	UASSERT_MSG(uContains(posesIn, fromId), uFormat("poses=%ld (first=%d last=%d) fromId=%d",
+		posesIn.size(), posesIn.empty()?0:posesIn.begin()->first, posesIn.empty()?0:posesIn.rbegin()->first, fromId).c_str());
 
 	posesOut.clear();
 	linksOut.clear();

corelib/src/Parameters.cpp

@@ -903,6 +903,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With LIO-SAM:";
+#ifdef RTABMAP_LIOSAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With FOVIS:";
 #ifdef RTABMAP_FOVIS
@@ -1227,13 +1233,13 @@ void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, Pa
 				std::vector<std::string> version = uListToVector(uSplit((*iter).second, '.'));
 				if(version.size() == 3)
 				{
-					if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
+					if(RTABMAP_VERSION_COMPARE(<, std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
 					{
 						if(configFilePath.find(".rtabmap") != std::string::npos)
 						{
 							UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
-								   "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
-								   "to new version.",
+								   "current RTAB-Map version used (\"%s\"). The config file will be downgraded "
+								   "to current RTAB-Map version if saved.",
 								   configFilePath.c_str(),
 								   (*iter).second,
 								   RTABMAP_VERSION);

corelib/src/RtabmapThread.cpp

@@ -47,8 +47,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 		_rate(Parameters::defaultRtabmapDetectionRate()),
 		_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
-		_frameRateTimer(new UTimer()),
-		_previousStamp(0.0),
+		_previousStamp(-1.0),
 		_rtabmap(rtabmap),
 		_paused(false),
 		lastPose_(Transform::getIdentity())
@@ -62,8 +61,6 @@ RtabmapThread::~RtabmapThread()
 	UEventsManager::removeHandler(this);
 
 	close(true);
-
-	delete _frameRateTimer;
 }
 
 void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
@@ -88,7 +85,7 @@ void RtabmapThread::clearBufferedData()
 		_newMapEvents.clear();
 		lastPose_.setIdentity();
 		covariance_ = cv::Mat();
-		_previousStamp = 0;
+		_previousStamp = -1;
 	}
 	_dataMutex.unlock();
 
@@ -500,9 +497,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
-			if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
-				((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
+			if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate))
 			{
+				UDEBUG("Ignoring frame %f (previous stamp=%f, period=%f)",
+					odomEvent.data().stamp(), _previousStamp, 1.0/_rate);
 				ignoreFrame = true;
 			}
 		}
@@ -540,7 +538,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		else if(!ignoreFrame)
 		{
-			_frameRateTimer->start();
 			_previousStamp = odomEvent.data().stamp();
 		}
 
@@ -559,7 +556,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			// set negative id so rtabmap will detect it as an intermediate node
 			SensorData tmp = odomEvent.data();
 			tmp.setId(-1);
-			tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
 		}
 		else

corelib/src/SensorData.cpp

@@ -1005,7 +1005,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
 }
 
-void SensorData::clearCompressedData(bool images, bool scan, bool userData)
+void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
@@ -1021,14 +1021,24 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_userDataCompressed=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsCompressed=cv::Mat();
+		_emptyCellsCompressed=cv::Mat();
+		_obstacleCellsCompressed=cv::Mat();
+	}
 }
-void SensorData::clearRawData(bool images, bool scan, bool userData)
+void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
 		_imageRaw=cv::Mat();
 		_depthOrRightRaw=cv::Mat();
 		_depthConfidenceRaw=cv::Mat();
+#ifdef HAVE_OPENCV_CUDEV
+		_imageRawGpu = cv::cuda::GpuMat();
+		_depthOrRightRawGpu = cv::cuda::GpuMat();
+#endif
 	}
 	if(scan)
 	{
@@ -1038,6 +1048,12 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
 	{
 		_userDataRaw=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsRaw=cv::Mat();
+		_emptyCellsRaw=cv::Mat();
+		_obstacleCellsRaw=cv::Mat();
+	}
 }
 
 

corelib/src/global_map/OccupancyGrid.cpp

@@ -222,14 +222,14 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
 
 	if(!cache().empty())
 	{
-		UDEBUG("Updating from cache");
+		UDEBUG("Updating %ld poses from cache", newPoses.size());
 		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			if(uContains(cache(), iter->first))
 			{
 				const LocalGrid & localGrid = cache().at(iter->first);
 
-				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
+				//UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
 
 				//ground
 				cv::Mat ground;

corelib/src/odometry/OdometryLIOSAM.cpp

@@ -0,0 +1,478 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/odometry/OdometryLIOSAM.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+
+#ifdef RTABMAP_LIOSAM
+#include <LioSamCore.h>
+#include <pcl/common/transforms.h>
+#endif
+
+namespace rtabmap {
+
+static ParametersMap disableDeskewing(ParametersMap params) {
+	// LIO-SAM performs its own internal deskewing via imageProjection.
+	// The base-class deskew must be disabled so that the original per-point
+	// timestamps reach LIO-SAM intact.
+	params[Parameters::kOdomDeskewing()] = "false";
+	return params;
+}
+
+OdometryLIOSAM::OdometryLIOSAM(const ParametersMap & parameters) :
+	Odometry(disableDeskewing(parameters))
+#ifdef RTABMAP_LIOSAM
+	,lioSam_(0)
+	,lastPose_(Transform::getIdentity())
+	,lost_(false)
+	,linVar_(Parameters::defaultOdomLIOSAMLinVar())
+	,angVar_(Parameters::defaultOdomLIOSAMAngVar())
+	,parameters_(parameters)
+#endif
+{
+#ifdef RTABMAP_LIOSAM
+	Parameters::parse(parameters, Parameters::kOdomLIOSAMLinVar(), linVar_);
+	UASSERT(linVar_ > 0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLIOSAMAngVar(), angVar_);
+	UASSERT(angVar_ > 0.0f);
+#endif
+}
+
+OdometryLIOSAM::~OdometryLIOSAM()
+{
+#ifdef RTABMAP_LIOSAM
+	delete lioSam_;
+#endif
+}
+
+void OdometryLIOSAM::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_LIOSAM
+	if(lioSam_)
+	{
+		lioSam_->reset();
+	}
+	lastPose_ = Transform::getIdentity();
+	lost_ = false;
+	imuLocalTransform_ = Transform();
+	imuBuffer_.clear();
+#endif
+}
+
+#ifdef RTABMAP_LIOSAM
+bool OdometryLIOSAM::init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform)
+{
+	ParamServer config;
+
+	// Check if a config file path was provided
+	std::string configPath;
+	Parameters::parse(parameters_, Parameters::kOdomLIOSAMConfigPath(), configPath);
+	if(!configPath.empty())
+	{
+		configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
+		if(!UFile::exists(configPath))
+		{
+			UERROR("LIO-SAM config file not found: %s", configPath.c_str());
+			return false;
+		}
+		UINFO("Loading LIO-SAM parameters from config file: %s", configPath.c_str());
+		config = loadParamsFromYaml(configPath);
+	}
+	else
+	{
+		UINFO("No LIO-SAM config file provided, using rtabmap parameters");
+
+		// Build ParamServer from individual rtabmap parameters
+		int sensorType = Parameters::defaultOdomLIOSAMSensor();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMSensor(), sensorType);
+		if(sensorType == 1)
+			config.sensor = SensorType::OUSTER;
+		else if(sensorType == 2)
+			config.sensor = SensorType::LIVOX;
+		else
+			config.sensor = SensorType::VELODYNE;
+
+		config.N_SCAN = Parameters::defaultOdomLIOSAMNScan();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMNScan(), config.N_SCAN);
+
+		config.Horizon_SCAN = Parameters::defaultOdomLIOSAMHorizonScan();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMHorizonScan(), config.Horizon_SCAN);
+
+		config.imuAccNoise = Parameters::defaultOdomLIOSAMImuAccNoise();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccNoise(), config.imuAccNoise);
+
+		config.imuGyrNoise = Parameters::defaultOdomLIOSAMImuGyrNoise();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrNoise(), config.imuGyrNoise);
+
+		config.imuAccBiasN = Parameters::defaultOdomLIOSAMImuAccBiasN();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuAccBiasN(), config.imuAccBiasN);
+
+		config.imuGyrBiasN = Parameters::defaultOdomLIOSAMImuGyrBiasN();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGyrBiasN(), config.imuGyrBiasN);
+
+		config.imuGravity = Parameters::defaultOdomLIOSAMImuGravity();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMImuGravity(), config.imuGravity);
+
+		config.edgeThreshold = Parameters::defaultOdomLIOSAMEdgeThreshold();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMEdgeThreshold(), config.edgeThreshold);
+
+		config.surfThreshold = Parameters::defaultOdomLIOSAMSurfThreshold();
+		Parameters::parse(parameters_, Parameters::kOdomLIOSAMSurfThreshold(), config.surfThreshold);
+
+		// Set reasonable defaults for params not exposed via rtabmap
+		config.downsampleRate = 1;
+		config.lidarMinRange = 1.0f;
+		config.lidarMaxRange = 1000.0f;
+		config.imuRPYWeight = 0.01f;
+		config.odometrySurfLeafSize = 0.2f;
+		config.mappingCornerLeafSize = 0.2f;
+		config.mappingSurfLeafSize = 0.4f;
+		config.z_tollerance = FLT_MAX;
+		config.rotation_tollerance = FLT_MAX;
+		config.numberOfCores = 4;
+		config.mappingProcessInterval = 0.01;
+		config.surroundingkeyframeAddingDistThreshold = 1.0f;
+		config.surroundingkeyframeAddingAngleThreshold = 0.2f;
+		config.surroundingKeyframeDensity = 1.0f;
+		config.surroundingKeyframeSearchRadius = 50.0f;
+		config.loopClosureEnableFlag = false;  // rtabmap handles loop closures
+		config.loopClosureFrequency = 1.0f;
+		config.surroundingKeyframeSize = 50;
+		config.historyKeyframeSearchRadius = 10.0f;
+		config.historyKeyframeSearchTimeDiff = 30.0f;
+		config.historyKeyframeSearchNum = 25;
+		config.historyKeyframeFitnessScore = 0.3f;
+		config.globalMapVisualizationSearchRadius = 1e3f;
+		config.globalMapVisualizationPoseDensity = 10.0f;
+		config.globalMapVisualizationLeafSize = 1.0f;
+		config.edgeFeatureMinValidNum = 10;
+		config.surfFeatureMinValidNum = 100;
+		config.savePCD = false;
+		config.useImuHeadingInitialization = false;
+		config.useGpsElevation = false;
+		config.gpsCovThreshold = 2.0f;
+		config.poseCovThreshold = 25.0f;
+	}
+
+	// Always override extrinsics from sensor local transforms when available.
+	// This ensures the IMU-to-lidar transform matches the actual sensor setup
+	// regardless of what the config file says.
+	//   imuLocalTransform   = T_base_imu   (base_link -> imu_link)
+	//   lidarLocalTransform = T_base_lidar  (base_link -> lidar_link)
+	// LIO-SAM's imuConverter() expects T_lidar_imu:
+	//   T_lidar_imu = T_base_lidar^{-1} * T_base_imu
+	if(!imuLocalTransform.isNull() && !lidarLocalTransform.isNull())
+	{
+		Transform T_lidar_imu = lidarLocalTransform.inverse() * imuLocalTransform;
+		Eigen::Matrix4d T = T_lidar_imu.toEigen4d();
+		Eigen::Matrix3d rot = T.block<3,3>(0,0);
+		Eigen::Vector3d trans = T.block<3,1>(0,3);
+		config.extRotV = {rot(0,0), rot(0,1), rot(0,2),
+		                  rot(1,0), rot(1,1), rot(1,2),
+		                  rot(2,0), rot(2,1), rot(2,2)};
+		config.extRPYV = config.extRotV;
+		config.extTransV = {trans(0), trans(1), trans(2)};
+		UINFO("LIO-SAM extrinsics (T_lidar_imu) computed from sensor local transforms: %s", T_lidar_imu.prettyPrint().c_str());
+	}
+	else if(config.extRotV.size() != 9 || config.extTransV.size() != 3)
+	{
+		// No valid extrinsics from sensor data or config file
+		UERROR("Cannot compute IMU-to-lidar extrinsics: IMU local transform %s, lidar local transform %s. "
+		       "Both must be valid, or the config file must contain valid extrinsics.",
+		       imuLocalTransform.isNull() ? "is null" : "is valid",
+		       lidarLocalTransform.isNull() ? "is null" : "is valid");
+		return false;
+	}
+	else
+	{
+		UINFO("Using extrinsics from config file (sensor local transforms not available)");
+	}
+
+	// Set the global extrinsics used by imuConverter
+	extRot = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(config.extRotV.data());
+	extRPY = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >(config.extRPYV.data());
+	extTrans = Eigen::Map<const Eigen::Matrix<double, 3, 1> >(config.extTransV.data());
+	extQRPY = Eigen::Quaterniond(extRPY).inverse();
+
+	lioSam_ = new lio_sam::LioSamCore(config);
+
+	// Replay buffered IMU samples
+	UINFO("Replaying %d buffered IMU samples into LIO-SAM", (int)imuBuffer_.size());
+	for(const ImuSample & s : imuBuffer_)
+	{
+		lioSam_->addImu(s.stamp, s.acc, s.gyro, s.orientation);
+	}
+	imuBuffer_.clear();
+
+	return true;
+}
+#endif
+
+Transform OdometryLIOSAM::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_LIOSAM
+	UTimer timer;
+	UTimer timerTotal;
+
+	// Handle async IMU data (canProcessAsyncIMU() == true means
+	// the base class sends IMU-only data directly to computeTransform)
+	if(!data.imu().empty())
+	{
+		Eigen::Quaterniond qd(
+			data.imu().orientation()[3],  // w
+			data.imu().orientation()[0],  // x
+			data.imu().orientation()[1],  // y
+			data.imu().orientation()[2]); // z
+		Eigen::Vector3d acc(
+			data.imu().linearAcceleration()[0],
+			data.imu().linearAcceleration()[1],
+			data.imu().linearAcceleration()[2]);
+		Eigen::Vector3d gyro(
+			data.imu().angularVelocity()[0],
+			data.imu().angularVelocity()[1],
+			data.imu().angularVelocity()[2]);
+
+		// Deferred initialization: need both IMU and lidar local transforms
+		// to compute T_lidar_imu extrinsics for LIO-SAM.
+		if(!lioSam_)
+		{
+			// Cache IMU local transform when first available
+			if(imuLocalTransform_.isNull() && !data.imu().localTransform().isNull())
+			{
+				imuLocalTransform_ = data.imu().localTransform();
+			}
+
+			// Try to initialize if we have both transforms
+			if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() &&
+			   !data.laserScanRaw().localTransform().isNull())
+			{
+				if(!init(imuLocalTransform_, data.laserScanRaw().localTransform()))
+				{
+					UERROR("Failed to initialize LIO-SAM");
+					return t;
+				}
+			}
+			else
+			{
+				// Buffer IMU until we can initialize
+				ImuSample s;
+				s.stamp = data.stamp();
+				s.acc = acc;
+				s.gyro = gyro;
+				s.orientation = qd;
+				imuBuffer_.push_back(s);
+
+				if(data.laserScanRaw().isEmpty())
+				{
+					return t;
+				}
+			}
+		}
+
+		if(lioSam_)
+		{
+			lioSam_->addImu(data.stamp(), acc, gyro, qd);
+		}
+
+		// IMU-only: no pose to return
+		if(data.laserScanRaw().isEmpty())
+		{
+			return t;
+		}
+	}
+
+	if(!lioSam_)
+	{
+		// A scan arrived without IMU in the same message.
+		// Try to init if the IMU local transform was already cached.
+		if(!imuLocalTransform_.isNull() && !data.laserScanRaw().isEmpty() &&
+		   !data.laserScanRaw().localTransform().isNull())
+		{
+			if(!init(imuLocalTransform_, data.laserScanRaw().localTransform()))
+			{
+				UERROR("Failed to initialize LIO-SAM");
+				return t;
+			}
+		}
+		else
+		{
+			UDEBUG("LIO-SAM not yet initialized, waiting for IMU (have=%s) and lidar (need scan) local transforms...",
+				imuLocalTransform_.isNull() ? "no" : "yes");
+			return t;
+		}
+	}
+
+	if(data.laserScanRaw().isEmpty())
+	{
+		UERROR("LIO-SAM requires laser scans and the current input is empty. Aborting odometry update...");
+		return t;
+	}
+	else if(data.laserScanRaw().is2d())
+	{
+		UERROR("LIO-SAM requires 3D laser scans. Aborting odometry update...");
+		return t;
+	}
+
+	cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999;
+	if(!lost_)
+	{
+		const LaserScan & scan = data.laserScanRaw();
+		if(scan.format() != LaserScan::kXYZIRT)
+		{
+			UERROR("LIO-SAM requires a scan in format %s (got %s). "
+			       "Populate the scan via util3d::laserScanFromPointCloud<PointXYZIRT>() "
+			       "so that per-point ring and time fields are available.",
+			       LaserScan::formatName(LaserScan::kXYZIRT).c_str(),
+			       scan.formatName().c_str());
+			return t;
+		}
+
+		// Split the kXYZIRT scan into the three parallel buffers LIO-SAM expects.
+		const int numPoints = scan.size();
+		const int ringOffset = scan.getRingOffset();
+		const int timeOffset = scan.getTimeOffset();
+		pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZI>);
+		laserCloudIn->reserve(numPoints);
+		std::vector<int> rings;
+		std::vector<float> times;
+		rings.reserve(numPoints);
+		times.reserve(numPoints);
+		for(int i=0; i<numPoints; ++i)
+		{
+			const int row = i / scan.data().cols;
+			const int col = i - row * scan.data().cols;
+			const float * ptr = scan.data().ptr<float>(row, col);
+			pcl::PointXYZI pt;
+			pt.x = ptr[0];
+			pt.y = ptr[1];
+			pt.z = ptr[2];
+			pt.intensity = ptr[3];
+			laserCloudIn->push_back(pt);
+			rings.push_back(static_cast<int>(ptr[ringOffset]));
+			times.push_back(ptr[timeOffset]);
+		}
+		UDEBUG("Scan split: %fs, points=%d", timer.ticks(), (int)laserCloudIn->size());
+
+		// Process scan. Retrieve the deskewed (motion-compensated) cloud
+		// produced by LIO-SAM's image projection stage so we can propagate
+		// it back into SensorData: otherwise downstream consumers such as
+		// loop closure registration would still see the raw pre-deskew scan.
+		Eigen::Affine3f poseOut;
+		Eigen::MatrixXd covOut;
+		pcl::PointCloud<pcl::PointXYZI>::Ptr deskewedCloud(new pcl::PointCloud<pcl::PointXYZI>);
+		bool ok = lioSam_->processScan(data.stamp(), laserCloudIn, rings, times, poseOut, covOut, deskewedCloud);
+		UDEBUG("LIO-SAM process: %fs", timer.ticks());
+
+		if(ok)
+		{
+			// Replace the raw scan on SensorData with LIO-SAM's deskewed
+			// cloud so downstream stages (loop closure registration in
+			// particular) use the motion-compensated points instead of
+			// the raw pre-deskew scan. The deskewed cloud is still in the
+			// lidar frame, so the existing localTransform/rangeMax apply.
+			if(deskewedCloud && !deskewedCloud->empty())
+			{
+				const LaserScan & rawScan = data.laserScanRaw();
+				LaserScan deskewedScan(
+					util3d::laserScanFromPointCloud(*deskewedCloud),
+					rawScan.maxPoints(),
+					rawScan.rangeMax(),
+					rawScan.localTransform());
+				data.setLaserScan(deskewedScan);
+				UDEBUG("Replaced raw scan with deskewed cloud (%d -> %d points)",
+					(int)laserCloudIn->size(), (int)deskewedCloud->size());
+			}
+
+			Transform pose = Transform::fromEigen3f(poseOut);
+
+			if(!pose.isNull())
+			{
+				covariance = cv::Mat::eye(6, 6, CV_64FC1);
+				covariance(cv::Range(0, 3), cv::Range(0, 3)) *= linVar_;
+				covariance(cv::Range(3, 6), cv::Range(3, 6)) *= angVar_;
+
+				t = lastPose_.inverse() * pose;  // incremental
+				lastPose_ = pose;
+
+				const Transform & localTransform = data.laserScanRaw().localTransform();
+				if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+				{
+					// from laser frame to base frame
+					t = localTransform * t * localTransform.inverse();
+				}
+
+				if(info)
+				{
+					info->type = (int)kTypeLIOSAM;
+					if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
+					{
+						info->reg.covariance = covariance;
+					}
+
+					if(this->isInfoDataFilled())
+					{
+						pcl::PointCloud<pcl::PointXYZI>::Ptr localMap = lioSam_->getLocalMap();
+						if(localMap && !localMap->empty())
+						{
+							info->localScanMapSize = localMap->size();
+							info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
+						}
+						UDEBUG("Fill info data: %fs", timer.ticks());
+					}
+				}
+			}
+			else
+			{
+				lost_ = true;
+				UWARN("LIO-SAM failed to register the latest scan, odometry should be reset.");
+			}
+		}
+		else
+		{
+			UDEBUG("LIO-SAM processScan returned false (may be initializing)");
+		}
+	}
+	UINFO("LIO-SAM odom update time = %fs, lost=%s", timerTotal.elapsed(), lost_ ? "true" : "false");
+
+#else
+	UERROR("RTAB-Map is not built with LIO-SAM support! Select another odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap

corelib/src/odometry/OdometryMono.cpp

@@ -239,7 +239,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		{
 			UDEBUG("");
 			bool newPtsAdded = false;
-			const Signature * newS = memory_->getLastWorkingSignature();
+			const Signature * newS = memory_->getLastWorkingSignature(false);
 			UDEBUG("newWords=%d", (int)newS->getWords().size());
 			nFeatures = (int)newS->getWords().size();
 			if((int)newS->getWords().size() > minInliers_)
@@ -646,7 +646,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			info->type = 1;
 		}
 
-		const Signature * refS = memory_->getLastWorkingSignature();
+		const Signature * refS = memory_->getLastWorkingSignature(false);
 
 		std::vector<cv::Point2f> refCorners(firstFrameGuessCorners_.size());
 		std::vector<cv::Point2f> refCornersGuess(firstFrameGuessCorners_.size());
@@ -804,10 +804,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 				if(!refWords3.empty())
 				{
 					UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size());
-					keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3));
+					keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), refWords3));
 				}
-				keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
-				keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), newModel));
+				keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), this->getPose()));
+				keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), newModel));
 			}
 		}
 		else
@@ -829,7 +829,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			// generate kpts
 		if(memory_->update(SensorData(data)))
 		{
-			const Signature * s = memory_->getLastWorkingSignature();
+			const Signature * s = memory_->getLastWorkingSignature(false);
 			const std::multimap<int, int> & words = s->getWords();
 			if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
 			{

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/Values.h>
-#include "gtsam/GravityFactor.h"
+#include <gtsam/navigation/AttitudeFactor.h>
 #include <optimizer/gtsam/XYFactor.h>
 #include <optimizer/gtsam/XYZFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
@@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
 		params.relinearizeThreshold = threshold;
 		params.relinearizeSkip = skip;
 		params.evaluateNonlinearError = true;
-		isam2_ = new ISAM2(params);
+		isam2_ = new gtsam::ISAM2(params);
 
 		addedPoses_.clear();
 		lastAddedConstraints_.clear();
@@ -379,8 +379,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			int id1 = iter->second.from();
 			int id2 = iter->second.to();
 
-            UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str());
-            UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str());
+            UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d for constraint %d->%d (type=%d)", id1, id1, id2, iter->second.type()).c_str());
+            UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d for constraint %d->%d (type=%d)", id2, id1, id2, iter->second.type()).c_str());
 
 			UASSERT(!iter->second.transform().isNull());
 			if(id1 == id2)
@@ -392,7 +392,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					{
 						if(id1 < 0 && !isLandmarkWithRotation.at(id1))
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
 									1/iter->second.infMatrix().at<double>(0,0),
 									1/iter->second.infMatrix().at<double>(1,1)));
 							graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -400,7 +400,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						}
 						else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
 									1/iter->second.infMatrix().at<double>(0,0),
 									1/iter->second.infMatrix().at<double>(1,1)));
 							graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -431,7 +431,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					{
 						if(id1 < 0 && !isLandmarkWithRotation.at(id1))
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
 										iter->second.infMatrix().at<double>(0,0),
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
@@ -442,7 +442,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 							1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
 							1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 						{
-							noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
+							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
 										iter->second.infMatrix().at<double>(0,0),
 										iter->second.infMatrix().at<double>(1,1),
 										iter->second.infMatrix().at<double>(2,2)));
@@ -471,10 +471,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 				else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
 				{
-					Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
-					gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
-					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
-					graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
+					gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation();
+					gtsam::Unit3 nZ(0,0,1);
+					gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
+					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
+#if GTSAM_VERSION_NUMERIC <= 40300
+					// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
+					//       Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
+					graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
+#else
+					graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
+#endif
 					lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
 				}
 			}
@@ -783,7 +790,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				float x,y,z,roll,pitch,yaw;
 				std::map<int, Transform> tmpPoses;
-				const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
+				const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
 #if GTSAM_VERSION_NUMERIC >= 40200
 				for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
 #else

corelib/src/optimizer/gtsam/GravityFactor.cpp

@@ -1,91 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * Author: Mathieu Labbe
- * This file is a copy of AttitudeFactor.cpp of gtsam library but
- * with attitudeError() function overridden to ignore yaw errors.
- * For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
- */
-
-/**
- *  @file   GravityFactor.cpp
- *  @author Frank Dellaert
- *  @brief  Implementation file for Attitude factor
- *  @date   January 28, 2014
- **/
-
-#include "GravityFactor.h"
-
-using namespace std;
-
-namespace rtabmap {
-
-//***************************************************************************
-Vector GravityFactor::attitudeError(const Rot3& nRb,
-    OptionalJacobian<2, 3> H) const {
-  if (H) {
-    Matrix23 D_nRef_R;
-    Matrix22 D_e_nRef;
-    Vector3 r = nRb.xyz();
-    Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
-    Vector e = nZ_.error(nRef, D_e_nRef);
-    (*H) = D_e_nRef * D_nRef_R;
-    //printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
-    //		nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
-	//		(*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
-    return e;
-  } else {
-    Vector3 r = nRb.xyz();
-    Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
-    Vector e = nZ_.error(nRef);
-    //printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
-    return e;
-  }
-}
-
-//***************************************************************************
-void Rot3GravityFactor::print(const string& s,
-    const KeyFormatter& keyFormatter) const {
-  cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
-  nZ_.print("  measured direction in nav frame: ");
-  bRef_.print("  reference direction in body frame: ");
-  this->noiseModel_->print("  noise model: ");
-}
-
-//***************************************************************************
-bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
-    double tol) const {
-  const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
-      && this->bRef_.equals(e->bRef_, tol);
-}
-
-//***************************************************************************
-void Pose3GravityFactor::print(const string& s,
-    const KeyFormatter& keyFormatter) const {
-  cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
-  nZ_.print("  measured direction in nav frame: ");
-  bRef_.print("  reference direction in body frame: ");
-  this->noiseModel_->print("  noise model: ");
-}
-
-//***************************************************************************
-bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
-    double tol) const {
-  const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
-      && this->bRef_.equals(e->bRef_, tol);
-}
-
-//***************************************************************************
-
-}/// namespace gtsam

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -1,271 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * Author: Mathieu Labbe
- * This file is a copy of AttitudeFactor.h of gtsam library but
- * with attitudeError() function overridden to ignore yaw errors.
- * For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
- */
-
-/**
- *  @file   Pose3GravityFactor.h
- *  @author Frank Dellaert
- *  @brief  Header file for Attitude factor
- *  @date   January 28, 2014
- **/
-#pragma once
-
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
-#include <gtsam/nonlinear/NoiseModelFactorN.h>
-#endif
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Unit3.h>
-
-using namespace gtsam;
-
-namespace rtabmap {
-
-/**
- * Base class for prior on gravity
- * Example:
- * - measurement is direction of gravity in navigation frame nG
- * - reference is direction of z axis in body frame bF
- * This factor will give zero error if nG is opposite direction of bF
- * @addtogroup Navigation
- */
-class GravityFactor {
-
-protected:
-
-  const Unit3 nZ_, bRef_; ///< Position measurement in
-
-public:
-
-  /** default constructor - only use for serialization */
-  GravityFactor() {
-  }
-
-  /**
-   * @brief Constructor
-   * @param nZ measured direction in navigation frame
-   * @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
-   */
-  GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
-      nZ_(nZ), bRef_(bRef) {
-  }
-
-  /** vector of errors */
-  Vector attitudeError(const Rot3& p,
-#if GTSAM_VERSION_NUMERIC >= 40300
-      OptionalJacobian<2,3> H = {}) const;
-#else
-  	  OptionalJacobian<2,3> H = boost::none) const;
-#endif
-
-  /** Serialization function */
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
-    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
-  }
-#endif
-};
-
-/**
- * Version of GravityFactor for Rot3
- * @addtogroup Navigation
- */
-class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
-
-  typedef NoiseModelFactor1<Rot3> Base;
-
-public:
-
-  /// shorthand for a smart pointer to a factor
-#if GTSAM_VERSION_NUMERIC >= 40300
-  typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
-  #else
-  typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
-#endif
-
-  /// Typedef to this class
-  typedef Rot3GravityFactor This;
-
-  /** default constructor - only use for serialization */
-  Rot3GravityFactor() {
-  }
-
-  virtual ~Rot3GravityFactor() {
-  }
-
-  /**
-   * @brief Constructor
-   * @param key of the Rot3 variable that will be constrained
-   * @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
-   * @param model Gaussian noise model
-   * @param bRef reference direction in body frame (default Z-axis)
-   */
-  Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
-      const Unit3& bRef = Unit3(0, 0, 1)) :
-      Base(model, key), GravityFactor(nZ, bRef) {
-  }
-
-  /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-#if GTSAM_VERSION_NUMERIC >= 40300
-    return std::static_pointer_cast<gtsam::NonlinearFactor>(
-#else
-    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-#endif
-        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
-  }
-
-  /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
-
-  /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-
-  /** vector of errors */
-  virtual Vector evaluateError(const Rot3& nRb, //
-#if GTSAM_VERSION_NUMERIC >= 40300
-	OptionalMatrixType H = OptionalNone) const {
-#else
-	boost::optional<Matrix&> H = boost::none) const {
-#endif
-    return attitudeError(nRb, H);
-  }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
-
-private:
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
-        boost::serialization::base_object<Base>(*this));
-    ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));*/
-  }
-#endif
-
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-
-/**
- * Version of GravityFactor for Pose3
- * @addtogroup Navigation
- */
-class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
-    public GravityFactor {
-
-  typedef NoiseModelFactor1<Pose3> Base;
-
-public:
-
-  /// shorthand for a smart pointer to a factor
-#if GTSAM_VERSION_NUMERIC >= 40300
-  typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
-#else
-  typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
-#endif
-  /// Typedef to this class
-  typedef Pose3GravityFactor This;
-
-  /** default constructor - only use for serialization */
-  Pose3GravityFactor() {
-  }
-
-  virtual ~Pose3GravityFactor() {
-  }
-
-  /**
-   * @brief Constructor
-   * @param key of the Pose3 variable that will be constrained
-   * @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
-   * @param model Gaussian noise model
-   * @param bRef reference direction in body frame (default Z-axis)
-   */
-  Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
-      const Unit3& bRef = Unit3(0, 0, 1)) :
-      Base(model, key), GravityFactor(nZ, bRef) {
-  }
-
-  /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-#if GTSAM_VERSION_NUMERIC >= 40300
-    return std::static_pointer_cast<gtsam::NonlinearFactor>(
-#else
-    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-#endif
-        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
-  }
-
-  /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
-
-  /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-
-  /** vector of errors */
-  virtual Vector evaluateError(const Pose3& nTb, //
-#if GTSAM_VERSION_NUMERIC >= 40300
-      OptionalMatrixType H = OptionalNone) const {
-#else
-      boost::optional<Matrix&> H = boost::none) const {
-#endif
-    Vector e = attitudeError(nTb.rotation(), H);
-    if (H) {
-      Matrix H23 = *H;
-      *H = Matrix::Zero(2,6);
-      H->block<2,3>(0,0) = H23;
-    }
-    return e;
-  }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
-
-private:
-#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
-  /** Serialization function */
-  friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    /*ar & boost::serialization::make_nvp("NoiseModelFactor1",
-        boost::serialization::base_object<Base>(*this));
-    ar & boost::serialization::make_nvp("GravityFactor",
-        boost::serialization::base_object<GravityFactor>(*this));*/
-  }
-#endif
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-} /// namespace gtsam
-

corelib/src/optimizer/gtsam/XYFactor.h

@@ -43,7 +43,7 @@ public:
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
   gtsam::Vector evaluateError(const VALUE& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -59,5 +59,5 @@ public:
 
 };
 
-} // namespace gtsamexamples
+} // namespace rtabmap
 

corelib/src/optimizer/gtsam/XYZFactor.h

@@ -43,7 +43,7 @@ public:
   // @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
   gtsam::Vector evaluateError(const gtsam::Pose3& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -55,7 +55,7 @@ public:
   }
   gtsam::Vector evaluateError(const gtsam::Point3& p,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H = OptionalNone) const {
+		  gtsam::OptionalMatrixType H = OptionalNone) const {
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
@@ -63,5 +63,5 @@ public:
   }
 };
 
-} // namespace gtsamexamples
+} // namespace rtabmap
 

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -31,9 +31,9 @@ namespace vertigo {
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H1 = OptionalNone,
-		  OptionalMatrixType H2 = OptionalNone,
-		  OptionalMatrixType H3 = OptionalNone) const
+		  gtsam::OptionalMatrixType H1 = OptionalNone,
+		  gtsam::OptionalMatrixType H2 = OptionalNone,
+		  gtsam::OptionalMatrixType H3 = OptionalNone) const
 #else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
           boost::optional<gtsam::Matrix&> H2 = boost::none,
@@ -71,9 +71,9 @@ namespace vertigo {
 
       gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		  OptionalMatrixType H1 = OptionalNone,
-		  OptionalMatrixType H2 = OptionalNone,
-		  OptionalMatrixType H3 = OptionalNone) const
+		  gtsam::OptionalMatrixType H1 = OptionalNone,
+		  gtsam::OptionalMatrixType H2 = OptionalNone,
+		  gtsam::OptionalMatrixType H3 = OptionalNone) const
 #else
           boost::optional<gtsam::Matrix&> H1 = boost::none,
           boost::optional<gtsam::Matrix&> H2 = boost::none,

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -13,6 +13,7 @@
 // DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
 
@@ -77,8 +78,8 @@ namespace vertigo {
     /** between operation */
     inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
 #if GTSAM_VERSION_NUMERIC >= 40300
-		OptionalMatrixType H1=OptionalNone,
-		OptionalMatrixType H2=OptionalNone) const {
+		gtsam::OptionalMatrixType H1=OptionalNone,
+		gtsam::OptionalMatrixType H2=OptionalNone) const {
 #else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -13,6 +13,7 @@
 // DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
 #include "DerivedValue.h"
 #include <gtsam/base/Lie.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace vertigo {
 
@@ -77,8 +78,8 @@ namespace vertigo {
     /** between operation */
     inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
 #if GTSAM_VERSION_NUMERIC >= 40300
-        OptionalMatrixType H1=OptionalNone,
-		OptionalMatrixType H2=OptionalNone) const {
+        gtsam::OptionalMatrixType H1=OptionalNone,
+		    gtsam::OptionalMatrixType H2=OptionalNone) const {
 #else
         boost::optional<gtsam::Matrix&> H1=boost::none,
         boost::optional<gtsam::Matrix&> H2=boost::none) const {

corelib/src/python/PythonInterface.cpp

@@ -19,9 +19,11 @@ PythonInterface::PythonInterface()
 	guard_ = new pybind11::scoped_interpreter();
 
 	// Tell Python to look in this directory for DLLs
+#ifdef _WIN32
 	std::string exe_dir = std::filesystem::current_path().string();
     pybind11::module_ os = pybind11::module_::import("os");
     os.attr("add_dll_directory")(exe_dir);
+#endif
 
 	pybind11::module::import("threading");
 	release_ = new pybind11::gil_scoped_release();

corelib/src/util3d.cpp

@@ -1699,6 +1699,55 @@ LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
 }
 
+LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform, bool filterNaNs)
+{
+	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
+}
+
+LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
+{
+	// Layout: [x, y, z, intensity, ring, time] (ring cast to float, values up to
+	// ~16M are exactly representable so all realistic laser line counts fit).
+	cv::Mat laserScan;
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi = 0;
+	const int total = indices.get() ? (int)indices->size() : (int)cloud.size();
+	laserScan = cv::Mat(1, total, CV_32FC(6));
+	for(int i=0; i<total; ++i)
+	{
+		int index = indices.get() ? indices->at(i) : i;
+		const rtabmap::PointXYZIRT & src = cloud.at(index);
+		if(filterNaNs && !pcl::isFinite(src))
+		{
+			continue;
+		}
+		float * ptr = laserScan.ptr<float>(0, oi++);
+		if(!nullTransform)
+		{
+			pcl::PointXYZ pt(src.x, src.y, src.z);
+			pt = pcl::transformPoint(pt, transform3f);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.z;
+		}
+		else
+		{
+			ptr[0] = src.x;
+			ptr[1] = src.y;
+			ptr[2] = src.z;
+		}
+		ptr[3] = src.intensity;
+		ptr[4] = static_cast<float>(src.ring);
+		ptr[5] = src.time;
+	}
+	if(oi == 0)
+	{
+		return LaserScan();
+	}
+	return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZIRT);
+}
+
 LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
 {
 	cv::Mat laserScan;
@@ -2280,7 +2329,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
 	}
@@ -3738,9 +3787,11 @@ LaserScan deskew(
 		return LaserScan();
 	}
 
-	if(input.format() != LaserScan::kXYZIT)
+	if(!input.hasTime())
 	{
-		UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
+		UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
+			LaserScan::formatName(LaserScan::kXYZIT).c_str(),
+			LaserScan::formatName(LaserScan::kXYZIRT).c_str());
 		return LaserScan();
 	}
 
@@ -3796,7 +3847,14 @@ LaserScan deskew(
 	double stamp;
 	UTimer processingTime;
 	double scanTime = lastStamp - firstStamp;
-	cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
+	// Preserve ring when input carries it (kXYZIRT): the geometric channel is
+	// still meaningful after deskewing. Per-point time is zeroed because all
+	// points share the same pose after correction.
+	const bool preserveRing = input.hasRing();
+	const int offsetRing = input.getRingOffset();
+	const LaserScan::Format outputFormat = preserveRing ? LaserScan::kXYZIRT : LaserScan::kXYZI;
+	const int outputChannels = preserveRing ? 6 : 4;
+	cv::Mat output(1, input.size(), CV_32FC(outputChannels));
 	int offsetIntensity = input.getIntensityOffset();
 	bool isLocalTransformIdentity = input.localTransform().isIdentity();
 	Transform localTransformInv = input.localTransform().inverse();
@@ -3835,7 +3893,12 @@ LaserScan deskew(
 					dataPtr[0] = pt.x;
 					dataPtr[1] = pt.y;
 					dataPtr[2] = pt.z;
-					dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
+					dataPtr[3] = inputPtr[offsetIntensity];
+					if(preserveRing)
+					{
+						dataPtr[4] = inputPtr[offsetRing];
+						dataPtr[5] = 0.0f;
+					}
 				}
 			}
 		}
@@ -3872,14 +3935,19 @@ LaserScan deskew(
 					dataPtr[0] = pt.x;
 					dataPtr[1] = pt.y;
 					dataPtr[2] = pt.z;
-					dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
+					dataPtr[3] = inputPtr[offsetIntensity];
+					if(preserveRing)
+					{
+						dataPtr[4] = inputPtr[offsetRing];
+						dataPtr[5] = 0.0f;
+					}
 				}
 			}
 		}
 	}
 	output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
 	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
-	return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
+	return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
 }