i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 04:19:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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]>
This commit is contained in:
Abhijith
2026-04-12 18:06:44 -07:00
committed by GitHub
co-authored by matlabbe
parent 8fd701aabe
commit a9f63bd5fd
17 changed files with 1316 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+17
View File
@@ -192,6 +192,7 @@ option(WITH_CCCORELIB "Include CCCoreLib support" OFF)
option(WITH_OPEN3D "Include Open3D support" OFF) option(WITH_OPEN3D "Include Open3D support" OFF)
option(WITH_LOAM "Include LOAM support" OFF) option(WITH_LOAM "Include LOAM support" OFF)
option(WITH_FLOAM "Include FLOAM 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_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON)
option(WITH_ZED "Include ZED sdk support" ON) option(WITH_ZED "Include ZED sdk support" ON)
option(WITH_ZEDOC "Include ZED Open Capture support" ON) option(WITH_ZEDOC "Include ZED Open Capture support" ON)
@@ -630,6 +631,12 @@ IF(WITH_FLOAM)
FIND_PACKAGE(Ceres REQUIRED) FIND_PACKAGE(Ceres REQUIRED)
ENDIF(floam_FOUND) ENDIF(floam_FOUND)
ENDIF(WITH_FLOAM) 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) SET(ZED_FOUND FALSE)
IF(WITH_ZED) IF(WITH_ZED)
@@ -1069,6 +1076,9 @@ ENDIF(NOT loam_velodyne_FOUND)
IF(NOT floam_FOUND) IF(NOT floam_FOUND)
SET(FLOAM "//") SET(FLOAM "//")
ENDIF(NOT floam_FOUND) ENDIF(NOT floam_FOUND)
IF(NOT lio_sam_FOUND)
SET(LIOSAM "//")
ENDIF(NOT lio_sam_FOUND)
IF(NOT Freenect_FOUND) IF(NOT Freenect_FOUND)
SET(FREENECT "//") SET(FREENECT "//")
ENDIF() ENDIF()
@@ -1873,6 +1883,13 @@ MESSAGE(STATUS " With floam = NO (WITH_FLOAM=OFF)")
ELSE() ELSE()
MESSAGE(STATUS " With floam = NO (floam not found)") MESSAGE(STATUS " With floam = NO (floam not found)")
ENDIF() 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) IF(libfovis_FOUND)
MESSAGE(STATUS " With libfovis = YES (License: GPLv2)") MESSAGE(STATUS " With libfovis = YES (License: GPLv2)")
Version.h.in
+1
View File
@@ -63,6 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@CUDASIFT@#define RTABMAP_CUDASIFT @CUDASIFT@#define RTABMAP_CUDASIFT
@LOAM@#define RTABMAP_LOAM @LOAM@#define RTABMAP_LOAM
@FLOAM@#define RTABMAP_FLOAM @FLOAM@#define RTABMAP_FLOAM
@LIOSAM@#define RTABMAP_LIOSAM
@DC1394@#define RTABMAP_DC1394 @DC1394@#define RTABMAP_DC1394
@FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
@ZED@#define RTABMAP_ZED @ZED@#define RTABMAP_ZED
corelib/include/rtabmap/core/LaserScan.h
+6 -2
View File
@@ -48,7 +48,8 @@ public:
kXYZNormal=8, kXYZNormal=8,
kXYZINormal=9, kXYZINormal=9,
kXYZRGBNormal=10, kXYZRGBNormal=10,
kXYZIT=11}; kXYZIT=11,
kXYZIRT=12};
static std::string formatName(const Format & format); static std::string formatName(const Format & format);
static int channels(const Format & format); static int channels(const Format & format);
@@ -57,6 +58,7 @@ public:
static bool isScanHasRGB(const Format & format); static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format); static bool isScanHasIntensity(const Format & format);
static bool isScanHasTime(const Format & format); static bool isScanHasTime(const Format & format);
static bool isScanHasRing(const Format & format);
static LaserScan backwardCompatibility( static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat, const cv::Mat & oldScanFormat,
int maxPoints = 0, int maxPoints = 0,
@@ -135,6 +137,7 @@ public:
bool hasRGB() const {return isScanHasRGB(format_);} bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);} bool hasIntensity() const {return isScanHasIntensity(format_);}
bool hasTime() const {return isScanHasTime(format_);} bool hasTime() const {return isScanHasTime(format_);}
bool hasRing() const {return isScanHasRing(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;} bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
bool isOrganized() const {return data_.rows > 1;} bool isOrganized() const {return data_.rows > 1;}
LaserScan clone() const; LaserScan clone() const;
@@ -143,7 +146,8 @@ public:
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;} int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
int getRGBOffset() const {return hasRGB()?(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 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); float & field(unsigned int pointIndex, unsigned int channelOffset);
corelib/include/rtabmap/core/Odometry.h
+2 -1
View File
@@ -57,7 +57,8 @@ public:
kTypeOpenVINS = 10, kTypeOpenVINS = 10,
kTypeFLOAM = 11, kTypeFLOAM = 11,
kTypeOpen3D = 12, kTypeOpen3D = 12,
kTypeCuVSLAM = 13 kTypeCuVSLAM = 13,
kTypeLIOSAM = 14
}; };
public: public:
corelib/include/rtabmap/core/Parameters.h
+16 -1
View File
@@ -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."); RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
// Odometry // 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, 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, 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)."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
@@ -687,6 +687,21 @@ class RTABMAP_CORE_EXPORT Parameters
// Odometry cuVSLAM // Odometry cuVSLAM
RTABMAP_PARAM(OdomCuVSLAM, MulticamMode, int, 0, "cuVSLAM multicam_mode setting: 0=moderate, 1=performance, 2=precision."); 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 // 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, 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"); RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");
corelib/include/rtabmap/core/impl/util3d.hpp
+84 -15
View File
@@ -41,11 +41,11 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
return LaserScan(); return LaserScan();
} }
//determine the output type //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) #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
std::uint32_t fieldOffsets[8] = {0}; std::uint32_t fieldOffsets[10] = {0};
#else #else
pcl::uint32_t fieldOffsets[8] = {0}; pcl::uint32_t fieldOffsets[10] = {0};
#endif #endif
for(unsigned int i=0; i<cloud.fields.size(); ++i) 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; fieldStates[7] = 1;
fieldOffsets[7] = cloud.fields[i].offset; 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 else
{ {
UDEBUG("Ignoring \"%s\" field", cloud.fields[i].name.c_str()); 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 hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
bool hasIntensity = fieldStates[7]; bool hasIntensity = fieldStates[7];
bool hasRGB = !hasIntensity&&fieldStates[6]; bool hasRGB = !hasIntensity&&fieldStates[6];
bool hasTime = hasIntensity&&fieldStates[8];
bool hasRing = hasIntensity&&fieldStates[9];
bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2]; bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
LaserScan::Format format; LaserScan::Format format;
@@ -140,7 +178,18 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
} }
else if(!hasNormals && hasIntensity) 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) else if(!hasNormals && hasRGB)
{ {
@@ -183,6 +232,7 @@ LaserScan laserScanFromPointCloud(const PointCloud2T & cloud, bool filterNaNs, b
transformRot = transform.rotation(); transformRot = transform.rotation();
} }
int oi=0; int oi=0;
UASSERT(cloud.height == 1 || cloud.row_step != 0);
for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row) for (uint32_t row = 0; row < (uint32_t)cloud.height; ++row)
{ {
const uint8_t* row_data = &cloud.data[row * cloud.row_step]; 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[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 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]); valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
} }
else if(laserScan.channels() == 6) else if(laserScan.channels() == 6)
{ {
ptr[0] = *(float*)(msg_data + fieldOffsets[0]); ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
ptr[1] = *(float*)(msg_data + fieldOffsets[1]); 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[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]); valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]);
} }
else if(laserScan.channels() == 7) else if(laserScan.channels() == 7)
corelib/include/rtabmap/core/odometry/OdometryLIOSAM.h
+82
View File
@@ -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
+26
View File
@@ -39,12 +39,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
#include <rtabmap/core/ProgressState.h> #include <rtabmap/core/ProgressState.h>
#include <cstdint>
#include <map> #include <map>
#include <list> #include <list>
namespace rtabmap 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 namespace util3d
{ {
@@ -297,6 +311,9 @@ LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud<pcl:
// return CV_32FC4 (x,y,z,I) // 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 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); 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) // 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::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); 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 util3d
} // namespace rtabmap } // 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" #include "rtabmap/core/impl/util3d.hpp"
#endif /* UTIL3D_H_ */ #endif /* UTIL3D_H_ */
corelib/src/CMakeLists.txt
+13
View File
@@ -98,6 +98,7 @@ SET(SRC_FILES
odometry/OdometryORBSLAM3.cpp odometry/OdometryORBSLAM3.cpp
odometry/OdometryLOAM.cpp odometry/OdometryLOAM.cpp
odometry/OdometryFLOAM.cpp odometry/OdometryFLOAM.cpp
odometry/OdometryLIOSAM.cpp
odometry/OdometryMSCKF.cpp odometry/OdometryMSCKF.cpp
odometry/OdometryVINSFusion.cpp odometry/OdometryVINSFusion.cpp
odometry/OdometryOpenVINS.cpp odometry/OdometryOpenVINS.cpp
@@ -604,6 +605,18 @@ IF(floam_FOUND)
) )
ENDIF(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) IF(ZED_FOUND)
SET(INCLUDE_DIRS SET(INCLUDE_DIRS
${INCLUDE_DIRS} ${INCLUDE_DIRS}
corelib/src/LaserScan.cpp
+11 -3
View File
@@ -68,6 +68,9 @@ std::string LaserScan::formatName(const Format & format)
case kXYZIT: case kXYZIT:
name = "XYZIT"; name = "XYZIT";
break; break;
case kXYZIRT:
name = "XYZIRT";
break;
default: default:
name = "Unknown"; name = "Unknown";
break; break;
@@ -96,6 +99,7 @@ int LaserScan::channels(const Format & format)
break; break;
case kXYZNormal: case kXYZNormal:
case kXYINormal: case kXYINormal:
case kXYZIRT:
channels = 6; channels = 6;
break; break;
case kXYZINormal: case kXYZINormal:
@@ -123,11 +127,15 @@ bool LaserScan::isScanHasRGB(const Format & format)
} }
bool LaserScan::isScanHasIntensity(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) 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( 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() != 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() != 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() != 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()); 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
+4
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/odometry/OdometryORBSLAM3.h" #include "rtabmap/core/odometry/OdometryORBSLAM3.h"
#include "rtabmap/core/odometry/OdometryLOAM.h" #include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryFLOAM.h" #include "rtabmap/core/odometry/OdometryFLOAM.h"
#include "rtabmap/core/odometry/OdometryLIOSAM.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h" #include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/odometry/OdometryVINSFusion.h" #include "rtabmap/core/odometry/OdometryVINSFusion.h"
#include "rtabmap/core/odometry/OdometryOpenVINS.h" #include "rtabmap/core/odometry/OdometryOpenVINS.h"
@@ -101,6 +102,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
case Odometry::kTypeFLOAM: case Odometry::kTypeFLOAM:
odometry = new OdometryFLOAM(parameters); odometry = new OdometryFLOAM(parameters);
break; break;
case Odometry::kTypeLIOSAM:
odometry = new OdometryLIOSAM(parameters);
break;
case Odometry::kTypeMSCKF: case Odometry::kTypeMSCKF:
odometry = new OdometryMSCKF(parameters); odometry = new OdometryMSCKF(parameters);
break; break;
corelib/src/Parameters.cpp
+6
View File
@@ -903,6 +903,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else #else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; 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 #endif
str = "With FOVIS:"; str = "With FOVIS:";
#ifdef RTABMAP_FOVIS #ifdef RTABMAP_FOVIS
corelib/src/odometry/OdometryLIOSAM.cpp
+478
View File
@@ -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
+75 -7
View File
@@ -1762,6 +1762,55 @@ LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs); 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) LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{ {
cv::Mat laserScan; cv::Mat laserScan;
@@ -2343,7 +2392,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
{ {
pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud); 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); pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
} }
@@ -3807,9 +3856,11 @@ LaserScan deskew(
return LaserScan(); 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(); return LaserScan();
} }
@@ -3865,7 +3916,14 @@ LaserScan deskew(
double stamp; double stamp;
UTimer processingTime; UTimer processingTime;
double scanTime = lastStamp - firstStamp; 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(); int offsetIntensity = input.getIntensityOffset();
bool isLocalTransformIdentity = input.localTransform().isIdentity(); bool isLocalTransformIdentity = input.localTransform().isIdentity();
Transform localTransformInv = input.localTransform().inverse(); Transform localTransformInv = input.localTransform().inverse();
@@ -3904,7 +3962,12 @@ LaserScan deskew(
dataPtr[0] = pt.x; dataPtr[0] = pt.x;
dataPtr[1] = pt.y; dataPtr[1] = pt.y;
dataPtr[2] = pt.z; 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[0] = pt.x;
dataPtr[1] = pt.y; dataPtr[1] = pt.y;
dataPtr[2] = pt.z; 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)); output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed()); 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
+1
View File
@@ -371,6 +371,7 @@ private Q_SLOTS:
void changeOdometryORBSLAMVocabulary(); void changeOdometryORBSLAMVocabulary();
void changeOdometryOKVISConfigPath(); void changeOdometryOKVISConfigPath();
void changeOdometryVINSFusionConfigPath(); void changeOdometryVINSFusionConfigPath();
void changeOdometryLIOSAMConfigPath();
void changeOdometryOpenVINSLeftMask(); void changeOdometryOpenVINSLeftMask();
void changeOdometryOpenVINSRightMask(); void changeOdometryOpenVINSRightMask();
void changeIcpPMConfigPath(); void changeIcpPMConfigPath();
guilib/src/PreferencesDialog.cpp
+37
View File
@@ -243,6 +243,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
#ifndef RTABMAP_CUVSLAM #ifndef RTABMAP_CUVSLAM
_ui->odom_strategy->setItemData(13, 0, Qt::UserRole - 1); _ui->odom_strategy->setItemData(13, 0, Qt::UserRole - 1);
#endif #endif
#ifndef RTABMAP_LIOSAM
_ui->odom_strategy->setItemData(14, 0, Qt::UserRole - 1);
#endif
#if CV_MAJOR_VERSION < 3 #if CV_MAJOR_VERSION < 3
_ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1); _ui->stereosgbm_mode->setItemData(2, 0, Qt::UserRole - 1);
@@ -1698,6 +1701,22 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// Odometry CuVSLAM // Odometry CuVSLAM
_ui->odom_cuvslam_multicam_mode->setObjectName(Parameters::kOdomCuVSLAMMulticamMode().c_str()); _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 //StereoDense
_ui->comboBox_stereoDense_strategy->setObjectName(Parameters::kStereoDenseStrategy().c_str()); _ui->comboBox_stereoDense_strategy->setObjectName(Parameters::kStereoDenseStrategy().c_str());
connect(_ui->comboBox_stereoDense_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_stereoDense, SLOT(setCurrentIndex(int))); 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_odomOpenVINS->setVisible(index==10);
_ui->groupBox_odomOpen3D->setVisible(index==12); _ui->groupBox_odomOpen3D->setVisible(index==12);
_ui->groupBox_odomCuvslam->setVisible(index==13); _ui->groupBox_odomCuvslam->setVisible(index==13);
_ui->groupBox_odomLIOSAM->setVisible(index==14);
} }
void PreferencesDialog::useOdomFeatures() 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() void PreferencesDialog::changeOdometryOpenVINSLeftMask()
{ {
QString path; QString path;
guilib/src/ui/preferencesDialog.ui
+457
View File
@@ -16137,6 +16137,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>CuVSLAM</string> <string>CuVSLAM</string>
</property> </property>
</item> </item>
<item>
<property name="text">
<string>LIO-SAM</string>
</property>
</item>
</widget> </widget>
</item> </item>
<item row="3" column="1"> <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> </item>
</layout> </layout>
</widget> </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"> <widget class="QWidget" name="page_26">
<layout class="QVBoxLayout" name="verticalLayout_88"> <layout class="QVBoxLayout" name="verticalLayout_88">
<item> <item>