Add LIO-SAM as an odometry strategy (#1684)

* Added liosam odometry integration * Add kXYZIRT scan format with per-point ring channel for LIO-SAM integration Introduce PointXYZIRT point type and kXYZIRT LaserScan format (x,y,z, intensity,ring,time) so that ring indices survive the scan pipeline. Update OdometryLIOSAM to require kXYZIRT and properly split ring/time into the parallel buffers LIO-SAM expects. Extend deskewing to preserve ring data and disable base-class deskew in OdometryLIOSAM since LIO-SAM handles it internally. * Address PR review: config file, deferred init, and GUI panel for LIO-SAM - Add OdomLIOSAM/ConfigPath parameter to load LIO-SAM settings from a YAML file. When set, individual params are ignored. Extrinsics from sensor local transforms always override config file values. - Defer LioSamCore initialization until both IMU and lidar local transforms are available, computing T_lidar_imu from sensor data. IMU samples are buffered and replayed after init. - Fix deferred init for scan-only messages that arrive after IMU local transform is already cached. - Add LIO-SAM entry to odometry strategy combo box (index 14) with full PreferencesDialog panel including config path browse button and all parameter widgets. * OdometryLIOSAM: propagate deskewed scan to SensorData Capture the deskewed cloud produced by LIO-SAM's image projection stage and replace the raw scan on SensorData with it, so loop closure registration and other downstream stages operate on the motion- compensated points instead of the raw pre-deskew input. * Minor updates for rtabmap_ros --------- Co-authored-by: matlabbe <matlabbe@gmail.com>
2026-09-02 09:30:25 +08:00 · 2026-04-12 20:06:44 -05:00
parent 8fd701aabe
commit a9f63bd5fd
17 changed files with 1316 additions and 29 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/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}
--- a/corelib/src/LaserScan.cpp
+++ b/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;
 }

 LaserScan LaserScan::backwardCompatibility(
@@ -404,7 +412,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());
 		}
 	}
--- a/corelib/src/Odometry.cpp
+++ b/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;
--- a/corelib/src/Parameters.cpp
+++ b/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
--- a/corelib/src/odometry/OdometryLIOSAM.cpp
+++ b/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
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -1762,6 +1762,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;
@@ -2343,7 +2392,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);
 	}
@@ -3807,9 +3856,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();
 	}

@@ -3865,7 +3916,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();
@@ -3904,7 +3962,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;
+					}
 				}
 			}
 		}
@@ -3941,14 +4004,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());
 }