Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add livox horizon support #30

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add livox horizon support #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions SC-LIO-SAM/config/params_liosam.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 8 additions & 4 deletions SC-LIO-SAM/include/utility.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,6 @@
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <opencv/cv.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
Expand All @@ -28,6 +26,8 @@
#include <pcl/filters/crop_box.h>
#include <pcl_conversions/pcl_conversions.h>

#include <opencv2/opencv.hpp>

#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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();
}

Expand Down
25 changes: 18 additions & 7 deletions SC-LIO-SAM/src/imageProjection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,7 @@ class ImageProjection : public ParamServer
double timeScanEnd;
std_msgs::Header cloudHeader;

vector<int> columnIdnCountLivox;

public:
ImageProjection():
Expand Down Expand Up @@ -153,6 +154,8 @@ class ImageProjection : public ParamServer
imuRotY[i] = 0;
imuRotZ[i] = 0;
}

columnIdnCountLivox.assign(N_SCAN, 0);
}

~ImageProjection(){}
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 6 additions & 0 deletions SC-LIO-SAM/src/mapOptmization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
Expand Down