From 7dc3730214ef3d105bff5eba8430aad9f82fe229 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 12 Apr 2026 23:39:12 +0000 Subject: [PATCH] LIO-SAM datasets launch file example --- rtabmap_conversions/src/MsgConversion.cpp | 5 +- .../launch/liosam_datasets.launch | 189 ++++++++++++++++++ rtabmap_odom/src/nodelets/icp_odometry.cpp | 133 ++++++++---- .../src/nodelets/point_cloud_assembler.cpp | 5 +- 4 files changed, 294 insertions(+), 38 deletions(-) create mode 100644 rtabmap_examples/launch/liosam_datasets.launch diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index ff155b4f..bfc57e4a 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2740,8 +2740,9 @@ bool convertScan3dMsg( float maxRange, bool is2D) { - UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height, - uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str()); + UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height || + scan3dMsg.data.size() == scan3dMsg.point_step*scan3dMsg.width*scan3dMsg.height, + uFormat("data=%d row_step=%d point_step=%d width=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.point_step, scan3dMsg.width, scan3dMsg.height).c_str()); rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform); if(scanLocalTransform.isNull()) diff --git a/rtabmap_examples/launch/liosam_datasets.launch b/rtabmap_examples/launch/liosam_datasets.launch new file mode 100644 index 00000000..8a2af20f --- /dev/null +++ b/rtabmap_examples/launch/liosam_datasets.launch @@ -0,0 +1,189 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index ebe904bd..f2b68eee 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -577,8 +577,10 @@ private: void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg) { - UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height, - uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str()); + UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height || + (pointCloudMsg->data.size() == pointCloudMsg->point_step*pointCloudMsg->width*pointCloudMsg->height), + uFormat("data=%d row_step=%d point_step=%d width=%d height=%d", + pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->point_step, pointCloudMsg->width, pointCloudMsg->height).c_str()); if(scanReceived_) { @@ -686,6 +688,8 @@ private: LaserScan scan; bool hasNormals = false; bool hasIntensity = false; + bool hasTime = false; + bool hasRing = false; bool is3D = false; for(unsigned int i=0; ifields.size(); ++i) { @@ -715,6 +719,42 @@ private: } } } + if(cloudMsg->fields[i].name.compare("time") == 0) + { + if(cloudMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32) + { + hasTime = true; + } + else + { + static bool warningShown = false; + if(!warningShown) + { + ROS_WARN("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.", cloudMsg->fields[i].datatype); + warningShown = true; + } + } + } + if(cloudMsg->fields[i].name.compare("ring") == 0) + { + if(cloudMsg->fields[i].datatype == sensor_msgs::PointField::UINT16) + { + hasRing = true; + } + else + { + static bool warningShown = false; + if(!warningShown) + { + ROS_WARN("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.", cloudMsg->fields[i].datatype); + warningShown = true; + } + } + } } if(cloudMsg->height > 1) // organized cloud @@ -778,47 +818,72 @@ private: } else if(hasIntensity) { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*cloudMsg, *pclScan); - if(pclScan->size() && scanDownsamplingStep_ > 1) + if(hasTime && scanVoxelSize_== 0.0f && scanNormalK_ == 0 && scanNormalRadius_ == 0.0f) { - pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); - if(pclScan->height>1) - { - maxLaserScans = pclScan->height * pclScan->width; - } - else - { - maxLaserScans /= scanDownsamplingStep_; - } + pcl::PCLPointCloud2 pclCloud; + pcl_conversions::moveToPCL(*cloudMsg, pclCloud); + scan = util3d::laserScanFromPointCloud(pclCloud, true, !is3D); } - if(!pclScan->is_dense) + else { - pclScan = util3d::removeNaNFromPointCloud(pclScan); - } + if(hasTime) + { + static bool warned = false; + if(!warned) + { + NODELET_WARN("Input cloud has \"time\" channel, but as scan_voxel_size (%s) " + "and/or scan_normal_k (%s) and/or scan_normal_radius (%s) are set, " + "time is dropped. To make sure to passthrough the time channel to " + "underlaying odometry, set those parameters to 0.", + Parameters::kIcpVoxelSize().c_str(), + Parameters::kIcpPointToPlaneK().c_str(), + Parameters::kIcpPointToPlaneRadius().c_str()); + warned=true; + } + } - if(pclScan->size()) - { - if(scanVoxelSize_ > 0.0f) + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*cloudMsg, *pclScan); + if(pclScan->size() && scanDownsamplingStep_ > 1) { - float pointsBeforeFiltering = (float)pclScan->size(); - pclScan = util3d::voxelize(pclScan, scanVoxelSize_); - float ratio = float(pclScan->size()) / pointsBeforeFiltering; - maxLaserScans = int(float(maxLaserScans) * ratio); + pclScan = util3d::downsample(pclScan, scanDownsamplingStep_); + if(pclScan->height>1) + { + maxLaserScans = pclScan->height * pclScan->width; + } + else + { + maxLaserScans /= scanDownsamplingStep_; + } } - if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) + if(!pclScan->is_dense) { - //compute normals - pcl::PointCloud::Ptr normals = is3D? - util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_): - util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_); - pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); - pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); - scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal); + pclScan = util3d::removeNaNFromPointCloud(pclScan); } - else + + if(pclScan->size()) { - scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan); + if(scanVoxelSize_ > 0.0f) + { + float pointsBeforeFiltering = (float)pclScan->size(); + pclScan = util3d::voxelize(pclScan, scanVoxelSize_); + float ratio = float(pclScan->size()) / pointsBeforeFiltering; + maxLaserScans = int(float(maxLaserScans) * ratio); + } + if(scanNormalK_ > 0 || scanNormalRadius_>0.0f) + { + //compute normals + pcl::PointCloud::Ptr normals = is3D? + util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_): + util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_); + pcl::PointCloud::Ptr pclScanNormal(new pcl::PointCloud); + pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); + scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal); + } + else + { + scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan); + } } } } diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index a60779a8..b85f2310 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -305,8 +305,9 @@ private: callbackCalled_ = true; if(cloudPub_.getNumSubscribers()) { - UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, - uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str()); + UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height || + cloudMsg->data.size() == cloudMsg->point_step*cloudMsg->width*cloudMsg->height, + uFormat("data=%d row_step=%d point_step=%d width=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->point_step, cloudMsg->width, cloudMsg->height).c_str()); if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_) {