Livox Lidar 特征提取方式总结
传统的 旋转式雷达，激光固定在雷达的旋转部件上， 依靠转子的转动而获得360的旋转视野，由于旋转部件的存在，为了获得更加精准的数据，需要跟多的人工校准和更复杂的设计，也因此推高了其价格。 不过因为很直观的数据方式，所以 edge 特征和 plane 特征也比较直观。
Livox 雷达推出的特有的非重复扫描方式，使得特征提取变得不那么直观，而且LIvox 推出的自由的数据结构方式，加上ROS原有的 Pointcloud2 使得很多开源的SLAM 算法只支持一种数据格式， 导致很多辛辛苦苦采集的数据不能直接使用。
接下来就自己总结一下 Livox 激光雷达的一些特征
开源SLAM算法
Horizon 发布时间较早， 相关算法支持也最好。
- LIO-Livox : Lidar-Inertial Odometry, 使用了内置的 6 轴IMU， 目前只支持 horizon 雷达， 雷达数据结构只支持 livox_ros_driver/CustomMsg. LINK
- Livox-mapping: 仅使用激光雷达的建图包， 同时支持了 Mid 系列和 horizon 系统的扫描方式， 但Horizon 数据结构还是必须为 livox_ros_driver/CustomMsg， Mid 系列的数据类型为 sensor_msgs::PointCloud。LINK
- hku-mars/loam_livox: Lidar only 的建图包， 只支持 Mid 系列（sensor_msgs::PointCloud2） 格式的数据。 LINK
- BALM ，使用了bundle adjustment 的仅使用激光的建图包， 同时支持三种， horizon 支持 livox_ros_driver/CustomMsg 格式，Mid 系列的数据类型为 sensor_msgs::PointCloud， 也提供了velodyne 激光的 sensor_msgs::PointCloud 格式。link
- KIT-ISAS/lili-om: LINK LiDAR-Inertial Odometry, 但此处没有使用内置的IMU， 而是使用的的 9 轴 Xsens MTi-670 (200 Hz) IMU, Livox雷达内置的是一个 6 轴激光雷达， 支持Horizon 和 Velodyne 雷达 LINK
Horzion 特征提取
目前能找到的开源算法中， 都只支持 livox_ros_driver/CustomMsg 数据格式， 其内容为：
link
## livox_ros_driver/CustomMsg
# Livox publish pointcloud msg format.
Header header # ROS standard message header
uint64 timebase # The time of first point
uint32 point_num # Total number of pointclouds
uint8 lidar_id # Lidar device id number
uint8[3] rsvd # Reserved use
CustomPoint[] points # Pointcloud data
## livox_ros_driver/CustomPoint
# Livox costom pointcloud format.
uint32 offset_time # offset time relative to the base time
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 tag # livox tag
uint8 line # laser number in lidar
其特点是， 我们能够根据 激光数据的 timebase 和每个激光数据点的 offset_time 推出每个点的时间先后顺序，同时此数据结构也提供了 每个点所在的激光序号， Horizon中有五个激光束。
以官方的Livox_mapping 为例：
首先其使用 livox_repub.cpp 文件，将 livox livox_ros_driver::CustomMsg 数据转换成 sensor_msgs::PointCloud2 数据结构：
ros::Subscriber sub_livox_msg1 = nh.subscribe<livox_ros_driver::CustomMsg>(
"/livox/lidar", 100, LivoxMsgCbk1);
pub_pcl_out1 = nh.advertise<sensor_msgs::PointCloud2>("/livox_pcl0", 100);
ros::Publisher pub_pcl_out0, pub_pcl_out1;
uint64_t TO_MERGE_CNT = 1;
constexpr bool b_dbg_line = false;
std::vector<livox_ros_driver::CustomMsgConstPtr> livox_data;
void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {
livox_data.push_back(livox_msg_in);
if (livox_data.size() < TO_MERGE_CNT) return;
pcl::PointCloud<PointType> pcl_in;
for (size_t j = 0; j < livox_data.size(); j++) {
auto& livox_msg = livox_data[j];
auto time_end = livox_msg->points.back().offset_time;
for (unsigned int i = 0; i < livox_msg->point_num; ++i) {
PointType pt;
pt.x = livox_msg->points[i].x;
pt.y = livox_msg->points[i].y;
pt.z = livox_msg->points[i].z;
float s = livox_msg->points[i].offset_time / (float)time_end;
pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp
pt.curvature = s*0.1;
pcl_in.push_back(pt);
}
}
unsigned long timebase_ns = livox_data[0]->timebase;
ros::Time timestamp;
timestamp.fromNSec(timebase_ns);
sensor_msgs::PointCloud2 pcl_ros_msg;
pcl::toROSMsg(pcl_in, pcl_ros_msg);
pcl_ros_msg.header.stamp.fromNSec(timebase_ns);
pcl_ros_msg.header.frame_id = "/livox";
pub_pcl_out1.publish(pcl_ros_msg);
livox_data.clear();
}
其中主要部分是
auto time_end = livox_msg->points.back().offset_time;
PointType pt;
pt.x = livox_msg->points[i].x;
pt.y = livox_msg->points[i].y;
pt.z = livox_msg->points[i].z;
float s = livox_msg->points[i].offset_time / (float)time_end;
pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamp
pt.curvature = s*0.1;
pcl_in.push_back(pt);
其中在这里记录了currature , 是该点在总时间段的比例， 并将其放在 curvature 字段。在 intensity 字段放的 line number 和 反射率。
在 scanRegistration_horizon.cpp 中， 其定义了 horizon的特征提取的方式。
平面点(plane)的提取
float depth = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
// if(depth < 2) depth_threshold = 0.05;
// if(depth > 30) depth_threshold = 0.1;
//left curvature
float ldiffX =
laserCloud->points[i - 4].x + laserCloud->points[i - 3].x
- 4 * laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x + laserCloud->points[i].x;
float ldiffY =
laserCloud->points[i - 4].y + laserCloud->points[i - 3].y
- 4 * laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y + laserCloud->points[i].y;
float ldiffZ =
laserCloud->points[i - 4].z + laserCloud->points[i - 3].z
- 4 * laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z + laserCloud->points[i].z;
float left_curvature = ldiffX * ldiffX + ldiffY * ldiffY + ldiffZ * ldiffZ;
if(left_curvature < 0.01){
std::vector<PointType> left_list;
for(int j = -4; j < 0; j++){
left_list.push_back(laserCloud->points[i+j]);
}
if( left_curvature < 0.001) CloudFeatureFlag[i-2] = 1; //surf point flag && plane_judge(left_list,1000)
left_surf_flag = true;
}
else{
left_surf_flag = false;
}
There is same process for right curve
角点Edge特征提取
//surf-surf corner feature
if(left_surf_flag && right_surf_flag){
debugnum4 ++;
Eigen::Vector3d norm_left(0,0,0);
Eigen::Vector3d norm_right(0,0,0);
for(int k = 1;k<5;k++){
Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i-k].x-laserCloud->points[i].x,
laserCloud->points[i-k].y-laserCloud->points[i].y,
laserCloud->points[i-k].z-laserCloud->points[i].z);
tmp.normalize();
norm_left += (k/10.0)* tmp;
}
for(int k = 1;k<5;k++){
Eigen::Vector3d tmp = Eigen::Vector3d(laserCloud->points[i+k].x-laserCloud->points[i].x,
laserCloud->points[i+k].y-laserCloud->points[i].y,
laserCloud->points[i+k].z-laserCloud->points[i].z);
tmp.normalize();
norm_right += (k/10.0)* tmp;
}
//calculate the angle between this group and the previous group
double cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) );
//calculate the maximum distance, the distance cannot be too small
Eigen::Vector3d last_tmp = Eigen::Vector3d(laserCloud->points[i-4].x-laserCloud->points[i].x,
laserCloud->points[i-4].y-laserCloud->points[i].y,
laserCloud->points[i-4].z-laserCloud->points[i].z);
Eigen::Vector3d current_tmp = Eigen::Vector3d(laserCloud->points[i+4].x-laserCloud->points[i].x,
laserCloud->points[i+4].y-laserCloud->points[i].y,
laserCloud->points[i+4].z-laserCloud->points[i].z);
double last_dis = last_tmp.norm();
double current_dis = current_tmp.norm();
if(cc < 0.5 && last_dis > 0.05 && current_dis > 0.05 ){ //
debugnum5 ++;
CloudFeatureFlag[i] = 150;
}
完成了 特征提取就是，mapping 的过程， 此部分和 LOAM 部分类似。
IMU对 Lidar 数据矫正
根据IMU 对数据进行矫正是十分有必要的， 依靠IMU 高频率的对角速度和线性加速度的测量 预测每一个点相对初始点的旋转误差和线性误差可以比较充分的纠正位姿：以下是一个纠正前后的对比实例：
可以看到由于运动扭曲的桌腿得到了恢复， 这对于特征的跟踪十分有帮助。
这里有一个 IMU integrator 对imu 数据进行 PreIntegration link
void IMUIntegrator::GyroIntegration(double lastTime){
double current_time = lastTime;
for(auto & imu : vimuMsg){
Eigen::Vector3d gyr;
gyr << imu->angular_velocity.x,
imu->angular_velocity.y,
imu->angular_velocity.z;
double dt = imu->header.stamp.toSec() - current_time;
ROS_ASSERT(dt >= 0);
// 将旋转角速度转换为 旋转矩阵
Eigen::Matrix3d dR = Sophus::SO3d::exp(gyr*dt).matrix();
// 得到此时的 orientation
Eigen::Quaterniond qr(dq*dR);
if (qr.w()<0)
qr.coeffs() *= -1;
// 正则化, 得到（积分后的）旋转矩阵
dq = qr.normalized();
current_time = imu->header.stamp.toSec();
}
}
void IMUIntegrator::PreIntegration(double lastTime, const Eigen::Vector3d& bg, const Eigen::Vector3d& ba){
Reset();
linearized_bg = bg;
linearized_ba = ba;
double current_time = lastTime;
for(auto & imu : vimuMsg){
Eigen::Vector3d gyr;
gyr << imu->angular_velocity.x,
imu->angular_velocity.y,
imu->angular_velocity.z;
Eigen::Vector3d acc;
acc << imu->linear_acceleration.x * gnorm,
imu->linear_acceleration.y * gnorm,
imu->linear_acceleration.z * gnorm;
double dt = imu->header.stamp.toSec() - current_time;
if(dt <= 0 )
ROS_WARN("dt <= 0");
gyr -= bg;
acc -= ba;
double dt2 = dt*dt;
Eigen::Vector3d gyr_dt = gyr*dt;
// 将小量李代数 转化到李群
Eigen::Matrix3d dR = Sophus::SO3d::exp(gyr_dt).matrix();
Eigen::Matrix3d Jr = Eigen::Matrix3d::Identity();
double gyr_dt_norm = gyr_dt.norm(); // Return to the suqarnorm
if(gyr_dt_norm > 0.00001){
Eigen::Vector3d k = gyr_dt.normalized();
Eigen::Matrix3d K = Sophus::SO3d::hat(k); //李代数到反对称矩阵
Jr = Eigen::Matrix3d::Identity()
- (1-cos(gyr_dt_norm))/gyr_dt_norm*K
+ (1-sin(gyr_dt_norm)/gyr_dt_norm)*K*K;
}
Eigen::Matrix<double,15,15> A = Eigen::Matrix<double,15,15>::Identity();
A.block<3,3>(0,3) = -0.5*dq.matrix()*Sophus::SO3d::hat(acc)*dt2;
A.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt;
A.block<3,3>(0,12) = -0.5*dq.matrix()*dt2;
A.block<3,3>(3,3) = dR.transpose();
A.block<3,3>(3,9) = - Jr*dt;
A.block<3,3>(6,3) = -dq.matrix()*Sophus::SO3d::hat(acc)*dt;
A.block<3,3>(6,12) = -dq.matrix()*dt;
Eigen::Matrix<double,15,12> B = Eigen::Matrix<double,15,12>::Zero();
B.block<3,3>(0,3) = 0.5*dq.matrix()*dt2;
B.block<3,3>(3,0) = Jr*dt;
B.block<3,3>(6,3) = dq.matrix()*dt;
B.block<3,3>(9,6) = Eigen::Matrix3d::Identity()*dt;
B.block<3,3>(12,9) = Eigen::Matrix3d::Identity()*dt;
jacobian = A * jacobian;
covariance = A * covariance * A.transpose() + B * noise * B.transpose();
dp += dv*dt + 0.5*dq.matrix()*acc*dt2;
dv += dq.matrix()*acc*dt;
Eigen::Matrix3d m3dR = dq.matrix()*dR;
Eigen::Quaterniond qtmp(m3dR);
if (qtmp.w()<0)
qtmp.coeffs() *= -1;
dq = qtmp.normalized();
dtime += dt;
current_time = imu->header.stamp.toSec();
}
}
boost::shared_ptr> lidar_list;
if(!vimuMsg.empty()){
if(!LidarIMUInited) {
// if get IMU msg successfully, use gyro integration to update delta_Rl
lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
lidarFrame.imuIntegrator.GyroIntegration(time_last_lidar);
delta_Rb = lidarFrame.imuIntegrator.GetDeltaQ().toRotationMatrix();
delta_Rl = exTlb.topLeftCorner(3, 3) * delta_Rb * exTlb.topLeftCorner(3, 3).transpose();
// predict current lidar pose
lidarFrame.P = transformAftMapped.topLeftCorner(3,3) * delta_tb
+ transformAftMapped.topRightCorner(3,1);
Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3,3) * delta_Rb;
lidarFrame.Q = m3d;
lidar_list.reset(new std::list);
lidar_list->push_back(lidarFrame);
}else{
// if get IMU msg successfully, use pre-integration to update delta lidar pose
lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
lidarFrame.imuIntegrator.PreIntegration(lidarFrameList->back().timeStamp, lidarFrameList->back().bg, lidarFrameList->back().ba);
const Eigen::Vector3d& Pwbpre = lidarFrameList->back().P;
const Eigen::Quaterniond& Qwbpre = lidarFrameList->back().Q;
const Eigen::Vector3d& Vwbpre = lidarFrameList->back().V;
const Eigen::Quaterniond& dQ = lidarFrame.imuIntegrator.GetDeltaQ();
const Eigen::Vector3d& dP = lidarFrame.imuIntegrator.GetDeltaP();
const Eigen::Vector3d& dV = lidarFrame.imuIntegrator.GetDeltaV();
double dt = lidarFrame.imuIntegrator.GetDeltaTime();
lidarFrame.Q = Qwbpre * dQ;
lidarFrame.P = Pwbpre + Vwbpre*dt + 0.5*GravityVector*dt*dt + Qwbpre*(dP);
lidarFrame.V = Vwbpre + GravityVector*dt + Qwbpre*(dV);
lidarFrame.bg = lidarFrameList->back().bg;
lidarFrame.ba = lidarFrameList->back().ba;
Eigen::Quaterniond Qwlpre = Qwbpre * Eigen::Quaterniond(exRbl);
Eigen::Vector3d Pwlpre = Qwbpre * exPbl + Pwbpre;
Eigen::Quaterniond Qwl = lidarFrame.Q * Eigen::Quaterniond(exRbl);
Eigen::Vector3d Pwl = lidarFrame.Q * exPbl + lidarFrame.P;
delta_Rl = Qwlpre.conjugate() * Qwl;
delta_tl = Qwlpre.conjugate() * (Pwl - Pwlpre);
delta_Rb = dQ.toRotationMatrix();
delta_tb = dP;
lidarFrameList->push_back(lidarFrame);
lidarFrameList->pop_front();
lidar_list = lidarFrameList;
}
}else{
if(LidarIMUInited)
break;
else{
// predict current lidar pose
lidarFrame.P = transformAftMapped.topLeftCorner(3,3) * delta_tb
+ transformAftMapped.topRightCorner(3,1);
Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3,3) * delta_Rb;
lidarFrame.Q = m3d;
lidar_list.reset(new std::list);
lidar_list->push_back(lidarFrame);
}
}
// remove lidar distortion 矫正运动
RemoveLidarDistortion(laserCloudFullRes, delta_Rl, delta_tl);
void RemoveLidarDistortion(pcl::PointCloud::Ptr& cloud,
const Eigen::Matrix3d& dRlc, const Eigen::Vector3d& dtlc){
int PointsNum = cloud->points.size();
for (int i = 0; i < PointsNum; i++) {
Eigen::Vector3d startP;
float s = cloud->points[i].normal_x;
Eigen::Quaterniond qlc = Eigen::Quaterniond(dRlc).normalized();
Eigen::Quaterniond delta_qlc = Eigen::Quaterniond::Identity().slerp(s, qlc).normalized();
const Eigen::Vector3d delta_Plc = s * dtlc;
startP = delta_qlc * Eigen::Vector3d(cloud->points[i].x,cloud->points[i].y,cloud->points[i].z) + delta_Plc;
Eigen::Vector3d _po = dRlc.transpose() * (startP - dtlc);
cloud->points[i].x = _po(0);
cloud->points[i].y = _po(1);
cloud->points[i].z = _po(2);
cloud->points[i].normal_x = 1.0;
}
}
IMU - GTSAM 中的使用
//北向角度、东向角度、地向角度,姿态w,姿态x,姿态y,姿态z,北向速度，东向速度，地向速度
//N，E，D，qx,qy,qz,qw,VelN,VelE,VelD
//i:表示初始化
i,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0
0,0.000100,0.0,0.0,0.0,0.0,0.0
1,0.000000,0.0,0.0,0.0,0.0,0.0,1.0
0,0.000200,0.0,0.0,0.0,0.0,0.0
0,0.000300,0.0,0.0,0.0,0.0,0.0
0,0.000400,0.0,0.0,0.0,0.0,0.0
0,0.000500,0.0,0.0,0.0,0.0,0.0
#include IMU Preintegration
IMU 的预积分已经成为了标准操作。 但是目前使用livox的算法， 只用内置的 6 轴的 IMU 进行激光数据的矫正， 还没有直接使用IMU进行预积分处理， 也没有找到6轴IMU 预计分的代码结果用以对比。 因为9 轴 imu 能够通过 磁力计提供较为准确的 航向角， 而通过angular_veolcity 不可避免的拥有累计误差，所以9轴的确实比较直接快捷。 不知道读者怎么看.
参考资料：
- https://github.com/KIT-ISAS/lili-om
- https://github.com/smilefacehh/LIO-SAM-DetailedNote
- https://github.com/YuePanEdward/MULLS
- https://github.com/hyye/lio-mapping
- https://github.com/ChaoqinRobotics/LINS—LiDAR-inertial-SLAM
- https://github.com/Livox-SDK/LIO-Livox
- https://github.com/Livox-SDK/livox_mapping
- https://github.com/hku-mars/loam_livox
- https://github.com/hku-mars/BALM