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]>

CMakeLists.txt

@@ -192,6 +192,7 @@ option(WITH_CCCORELIB     "Include CCCoreLib support"            OFF)
 option(WITH_OPEN3D        "Include Open3D support"               OFF)
 option(WITH_LOAM          "Include LOAM support"                 OFF)
 option(WITH_FLOAM         "Include FLOAM support"                OFF)
+option(WITH_LIOSAM        "Include LIO-SAM support"              OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
@@ -630,6 +631,12 @@ IF(WITH_FLOAM)
        FIND_PACKAGE(Ceres REQUIRED)
     ENDIF(floam_FOUND)
 ENDIF(WITH_FLOAM)
+IF(WITH_LIOSAM)
+    find_package(lio_sam QUIET)
+    IF(lio_sam_FOUND)
+       MESSAGE(STATUS "Found lio_sam: ${lio_sam_INCLUDE_DIRS}")
+    ENDIF(lio_sam_FOUND)
+ENDIF(WITH_LIOSAM)
 
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
@@ -1069,6 +1076,9 @@ ENDIF(NOT loam_velodyne_FOUND)
 IF(NOT floam_FOUND)
    SET(FLOAM "//")
 ENDIF(NOT floam_FOUND)
+IF(NOT lio_sam_FOUND)
+   SET(LIOSAM "//")
+ENDIF(NOT lio_sam_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
 ENDIF()
@@ -1873,6 +1883,13 @@ MESSAGE(STATUS "  With floam                = NO (WITH_FLOAM=OFF)")
 ELSE()
 MESSAGE(STATUS "  With floam                = NO (floam not found)")
 ENDIF()
+IF(lio_sam_FOUND)
+MESSAGE(STATUS "  With lio_sam              = YES (License: BSD)")
+ELSEIF(NOT WITH_LIOSAM)
+MESSAGE(STATUS "  With lio_sam              = NO (WITH_LIOSAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With lio_sam              = NO (lio_sam not found)")
+ENDIF()
 
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")

Version.h.in

@@ -63,6 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CUDASIFT@#define RTABMAP_CUDASIFT
 @LOAM@#define RTABMAP_LOAM
 @FLOAM@#define RTABMAP_FLOAM
+@LIOSAM@#define RTABMAP_LIOSAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED

corelib/include/rtabmap/core/LaserScan.h

@@ -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);
 

corelib/include/rtabmap/core/Odometry.h

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

corelib/include/rtabmap/core/Parameters.h

@@ -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");

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -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]);
+				if(format == LaserScan::kXYZIT)
-				ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
+				{
-				ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
+					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)
+				if(format == LaserScan::kXYZIRT)
-				{
-					ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
-				}
-				else // XYZNormal
 				{
 					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)

corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h

@@ -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_ */

corelib/include/rtabmap/core/util3d.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_ */

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}

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());
 		}
 	}

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;

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

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/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());
 }
 
 

guilib/include/rtabmap/gui/PreferencesDialog.h

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

guilib/src/PreferencesDialog.cpp

@@ -243,6 +243,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #ifndef RTABMAP_CUVSLAM
 	_ui->odom_strategy->setItemData(13, 0, Qt::UserRole - 1);
 #endif
+#ifndef RTABMAP_LIOSAM
+	_ui->odom_strategy->setItemData(14, 0, Qt::UserRole - 1);
+#endif
 
 #if CV_MAJOR_VERSION < 3
 	_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
@@ -1698,6 +1701,22 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Odometry CuVSLAM
 	_ui->odom_cuvslam_multicam_mode->setObjectName(Parameters::kOdomCuVSLAMMulticamMode().c_str());
 
+	// Odometry LIO-SAM
+	_ui->lineEdit_OdomLIOSAMPath->setObjectName(Parameters::kOdomLIOSAMConfigPath().c_str());
+	connect(_ui->toolButton_OdomLIOSAMPath, SIGNAL(clicked()), this, SLOT(changeOdometryLIOSAMConfigPath()));
+	_ui->odom_liosam_sensor->setObjectName(Parameters::kOdomLIOSAMSensor().c_str());
+	_ui->odom_liosam_nscan->setObjectName(Parameters::kOdomLIOSAMNScan().c_str());
+	_ui->odom_liosam_horizon_scan->setObjectName(Parameters::kOdomLIOSAMHorizonScan().c_str());
+	_ui->odom_liosam_imu_acc_noise->setObjectName(Parameters::kOdomLIOSAMImuAccNoise().c_str());
+	_ui->odom_liosam_imu_gyr_noise->setObjectName(Parameters::kOdomLIOSAMImuGyrNoise().c_str());
+	_ui->odom_liosam_imu_acc_bias_n->setObjectName(Parameters::kOdomLIOSAMImuAccBiasN().c_str());
+	_ui->odom_liosam_imu_gyr_bias_n->setObjectName(Parameters::kOdomLIOSAMImuGyrBiasN().c_str());
+	_ui->odom_liosam_imu_gravity->setObjectName(Parameters::kOdomLIOSAMImuGravity().c_str());
+	_ui->odom_liosam_edge_threshold->setObjectName(Parameters::kOdomLIOSAMEdgeThreshold().c_str());
+	_ui->odom_liosam_surf_threshold->setObjectName(Parameters::kOdomLIOSAMSurfThreshold().c_str());
+	_ui->odom_liosam_linvar->setObjectName(Parameters::kOdomLIOSAMLinVar().c_str());
+	_ui->odom_liosam_angvar->setObjectName(Parameters::kOdomLIOSAMAngVar().c_str());
+
 	//StereoDense
 	_ui->comboBox_stereoDense_strategy->setObjectName(Parameters::kStereoDenseStrategy().c_str());
 	connect(_ui->comboBox_stereoDense_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereoDense, SLOT(setCurrentIndex(int)));
@@ -5572,6 +5591,7 @@ void PreferencesDialog::updateOdometryStackedIndex(int index)
 	_ui->groupBox_odomOpenVINS->setVisible(index==10);
 	_ui->groupBox_odomOpen3D->setVisible(index==12);
 	_ui->groupBox_odomCuvslam->setVisible(index==13);
+	_ui->groupBox_odomLIOSAM->setVisible(index==14);
 }
 
 void PreferencesDialog::useOdomFeatures()
@@ -5677,6 +5697,23 @@ void PreferencesDialog::changeOdometryVINSFusionConfigPath()
 	}
 }
 
+void PreferencesDialog::changeOdometryLIOSAMConfigPath()
+{
+	QString path;
+	if(_ui->lineEdit_OdomLIOSAMPath->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), this->getWorkingDirectory(), tr("LIO-SAM config (*.yaml)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("LIO-SAM Config"), _ui->lineEdit_OdomLIOSAMPath->text(), tr("LIO-SAM config (*.yaml)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->lineEdit_OdomLIOSAMPath->setText(path);
+	}
+}
+
 void PreferencesDialog::changeOdometryOpenVINSLeftMask()
 {
 	QString path;

guilib/src/ui/preferencesDialog.ui

@@ -16137,6 +16137,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           <string>CuVSLAM</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>LIO-SAM</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
                       <item row="3" column="1">
@@ -21847,6 +21852,458 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </layout>
                       </widget>
+                      <widget class="QWidget" name="page_103">
+                       <layout class="QVBoxLayout" name="verticalLayout_187">
+                        <item>
+                         <widget class="QGroupBox" name="groupBox_odomLIOSAM">
+                          <property name="title">
+                           <string>LIO-SAM</string>
+                          </property>
+                          <layout class="QVBoxLayout" name="verticalLayout_188" stretch="0,0">
+                           <item>
+                            <widget class="QLabel" name="label_odom_liosam_info">
+                             <property name="text">
+                              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;LIO-SAM: &lt;a href=&quot;https://github.com/TixiaoShan/LIO-SAM&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/TixiaoShan/LIO-SAM&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Tightly-coupled lidar-inertial odometry via smoothing and mapping. Requires 3D lidar with per-point ring and time fields (kXYZIRT scan format) and an IMU.&lt;/p&gt;&lt;p&gt;If a config file path is provided, the parameters below are ignored and read from the file instead.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="openExternalLinks">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item>
+                            <layout class="QGridLayout" name="gridLayout_liosam" columnstretch="0,1">
+                             <item row="0" column="0">
+                              <layout class="QHBoxLayout" name="horizontalLayout_liosam_config">
+                               <item>
+                                <widget class="QLineEdit" name="lineEdit_OdomLIOSAMPath">
+                                 <property name="placeholderText">
+                                  <string>Optional: path to LIO-SAM config YAML</string>
+                                 </property>
+                                </widget>
+                               </item>
+                               <item>
+                                <widget class="QToolButton" name="toolButton_OdomLIOSAMPath">
+                                 <property name="text">
+                                  <string>...</string>
+                                 </property>
+                                </widget>
+                               </item>
+                              </layout>
+                             </item>
+                             <item row="0" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_config">
+                               <property name="text">
+                                <string>Config file path (overrides parameters below).</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="1" column="0">
+                              <widget class="QComboBox" name="odom_liosam_sensor">
+                               <property name="sizeAdjustPolicy">
+                                <enum>QComboBox::AdjustToContents</enum>
+                               </property>
+                               <item>
+                                <property name="text">
+                                 <string>Velodyne</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>Ouster</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>Livox</string>
+                                </property>
+                               </item>
+                              </widget>
+                             </item>
+                             <item row="1" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_sensor">
+                               <property name="text">
+                                <string>LiDAR sensor type.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="2" column="0">
+                              <widget class="QSpinBox" name="odom_liosam_nscan">
+                               <property name="minimum">
+                                <number>1</number>
+                               </property>
+                               <property name="maximum">
+                                <number>256</number>
+                               </property>
+                               <property name="value">
+                                <number>16</number>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="2" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_nscan">
+                               <property name="text">
+                                <string>Number of LiDAR channels (16, 32, 64, 128).</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="0">
+                              <widget class="QSpinBox" name="odom_liosam_horizon_scan">
+                               <property name="minimum">
+                                <number>1</number>
+                               </property>
+                               <property name="maximum">
+                                <number>10000</number>
+                               </property>
+                               <property name="value">
+                                <number>1800</number>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_horizon_scan">
+                               <property name="text">
+                                <string>Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048).</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_imu_acc_noise">
+                               <property name="decimals">
+                                <number>6</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000001000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.001000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.010000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_imu_acc_noise">
+                               <property name="text">
+                                <string>IMU accelerometer white noise.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="5" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_imu_gyr_noise">
+                               <property name="decimals">
+                                <number>6</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000001000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.001000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="5" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_imu_gyr_noise">
+                               <property name="text">
+                                <string>IMU gyroscope white noise.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="6" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_imu_acc_bias_n">
+                               <property name="decimals">
+                                <number>6</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000001000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.000200000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="6" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_imu_acc_bias_n">
+                               <property name="text">
+                                <string>IMU accelerometer bias noise.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="7" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_imu_gyr_bias_n">
+                               <property name="decimals">
+                                <number>6</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000001000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.000010000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.000030000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="7" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_imu_gyr_bias_n">
+                               <property name="text">
+                                <string>IMU gyroscope bias noise.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="8" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_imu_gravity">
+                               <property name="decimals">
+                                <number>5</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000000000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>20.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.010000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>9.805110000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="8" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_imu_gravity">
+                               <property name="text">
+                                <string>Gravity magnitude.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="9" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_edge_threshold">
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>100.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.100000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>1.000000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="9" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_edge_threshold">
+                               <property name="text">
+                                <string>Edge feature curvature threshold.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="10" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_surf_threshold">
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>100.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.010000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.100000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="10" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_surf_threshold">
+                               <property name="text">
+                                <string>Surface feature curvature threshold.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="11" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_linvar">
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.001000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.010000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="11" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_linvar">
+                               <property name="text">
+                                <string>Linear output variance.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="12" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_liosam_angvar">
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000100000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.001000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.010000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="12" column="1">
+                              <widget class="QLabel" name="label_odom_liosam_angvar">
+                               <property name="text">
+                                <string>Angular output variance.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                            </layout>
+                           </item>
+                          </layout>
+                         </widget>
+                        </item>
+                        <item>
+                         <spacer name="verticalSpacer_93">
+                          <property name="orientation">
+                           <enum>Qt::Vertical</enum>
+                          </property>
+                          <property name="sizeHint" stdset="0">
+                           <size>
+                            <width>20</width>
+                            <height>0</height>
+                           </size>
+                          </property>
+                         </spacer>
+                        </item>
+                       </layout>
+                      </widget>
                       <widget class="QWidget" name="page_26">
                        <layout class="QVBoxLayout" name="verticalLayout_88">
                         <item>