传统的 旋转式雷达，激光固定在雷达的旋转部件上， 依靠转子的转动而获得360的旋转视野，由于旋转部件的存在，为了获得更加精准的数据，需要跟多的人工校准和更复杂的设计，也因此推高了其价格。 不过因为很直观的数据方式，所以 edge 特征和 plane 特征也比较直观。
Livox 雷达推出的特有的非重复扫描方式，使得特征提取变得不那么直观，而且LIvox 推出的自由的数据结构方式，加上ROS原有的 Pointcloud2 使得很多开源的SLAM 算法只支持一种数据格式， 导致很多辛辛苦苦采集的数据不能直接使用。
接下来就自己总结一下 Livox 激光雷达的一些特征
Horizon 发布时间较早， 相关算法支持也最好。
目前能找到的开源算法中， 都只支持 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的特征提取的方式。
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
//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 对数据进行矫正是十分有必要的， 依靠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<std::list<Estimator::LidarFrame>> 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<Estimator::LidarFrame>);
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<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;
}
}
//北向角度、东向角度、地向角度,姿态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");
//解析 CSV
ifstream 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,VelD
getline(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_COMBINED
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#else
imu_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));
#endif
noiseModel::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 的预积分已经成为了标准操作。 但是目前使用livox的算法， 只用内置的 6 轴的 IMU 进行激光数据的矫正， 还没有直接使用IMU进行预积分处理， 也没有找到6轴IMU 预计分的代码结果用以对比。 因为9 轴 imu 能够通过 磁力计提供较为准确的 航向角， 而通过angular_veolcity 不可避免的拥有累计误差，所以9轴的确实比较直接快捷。 不知道读者怎么看.
参考资料：