i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +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
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);
}
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());
}