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 <[email protected]>
2026-10-07 18:47:48 +08:00 · 2026-04-12 18:06:44 -07:00
parent 8fd701aabe
commit a9f63bd5fd
17 changed files with 1316 additions and 29 deletions
@@ -48,7 +48,8 @@ public:
 		kXYZNormal=8,
 		kXYZINormal=9,
 		kXYZRGBNormal=10,
-		kXYZIT=11};
+		kXYZIT=11,
+		kXYZIRT=12};

 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
@@ -57,6 +58,7 @@ public:
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
 	static bool isScanHasTime(const Format & format);
+	static bool isScanHasRing(const Format & format);
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
@@ -135,6 +137,7 @@ public:
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
 	bool hasTime() const {return isScanHasTime(format_);}
+	bool hasRing() const {return isScanHasRing(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
 	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
@@ -143,7 +146,8 @@ public:
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
-	int getTimeOffset() const {return hasTime()?4:-1;}
+	int getRingOffset() const {return format_==kXYZIRT?4:-1;}
+	int getTimeOffset() const {return format_==kXYZIT?4:(format_==kXYZIRT?5:-1);}

 	float & field(unsigned int pointIndex, unsigned int channelOffset);

@@ -57,7 +57,8 @@ public:
 		kTypeOpenVINS = 10,
 		kTypeFLOAM = 11,
 		kTypeOpen3D = 12,
-		kTypeCuVSLAM = 13
+		kTypeCuVSLAM = 13,
+		kTypeLIOSAM = 14
 	};

 public:
@@ -465,7 +465,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");

    // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM 14=LIO-SAM");
    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
    RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
    RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
@@ -687,6 +687,21 @@ class RTABMAP_CORE_EXPORT Parameters
    // Odometry cuVSLAM
    RTABMAP_PARAM(OdomCuVSLAM, MulticamMode,        int, 0,  "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");

+    // Odometry LIO-SAM
+    RTABMAP_PARAM_STR(OdomLIOSAM, ConfigPath,  "", "Path to LIO-SAM params.yaml config file. When set, sensor/IMU/feature parameters are loaded from the file and the individual parameters below are ignored.");
+    RTABMAP_PARAM(OdomLIOSAM, Sensor,       int,   0,     "LiDAR sensor: 0=Velodyne, 1=Ouster, 2=Livox");
+    RTABMAP_PARAM(OdomLIOSAM, NScan,        int,   16,    "Number of LiDAR channels (16, 32, 64, 128).");
+    RTABMAP_PARAM(OdomLIOSAM, HorizonScan,  int,   1800,  "Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048).");
+    RTABMAP_PARAM(OdomLIOSAM, ImuAccNoise,  float, 0.01,  "IMU accelerometer white noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGyrNoise,  float, 0.001, "IMU gyroscope white noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuAccBiasN,  float, 0.0002,"IMU accelerometer bias noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGyrBiasN,  float, 0.00003,"IMU gyroscope bias noise.");
+    RTABMAP_PARAM(OdomLIOSAM, ImuGravity,   float, 9.80511,"Gravity magnitude.");
+    RTABMAP_PARAM(OdomLIOSAM, EdgeThreshold,float, 1.0,   "Edge feature curvature threshold.");
+    RTABMAP_PARAM(OdomLIOSAM, SurfThreshold,float, 0.1,   "Surface feature curvature threshold.");
+    RTABMAP_PARAM(OdomLIOSAM, LinVar,       float, 0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLIOSAM, AngVar,       float, 0.01,  "Angular output variance.");
+
    // Common registration parameters
    RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
    RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
@@ -41,11 +41,11 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 		return LaserScan();
 	}
 	//determine the output type
-	int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
+	int fieldStates[10] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity,time,ring
 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
-	std::uint32_t fieldOffsets[8] = {0};
+	std::uint32_t fieldOffsets[10] = {0};
 #else
-	pcl::uint32_t fieldOffsets[8] = {0};
+	pcl::uint32_t fieldOffsets[10] = {0};
 #endif
 	for(unsigned int i=0; i<cloud.fields.size(); ++i)
 	{
@@ -102,6 +102,42 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 			fieldStates[7] = 1;
 			fieldOffsets[7] = cloud.fields[i].offset;
 		}
+		else if(cloud.fields[i].name.compare("time") == 0)
+		{
+			if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					UWARN("The input scan cloud has an \"time\" field "
+							"but the datatype (%d) is not supported. Time will be ignored. "
+							"This message is only shown once.", cloud.fields[i].datatype);
+					warningShown = true;
+				}
+				continue;
+			}
+
+			fieldStates[8] = 1;
+			fieldOffsets[8] = cloud.fields[i].offset;
+		}
+		else if(cloud.fields[i].name.compare("ring") == 0)
+		{
+			if(cloud.fields[i].datatype != pcl::PCLPointField::UINT16)
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					UWARN("The input scan cloud has an \"ring\" field "
+							"but the datatype (%d) is not supported. Ring will be ignored. "
+							"This message is only shown once.", cloud.fields[i].datatype);
+					warningShown = true;
+				}
+				continue;
+			}
+
+			fieldStates[9] = 1;
+			fieldOffsets[9] = cloud.fields[i].offset;
+		}
 		else
 		{
 			UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
@@ -117,6 +153,8 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 	bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
 	bool hasIntensity = fieldStates[7];
 	bool hasRGB = !hasIntensity&&fieldStates[6];
+	bool hasTime = hasIntensity&&fieldStates[8];
+	bool hasRing = hasIntensity&&fieldStates[9];
 	bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];

 	LaserScan::Format format;
@@ -140,7 +178,18 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 		}
 		else if(!hasNormals && hasIntensity)
 		{
-			format = LaserScan::kXYZI;
+			if(hasTime && hasRing)
+			{
+				format = LaserScan::kXYZIRT;
+			}
+			else if(hasTime)
+			{
+				format = LaserScan::kXYZIT;
+			}
+			else
+			{
+				format = LaserScan::kXYZI;
+			}
 		}
 		else if(!hasNormals && hasRGB)
 		{
@@ -183,6 +232,7 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 		transformRot = transform.rotation();
 	}
 	int oi=0;
+	UASSERT(cloud.height == 1 || cloud.row_step != 0);
 	for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
 	{
 		const uint8_t* row_data = &cloud.data[row * cloud.row_step];
@@ -242,26 +292,45 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
 			{
 				ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
 				ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
-				ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
-				ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
-				ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
+				if(format == LaserScan::kXYZIT)
+				{
+					ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
+					ptr[4] = *(float*)(msg_data + fieldOffsets[8]);
+				}
+				else // kXYNormal
+				{
+					ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
+					ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
+					ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
+				}
 				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
 			}
 			else if(laserScan.channels() == 6)
 			{
 				ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
 				ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
-				if(format == LaserScan::kXYINormal)
-				{
-					ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
-				}
-				else // XYZNormal
+				if(format == LaserScan::kXYZIRT)
 				{
 					ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
+					ptr[4] = float(*(unsigned short*)(msg_data + fieldOffsets[9])); // Convert 16U to float
+					ptr[5] = *(float*)(msg_data + fieldOffsets[8]);
+				}
+				else // with normal
+				{
+					if(format == LaserScan::kXYINormal)
+					{
+						ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
+					}
+					else // XYZNormal
+					{
+						ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					}
+					ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
+					ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
+					ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
 				}
-				ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
-				ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
-				ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
 				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) &&  uIsFinite(ptr[5]);
 			}
 			else if(laserScan.channels() == 7)
@@ -0,0 +1,82 @@
+/*
+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.
+*/
+
+#ifndef ODOMETRYLIOSAM_H_
+#define ODOMETRYLIOSAM_H_
+
+#include <rtabmap/core/Odometry.h>
+
+#ifdef RTABMAP_LIOSAM
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <vector>
+namespace lio_sam { class LioSamCore; }
+#endif
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT OdometryLIOSAM : public Odometry
+{
+public:
+	OdometryLIOSAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryLIOSAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeLIOSAM;}
+	virtual bool canProcessAsyncIMU() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+#ifdef RTABMAP_LIOSAM
+	bool init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform);
+#endif
+
+private:
+#ifdef RTABMAP_LIOSAM
+	lio_sam::LioSamCore * lioSam_;
+	Transform lastPose_;
+	bool lost_;
+	float linVar_;
+	float angVar_;
+	ParametersMap parameters_;
+	Transform imuLocalTransform_; // base_link -> imu_link (cached for deferred init)
+
+	// Buffered IMU samples received before initialization
+	struct ImuSample {
+		double stamp;
+		Eigen::Vector3d acc;
+		Eigen::Vector3d gyro;
+		Eigen::Quaterniond orientation;
+	};
+	std::vector<ImuSample> imuBuffer_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYLIOSAM_H_ */
@@ -39,12 +39,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/ProgressState.h>
+#include <cstdint>
 #include <map>
 #include <list>

 namespace rtabmap
 {

+// Point type carrying xyz + intensity + ring (laser line index) + time
+// (per-point acquisition offset, seconds from the scan start). Matches the
+// layout expected by LIO-SAM's Velodyne feature extractor so it can be fed
+// directly via util3d::laserScanFromPointCloud().
+struct EIGEN_ALIGN16 PointXYZIRT
+{
+	PCL_ADD_POINT4D;
+	float intensity;
+	std::uint16_t ring;
+	float time;
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
 namespace util3d
 {

@@ -297,6 +311,9 @@ LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl:
 // return CV_32FC4 (x,y,z,I)
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
+// return CV_32FC6 (x,y,z,I,ring,time)
+LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
+LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);
 // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
@@ -512,6 +529,15 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
 } // namespace util3d
 } // namespace rtabmap

+POINT_CLOUD_REGISTER_POINT_STRUCT(rtabmap::PointXYZIRT,
+	(float, x, x)
+	(float, y, y)
+	(float, z, z)
+	(float, intensity, intensity)
+	(std::uint16_t, ring, ring)
+	(float, time, time)
+)
+
 #include "rtabmap/core/impl/util3d.hpp"

 #endif /* UTIL3D_H_ */