Skip to content

Commit a9f63bd

Browse files
abhi2198matlabbe
andauthored
Add LIO-SAM as an odometry strategy (#1684)
* Added liosam odometry integration * Add kXYZIRT scan format with per-point ring channel for LIO-SAM integration Introduce PointXYZIRT point type and kXYZIRT LaserScan format (x,y,z, intensity,ring,time) so that ring indices survive the scan pipeline. Update OdometryLIOSAM to require kXYZIRT and properly split ring/time into the parallel buffers LIO-SAM expects. Extend deskewing to preserve ring data and disable base-class deskew in OdometryLIOSAM since LIO-SAM handles it internally. * Address PR review: config file, deferred init, and GUI panel for LIO-SAM - Add OdomLIOSAM/ConfigPath parameter to load LIO-SAM settings from a YAML file. When set, individual params are ignored. Extrinsics from sensor local transforms always override config file values. - Defer LioSamCore initialization until both IMU and lidar local transforms are available, computing T_lidar_imu from sensor data. IMU samples are buffered and replayed after init. - Fix deferred init for scan-only messages that arrive after IMU local transform is already cached. - Add LIO-SAM entry to odometry strategy combo box (index 14) with full PreferencesDialog panel including config path browse button and all parameter widgets. * OdometryLIOSAM: propagate deskewed scan to SensorData Capture the deskewed cloud produced by LIO-SAM's image projection stage and replace the raw scan on SensorData with it, so loop closure registration and other downstream stages operate on the motion- compensated points instead of the raw pre-deskew input. * Minor updates for rtabmap_ros --------- Co-authored-by: matlabbe <matlabbe@gmail.com>
1 parent 8fd701a commit a9f63bd

17 files changed

Lines changed: 1315 additions & 28 deletions

File tree

CMakeLists.txt

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -192,6 +192,7 @@ option(WITH_CCCORELIB "Include CCCoreLib support" OFF)
192192
option(WITH_OPEN3D "Include Open3D support" OFF)
193193
option(WITH_LOAM "Include LOAM support" OFF)
194194
option(WITH_FLOAM "Include FLOAM support" OFF)
195+
option(WITH_LIOSAM "Include LIO-SAM support" OFF)
195196
option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON)
196197
option(WITH_ZED "Include ZED sdk support" ON)
197198
option(WITH_ZEDOC "Include ZED Open Capture support" ON)
@@ -630,6 +631,12 @@ IF(WITH_FLOAM)
630631
FIND_PACKAGE(Ceres REQUIRED)
631632
ENDIF(floam_FOUND)
632633
ENDIF(WITH_FLOAM)
634+
IF(WITH_LIOSAM)
635+
find_package(lio_sam QUIET)
636+
IF(lio_sam_FOUND)
637+
MESSAGE(STATUS "Found lio_sam: ${lio_sam_INCLUDE_DIRS}")
638+
ENDIF(lio_sam_FOUND)
639+
ENDIF(WITH_LIOSAM)
633640

634641
SET(ZED_FOUND FALSE)
635642
IF(WITH_ZED)
@@ -1069,6 +1076,9 @@ ENDIF(NOT loam_velodyne_FOUND)
10691076
IF(NOT floam_FOUND)
10701077
SET(FLOAM "//")
10711078
ENDIF(NOT floam_FOUND)
1079+
IF(NOT lio_sam_FOUND)
1080+
SET(LIOSAM "//")
1081+
ENDIF(NOT lio_sam_FOUND)
10721082
IF(NOT Freenect_FOUND)
10731083
SET(FREENECT "//")
10741084
ENDIF()
@@ -1873,6 +1883,13 @@ MESSAGE(STATUS " With floam = NO (WITH_FLOAM=OFF)")
18731883
ELSE()
18741884
MESSAGE(STATUS " With floam = NO (floam not found)")
18751885
ENDIF()
1886+
IF(lio_sam_FOUND)
1887+
MESSAGE(STATUS " With lio_sam = YES (License: BSD)")
1888+
ELSEIF(NOT WITH_LIOSAM)
1889+
MESSAGE(STATUS " With lio_sam = NO (WITH_LIOSAM=OFF)")
1890+
ELSE()
1891+
MESSAGE(STATUS " With lio_sam = NO (lio_sam not found)")
1892+
ENDIF()
18761893

18771894
IF(libfovis_FOUND)
18781895
MESSAGE(STATUS " With libfovis = YES (License: GPLv2)")

Version.h.in

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
6363
@CUDASIFT@#define RTABMAP_CUDASIFT
6464
@LOAM@#define RTABMAP_LOAM
6565
@FLOAM@#define RTABMAP_FLOAM
66+
@LIOSAM@#define RTABMAP_LIOSAM
6667
@DC1394@#define RTABMAP_DC1394
6768
@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
6869
@ZED@#define RTABMAP_ZED

corelib/include/rtabmap/core/LaserScan.h

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,8 @@ class RTABMAP_CORE_EXPORT LaserScan
4848
kXYZNormal=8,
4949
kXYZINormal=9,
5050
kXYZRGBNormal=10,
51-
kXYZIT=11};
51+
kXYZIT=11,
52+
kXYZIRT=12};
5253

5354
static std::string formatName(const Format & format);
5455
static int channels(const Format & format);
@@ -57,6 +58,7 @@ class RTABMAP_CORE_EXPORT LaserScan
5758
static bool isScanHasRGB(const Format & format);
5859
static bool isScanHasIntensity(const Format & format);
5960
static bool isScanHasTime(const Format & format);
61+
static bool isScanHasRing(const Format & format);
6062
static LaserScan backwardCompatibility(
6163
const cv::Mat & oldScanFormat,
6264
int maxPoints = 0,
@@ -135,6 +137,7 @@ class RTABMAP_CORE_EXPORT LaserScan
135137
bool hasRGB() const {return isScanHasRGB(format_);}
136138
bool hasIntensity() const {return isScanHasIntensity(format_);}
137139
bool hasTime() const {return isScanHasTime(format_);}
140+
bool hasRing() const {return isScanHasRing(format_);}
138141
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
139142
bool isOrganized() const {return data_.rows > 1;}
140143
LaserScan clone() const;
@@ -143,7 +146,8 @@ class RTABMAP_CORE_EXPORT LaserScan
143146
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
144147
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
145148
int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
146-
int getTimeOffset() const {return hasTime()?4:-1;}
149+
int getRingOffset() const {return format_==kXYZIRT?4:-1;}
150+
int getTimeOffset() const {return format_==kXYZIT?4:(format_==kXYZIRT?5:-1);}
147151

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

corelib/include/rtabmap/core/Odometry.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,8 @@ class RTABMAP_CORE_EXPORT Odometry
5757
kTypeOpenVINS = 10,
5858
kTypeFLOAM = 11,
5959
kTypeOpen3D = 12,
60-
kTypeCuVSLAM = 13
60+
kTypeCuVSLAM = 13,
61+
kTypeLIOSAM = 14
6162
};
6263

6364
public:

corelib/include/rtabmap/core/Parameters.h

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -465,7 +465,7 @@ class RTABMAP_CORE_EXPORT Parameters
465465
RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
466466

467467
// Odometry
468-
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");
468+
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");
469469
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.");
470470
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)).");
471471
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
@@ -687,6 +687,21 @@ class RTABMAP_CORE_EXPORT Parameters
687687
// Odometry cuVSLAM
688688
RTABMAP_PARAM(OdomCuVSLAM, MulticamMode, int, 0, "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision.");
689689

690+
// Odometry LIO-SAM
691+
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.");
692+
RTABMAP_PARAM(OdomLIOSAM, Sensor, int, 0, "LiDAR sensor: 0=Velodyne, 1=Ouster, 2=Livox");
693+
RTABMAP_PARAM(OdomLIOSAM, NScan, int, 16, "Number of LiDAR channels (16, 32, 64, 128).");
694+
RTABMAP_PARAM(OdomLIOSAM, HorizonScan, int, 1800, "Horizontal resolution (Velodyne:1800, Ouster:512/1024/2048).");
695+
RTABMAP_PARAM(OdomLIOSAM, ImuAccNoise, float, 0.01, "IMU accelerometer white noise.");
696+
RTABMAP_PARAM(OdomLIOSAM, ImuGyrNoise, float, 0.001, "IMU gyroscope white noise.");
697+
RTABMAP_PARAM(OdomLIOSAM, ImuAccBiasN, float, 0.0002,"IMU accelerometer bias noise.");
698+
RTABMAP_PARAM(OdomLIOSAM, ImuGyrBiasN, float, 0.00003,"IMU gyroscope bias noise.");
699+
RTABMAP_PARAM(OdomLIOSAM, ImuGravity, float, 9.80511,"Gravity magnitude.");
700+
RTABMAP_PARAM(OdomLIOSAM, EdgeThreshold,float, 1.0, "Edge feature curvature threshold.");
701+
RTABMAP_PARAM(OdomLIOSAM, SurfThreshold,float, 0.1, "Surface feature curvature threshold.");
702+
RTABMAP_PARAM(OdomLIOSAM, LinVar, float, 0.01, "Linear output variance.");
703+
RTABMAP_PARAM(OdomLIOSAM, AngVar, float, 0.01, "Angular output variance.");
704+
690705
// Common registration parameters
691706
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.");
692707
RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");

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

Lines changed: 83 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -41,11 +41,11 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
4141
return LaserScan();
4242
}
4343
//determine the output type
44-
int fieldStates[8] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity
44+
int fieldStates[10] = {0}; // x,y,z,normal_x,normal_y,normal_z,rgb,intensity,time,ring
4545
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
46-
std::uint32_t fieldOffsets[8] = {0};
46+
std::uint32_t fieldOffsets[10] = {0};
4747
#else
48-
pcl::uint32_t fieldOffsets[8] = {0};
48+
pcl::uint32_t fieldOffsets[10] = {0};
4949
#endif
5050
for(unsigned int i=0; i<cloud.fields.size(); ++i)
5151
{
@@ -102,6 +102,42 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
102102
fieldStates[7] = 1;
103103
fieldOffsets[7] = cloud.fields[i].offset;
104104
}
105+
else if(cloud.fields[i].name.compare("time") == 0)
106+
{
107+
if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
108+
{
109+
static bool warningShown = false;
110+
if(!warningShown)
111+
{
112+
UWARN("The input scan cloud has an \"time\" field "
113+
"but the datatype (%d) is not supported. Time will be ignored. "
114+
"This message is only shown once.", cloud.fields[i].datatype);
115+
warningShown = true;
116+
}
117+
continue;
118+
}
119+
120+
fieldStates[8] = 1;
121+
fieldOffsets[8] = cloud.fields[i].offset;
122+
}
123+
else if(cloud.fields[i].name.compare("ring") == 0)
124+
{
125+
if(cloud.fields[i].datatype != pcl::PCLPointField::UINT16)
126+
{
127+
static bool warningShown = false;
128+
if(!warningShown)
129+
{
130+
UWARN("The input scan cloud has an \"ring\" field "
131+
"but the datatype (%d) is not supported. Ring will be ignored. "
132+
"This message is only shown once.", cloud.fields[i].datatype);
133+
warningShown = true;
134+
}
135+
continue;
136+
}
137+
138+
fieldStates[9] = 1;
139+
fieldOffsets[9] = cloud.fields[i].offset;
140+
}
105141
else
106142
{
107143
UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str());
@@ -117,6 +153,8 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
117153
bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
118154
bool hasIntensity = fieldStates[7];
119155
bool hasRGB = !hasIntensity&&fieldStates[6];
156+
bool hasTime = hasIntensity&&fieldStates[8];
157+
bool hasRing = hasIntensity&&fieldStates[9];
120158
bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
121159

122160
LaserScan::Format format;
@@ -140,7 +178,18 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
140178
}
141179
else if(!hasNormals && hasIntensity)
142180
{
143-
format = LaserScan::kXYZI;
181+
if(hasTime && hasRing)
182+
{
183+
format = LaserScan::kXYZIRT;
184+
}
185+
else if(hasTime)
186+
{
187+
format = LaserScan::kXYZIT;
188+
}
189+
else
190+
{
191+
format = LaserScan::kXYZI;
192+
}
144193
}
145194
else if(!hasNormals && hasRGB)
146195
{
@@ -183,6 +232,7 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
183232
transformRot = transform.rotation();
184233
}
185234
int oi=0;
235+
UASSERT(cloud.height == 1 || cloud.row_step != 0);
186236
for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
187237
{
188238
const uint8_t* row_data = &cloud.data[row * cloud.row_step];
@@ -242,26 +292,45 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
242292
{
243293
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
244294
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
245-
ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
246-
ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
247-
ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
295+
if(format == LaserScan::kXYZIT)
296+
{
297+
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
298+
ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
299+
ptr[4] = *(float*)(msg_data + fieldOffsets[8]);
300+
}
301+
else // kXYNormal
302+
{
303+
ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
304+
ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
305+
ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
306+
}
248307
valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
249308
}
250309
else if(laserScan.channels() == 6)
251310
{
252311
ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
253312
ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
254-
if(format == LaserScan::kXYINormal)
313+
if(format == LaserScan::kXYZIRT)
255314
{
256-
ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
315+
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
316+
ptr[3] = *(float*)(msg_data + fieldOffsets[7]);
317+
ptr[4] = float(*(unsigned short*)(msg_data + fieldOffsets[9])); // Convert 16U to float
318+
ptr[5] = *(float*)(msg_data + fieldOffsets[8]);
257319
}
258-
else // XYZNormal
320+
else // with normal
259321
{
260-
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
322+
if(format == LaserScan::kXYINormal)
323+
{
324+
ptr[2] = *(float*)(msg_data + fieldOffsets[7]);
325+
}
326+
else // XYZNormal
327+
{
328+
ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
329+
}
330+
ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
331+
ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
332+
ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
261333
}
262-
ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
263-
ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
264-
ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
265334
valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
266335
}
267336
else if(laserScan.channels() == 7)
Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
/*
2+
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3+
All rights reserved.
4+
5+
Redistribution and use in source and binary forms, with or without
6+
modification, are permitted provided that the following conditions are met:
7+
* Redistributions of source code must retain the above copyright
8+
notice, this list of conditions and the following disclaimer.
9+
* Redistributions in binary form must reproduce the above copyright
10+
notice, this list of conditions and the following disclaimer in the
11+
documentation and/or other materials provided with the distribution.
12+
* Neither the name of the Universite de Sherbrooke nor the
13+
names of its contributors may be used to endorse or promote products
14+
derived from this software without specific prior written permission.
15+
16+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17+
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18+
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20+
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21+
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22+
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24+
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25+
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26+
*/
27+
28+
#ifndef ODOMETRYLIOSAM_H_
29+
#define ODOMETRYLIOSAM_H_
30+
31+
#include <rtabmap/core/Odometry.h>
32+
33+
#ifdef RTABMAP_LIOSAM
34+
#include <Eigen/Core>
35+
#include <Eigen/Geometry>
36+
#include <vector>
37+
namespace lio_sam { class LioSamCore; }
38+
#endif
39+
40+
namespace rtabmap {
41+
42+
class RTABMAP_CORE_EXPORT OdometryLIOSAM : public Odometry
43+
{
44+
public:
45+
OdometryLIOSAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
46+
virtual ~OdometryLIOSAM();
47+
48+
virtual void reset(const Transform & initialPose = Transform::getIdentity());
49+
virtual Odometry::Type getType() {return Odometry::kTypeLIOSAM;}
50+
virtual bool canProcessAsyncIMU() const {return true;}
51+
52+
private:
53+
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
54+
55+
#ifdef RTABMAP_LIOSAM
56+
bool init(const Transform & imuLocalTransform, const Transform & lidarLocalTransform);
57+
#endif
58+
59+
private:
60+
#ifdef RTABMAP_LIOSAM
61+
lio_sam::LioSamCore * lioSam_;
62+
Transform lastPose_;
63+
bool lost_;
64+
float linVar_;
65+
float angVar_;
66+
ParametersMap parameters_;
67+
Transform imuLocalTransform_; // base_link -> imu_link (cached for deferred init)
68+
69+
// Buffered IMU samples received before initialization
70+
struct ImuSample {
71+
double stamp;
72+
Eigen::Vector3d acc;
73+
Eigen::Vector3d gyro;
74+
Eigen::Quaterniond orientation;
75+
};
76+
std::vector<ImuSample> imuBuffer_;
77+
#endif
78+
};
79+
80+
}
81+
82+
#endif /* ODOMETRYLIOSAM_H_ */

0 commit comments

Comments
 (0)