传统的旋转式雷达，激光固定在雷达的旋转部件上，依靠转子的转动而获得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 timestamppt.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 curvaturefloat 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 featureif(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 groupdouble cc = fabs( norm_left.dot(norm_right) / (norm_left.norm()*norm_right.norm()) );//calculate the maximum distance, the distance cannot be too smallEigen::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();// 得到此时的 orientationEigen::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 suqarnormif(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<std::list<Estimator::LidarFrame>> lidar_list;if(!vimuMsg.empty()){if(!LidarIMUInited) {// if get IMU msg successfully, use gyro integration to update delta_RllidarFrame.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 poselidarFrame.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<Estimator::LidarFrame>);lidar_list->push_back(lidarFrame);}else{// if get IMU msg successfully, use pre-integration to update delta lidar poselidarFrame.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 poselidarFrame.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<Estimator::LidarFrame>);lidar_list->push_back(lidarFrame);}}// remove lidar distortion 矫正运动RemoveLidarDistortion(laserCloudFullRes, delta_Rl, delta_tl);

void RemoveLidarDistortion(pcl::PointCloud<PointType>::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 <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <fstream>
#include <iostream>using namespace std;
using namespace gtsam;
using symbol_shorthand::X;//用作表示    姿态(x,y,z,r,p,y)
using symbol_shorthand::V;//用表示      速度导数(xdot,ydot,zdot)
using symbol_shorthand::B;//陀螺仪残差  (ax,ay,az,gx,gy,gz)
string inputfile="/home/n1/notes/gtsam/Imu/imuAndGPSdata.csv";
string outputfile="/home/n1/notes/gtsam/Imu/result1.csv";
PreintegrationType *imu_preintegrated_;
//    Matrix3 biasAccCovariance;     3*3矩阵 加速度计的协防差矩阵，（可以根据残差计算加速度雅克比矩阵逐步更新）
//    Matrix3 biasOmegaCovariance;   3*3矩阵 陀螺仪的协防差矩阵， （可以根据残差计算雅克比矩阵递归更新预计分值，防止从头计算）
//    Matrix6 biasAccOmegaInt;       6*6矩阵 位置残关于加速度和速度残差的协防差，用作更新预计分
int main(int argc, const char** argv) {FILE* fp=fopen(outputfile.c_str(),"w+");//输出fprintf(fp,"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");//解析 CSVifstream file(inputfile.c_str());string value;Eigen::Matrix<double,10,1> initial_state=Eigen::Matrix<double,10,1>::Zero();N，E，D，qx,qy,qz,qw,VelN,VelE,VelDgetline(file,value,',');for(int i=0;i<9;i++){getline(file,value,',');initial_state(i)=atof(value.c_str());//转为浮点型}getline(file,value,'\n');//换行initial_state(9) = atof(value.c_str());Rot3 prior_rotation=Rot3::Quaternion(initial_state(6),initial_state(3),initial_state(4),initial_state(5));Point3 prior_point(initial_state(0),initial_state(1),initial_state(2));Pose3 prior_pose(prior_rotation,prior_point);                               //初始位姿Vector3 prior_velocity(initial_state(7),initial_state(8),initial_state(9)); //初始速度imuBias::ConstantBias prior_imu_bias;//残差，默认设为0//初始化值Values initial_values;int correction_count=0;//位姿initial_values.insert(X(correction_count),prior_pose);//速度initial_values.insert(V(correction_count),prior_velocity);//残差initial_values.insert(B(correction_count),prior_imu_bias);cout << "initial state:\n" << initial_state.transpose() <<endl;//设置噪声模型//一般为设置为对角噪声noiseModel::Diagonal::shared_ptr pose_noise_model=noiseModel::Diagonal::Sigmas(Vector6(0.01,0.01,0.01,0.5,0.5,0.5));noiseModel::Diagonal::shared_ptr velocity_noise_model=noiseModel::Isotropic::Sigma(3,0.1);noiseModel::Diagonal::shared_ptr bias_noise_model=noiseModel::Isotropic::Sigma(6,0.001);NonlinearFactorGraph *graph = new NonlinearFactorGraph();graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));//使用传感器信息构建IMU的噪声模型double accel_noise_sigma = 0.0003924;double gyro_noise_sigma = 0.000205689024915;double accel_bias_rw_sigma = 0.004905;double gyro_bias_rw_sigma = 0.000001454441043;Matrix33 measured_acc_cov=Matrix33::Identity(3,3)*pow(accel_noise_sigma,2);Matrix33 measured_omega_cov=Matrix33::Identity(3,3)*pow(gyro_bias_rw_sigma,2);Matrix33 integration_error_cov=Matrix33::Identity(3,3)*1e-8;        //速度积分误差Matrix33 bias_acc_cov=Matrix33::Identity(3,3)*pow(accel_bias_rw_sigma,2);Matrix33 bias_omega_cov=Matrix33::Identity(3,3)*pow(gyro_bias_rw_sigma,2);Matrix66 bias_acc_omega_int=Matrix66::Identity(6,6)*1e-5;           //积分骗到误差boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p=PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);//MakeSharedD:NED坐标系，g默认为 9.81，这里设置为0//MakeSharedU：NEU坐标系，g默认为 9.81//设置预积分分参数p->accelerometerCovariance=measured_acc_cov;p->integrationCovariance=integration_error_cov;p->gyroscopeCovariance=measured_omega_cov;//预计分测量值p->biasAccCovariance=bias_acc_cov;p->biasAccOmegaInt=bias_acc_omega_int;p->biasOmegaCovariance=bias_omega_cov;
#ifdef USE_COMBINEDimu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#elseimu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
#endif//保存上一次的imu积分值和结果NavState prev_state(prior_pose,prior_velocity);NavState prop_state=prev_state;imuBias::ConstantBias prev_bias=prior_imu_bias;////记录总体误差double current_position_error = 0.0, current_orientation_error = 0.0;double output_time=0;double dt=0.005;    //积分时间//使用数据进行迭代while(file.good()){getline(file,value,',');int type=atoi(value.c_str());//字符转为整形if (type == 0) { // IMU 测量数据Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();//读取imu数据for (int i=0; i<5; ++i) {getline(file, value, ',');imu(i) = atof(value.c_str());}getline(file, value, '\n');imu(5) = atof(value.c_str());// 检测测量值加入预计分imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);}else if(type ==1){//Gps测量数据Eigen::Matrix<double,7,1> gps=Eigen::Matrix<double,7,1>::Zero();for(int i=0;i<6;i++){getline(file,value,',');gps(i)=atof(value.c_str());}getline(file, value, '\n');gps(6)=atof(value.c_str());correction_count++;#ifdef USE_COMBINED//预计分测量值PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);//IMU 因子//typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,imuBias::ConstantBias, imuBias::ConstantBias>CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),X(correction_count  ), V(correction_count  ),B(correction_count-1), B(correction_count  ),*preint_imu_combined);graph->add(imu_factor);
#else//预计分测量值PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),X(correction_count  ), V(correction_count  ),B(correction_count-1),*preint_imu);graph->add(imu_factor);imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),B(correction_count  ),zero_bias, bias_noise_model));
#endifnoiseModel::Diagonal::shared_ptr correction_noise=noiseModel::Isotropic::Sigma(3,1.0);GPSFactor gps_factor(X(correction_count),Point3(gps(0),gps(1),gps(2)),//(N,E,D)correction_noise);graph->add(gps_factor);//迭代更新求解imu预测值prop_state=imu_preintegrated_->predict(prev_state,prev_bias);initial_values.insert(X(correction_count), prop_state.pose());initial_values.insert(V(correction_count), prop_state.v());initial_values.insert(B(correction_count), prev_bias);//求解LevenbergMarquardtOptimizer optimizer(*graph,initial_values);Values result=optimizer.optimize();//更新下一步预计分初始值//导航状态prev_state=NavState(result.at<Pose3>(X(correction_count)),result.at<Vector3>(V(correction_count)));//偏导数prev_bias=result.at<imuBias::ConstantBias>(B(correction_count));//更新预计分值imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);//计算角度误差和误差Vector3 gtsam_position=prev_state.pose().translation();//位置误差Vector3 position_error=gtsam_position-gps.head<3>();//误差的范数current_position_error=position_error.norm();//归一化//姿态误差Quaternion gtsam_quat=prev_state.pose().rotation().toQuaternion();Quaternion gps_quat(gps(6),gps(3),gps(4),gps(5));Quaternion quat_error=gtsam_quat*gps_quat.inverse();quat_error.normalized();//归一化Vector3 euler_angle_error(quat_error.x()*2,quat_error.y()*2,quat_error.z()*2);//转换为欧拉角误差current_orientation_error=euler_angle_error.norm();//输出误差cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),gps(0), gps(1), gps(2),gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());output_time += 1.0;}else{cerr << "ERROR parsing file\n";return 1;}}fclose(fp);cout << "完成,结果见：" <<outputfile  << endl;return 0;
}

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

Livox Lidar 特征提取方式总结相关推荐

Livox LiDAR点云数据间的格式转化
livox_ros_driver下载:https://github.com/Livox-SDK/livox_ros_driver 官方示例数据下载:https://pan.baidu.com/s/1C ...
opencv 梯度幅值_opencv3/C++ HOG特征提取方式
opencv3/C++ HOG特征提取方式发布时间:2020-08-23 16:24:01 来源:脚本之家阅读:111 HOG特征 HOG(Histograms of Oriented Gradi ...
Livox Lidar+海康Camera实时生成彩色点云
Livox Lidar + HIKROBOT Camera系列最近在开发相机和激光雷达融合的slam算法,主要用于三维重建,想实时的得到彩色点云地图,传感器选择了海康威视的工业相机和大疆的固态激光 ...
Livox Lidar+海康Camera 基于loam的实时三维重建生成RGB彩色点云
Livox Lidar + HIKROBOT Camera系列最近在开发相机和激光雷达融合的slam算法,主要用于三维重建,想实时的得到彩色点云地图,传感器选择了海康威视的工业相机和大疆的固态激光 ...
基于激光雷达点云的3D检测方法汇总(LiDAR only)
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨柒柒@知乎来源丨https://zhuanlan.zhihu.com/p/436452723 ...
Livox 开源分享：关于激光雷达去畸变的那些事儿
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨Livox 览沃激光雷达@知乎来源丨https://zhuanlan.zhihu.com/p/ ...
一文览尽LiDAR点云目标检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达转载于 :计算机视觉之路,作者:山涧一壶酒 / 导读 / 自动驾驶中的激光雷达点云如何做特征表达,将基 ...
一文览尽基于激光雷达点云（lidar）的目标检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文来源:计算机视觉之路,作者:山涧一壶酒,编辑:智车科技 / 导读 / 上周文章:自动驾驶中的激光雷 ...
大疆激光雷达livox avia 在ubuntu18.04+ROS中基本使用方法
Readme 大疆livox avia 固态激光雷达操作步骤: 1.参考本人CSDN博客lsg_dawn中收藏中的一篇关于ubuntu18.04中使用livox avia 雷达的文章 Livox Av ...

Livox Lidar 特征提取方式总结