From fd0937fcbc16a7943b1f0e5b96fadcf6898b07ba Mon Sep 17 00:00:00 2001 From: Ab-Tx Date: Fri, 14 Apr 2023 16:52:02 +0100 Subject: [PATCH] add livox horizon support --- SC-LIO-SAM/config/params_liosam.yaml | 6 +++--- SC-LIO-SAM/include/utility.h | 12 ++++++++---- SC-LIO-SAM/src/imageProjection.cpp | 25 ++++++++++++++++++------- SC-LIO-SAM/src/mapOptmization.cpp | 6 ++++++ 4 files changed, 35 insertions(+), 14 deletions(-) diff --git a/SC-LIO-SAM/config/params_liosam.yaml b/SC-LIO-SAM/config/params_liosam.yaml index d5f6bc8..bea2e82 100644 --- a/SC-LIO-SAM/config/params_liosam.yaml +++ b/SC-LIO-SAM/config/params_liosam.yaml @@ -24,9 +24,9 @@ lio_sam: # warning: if you have already data in the above savePCDDirectory, it will all remove and remake them. Thus, backup is recommended if pre-made data exist. # Sensor Settings - sensor: ouster # lidar sensor type, either 'velodyne' or 'ouster' - N_SCAN: 128 # number of lidar channel (i.e., 16, 32, 64, 128) - Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) + sensor: ouster # lidar sensor type, 'velodyne' or 'ouster' or 'livox' + N_SCAN: 128 # number of lidar channel (i.e., 16, 32, 64, 128, Livox Horizon: 6) + Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)) downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used diff --git a/SC-LIO-SAM/include/utility.h b/SC-LIO-SAM/include/utility.h index 1d796a2..8c682a8 100644 --- a/SC-LIO-SAM/include/utility.h +++ b/SC-LIO-SAM/include/utility.h @@ -12,8 +12,6 @@ #include #include -#include - #include #include #include @@ -28,6 +26,8 @@ #include #include +#include + #include #include #include @@ -59,7 +59,7 @@ typedef std::numeric_limits< double > dbl; typedef pcl::PointXYZI PointType; -enum class SensorType { MULRAN, VELODYNE, OUSTER }; +enum class SensorType { MULRAN, VELODYNE, OUSTER, LIVOX }; class ParamServer { @@ -188,10 +188,14 @@ class ParamServer { sensor = SensorType::MULRAN; } + else if (sensorStr == "livox") + { + sensor = SensorType::LIVOX; + } else { ROS_ERROR_STREAM( - "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'mulran'): " << sensorStr); + "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'mulran' or 'livox'): " << sensorStr); ros::shutdown(); } diff --git a/SC-LIO-SAM/src/imageProjection.cpp b/SC-LIO-SAM/src/imageProjection.cpp index ee76bd5..d45e7a5 100644 --- a/SC-LIO-SAM/src/imageProjection.cpp +++ b/SC-LIO-SAM/src/imageProjection.cpp @@ -98,6 +98,7 @@ class ImageProjection : public ParamServer double timeScanEnd; std_msgs::Header cloudHeader; + vector columnIdnCountLivox; public: ImageProjection(): @@ -153,6 +154,8 @@ class ImageProjection : public ParamServer imuRotY[i] = 0; imuRotZ[i] = 0; } + + columnIdnCountLivox.assign(N_SCAN, 0); } ~ImageProjection(){} @@ -220,7 +223,7 @@ class ImageProjection : public ParamServer cloudQueue.pop_front(); - if (sensor == SensorType::VELODYNE) + if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX) { pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn); } @@ -575,12 +578,20 @@ class ImageProjection : public ParamServer if (rowIdn % downsampleRate != 0) continue; - float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; - - static float ang_res_x = 360.0/float(Horizon_SCAN); - int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; - if (columnIdn >= Horizon_SCAN) - columnIdn -= Horizon_SCAN; + int columnIdn = -1; + if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER) + { + float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; + static float ang_res_x = 360.0/float(Horizon_SCAN); + columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; + if (columnIdn >= Horizon_SCAN) + columnIdn -= Horizon_SCAN; + } + else if (sensor == SensorType::LIVOX) + { + columnIdn = columnIdnCountLivox[rowIdn]; + columnIdnCountLivox[rowIdn] += 1; + } if (columnIdn < 0 || columnIdn >= Horizon_SCAN) continue; diff --git a/SC-LIO-SAM/src/mapOptmization.cpp b/SC-LIO-SAM/src/mapOptmization.cpp index d6503bd..fdaea99 100644 --- a/SC-LIO-SAM/src/mapOptmization.cpp +++ b/SC-LIO-SAM/src/mapOptmization.cpp @@ -1575,6 +1575,12 @@ class mapOptimization : public ParamServer if (cloudKeyPoses3D->points.empty()) return true; + if (sensor == SensorType::LIVOX) + { + if (timeLaserInfoCur - cloudKeyPoses6D->back().time > 1.0) + return true; + } + Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back()); Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);