SLAM中的非线性优化-2D图优化之视觉惯性VIO(二十-终结篇)

      本讲为2D图优化终极篇，本讲结束后，基本进入3D篇了，主要讲解完整的2d图优化slam，将之前章节的所有关键内容均融合进该讲，本节并非理论讲解，而是已工程化为主，与上一讲间隔时间较长，那是因为2D vio需要自己原创，且视觉开发需要较多时间，因此现在才更新，一个除了回环检测外的，滑动窗口算法，从工程构建到目前初版开发，也整整花费了我快十天时间了，与之前一两天开发的小工程相比，这个开发难度大很多，并且数据也较为难获取，本章节内容，并非写完后就不更新了，后续还会持续优化，力求做到理论与实际保持一致，2d惯性视觉slam的理论意义大于实际，主要还是为了理解理论知识，完整版的代码如下

https://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch20_vio_2dhttp://2D视觉惯性里程计注意：后续还会更新回环检测部分，敬请持续关注

一. 算法思路讲解

       核心算法实现部分参考vins-mono,不过因为地面机器人/汽车等有轮速里程计等信息，可简化很多，如零速，距离等信息，可直接通过轮子获取到。基本思路大概为，读取特征以及imu，轮速等的信息；

（1） 先获取200帧数据，进行imu偏置估计，并初始化；

（2）通过imu预积分，构建imu相关两帧间的约束；

（3） 通过轮速计获得两帧间的wheel odometry约束；

（4） 通过轮速计获得关键帧速度约束；

（5） 两个关键帧取间隔0.3米；

（6）对关键帧进行特征匹配，取10帧为一个滑动窗口，对窗口内的特征三角化

（7）最后优化滑动窗口算法，利用imu预积分，wheel 预积分，视觉特征，帧间bias， 轮速等信息构建优化器，并边缘化最老帧；

上述即为该算法的大体思路。

二. 代码讲解

初始化以及滑窗优化

void SlidingWindow::StartMapping(const ImuData& imu_data, const WheelData& wheel_data, const std::vector<FeaturePoint>& feature_data) {if(!is_init_) {Initialize(imu_data, wheel_data, feature_data);if(publisher_ptr_ != nullptr) {publisher_ptr_->SetState(Eigen::Vector3d(state_.p[0], state_.p[1], 1.0));}} else {StartProcessSliddingWindow(imu_data, wheel_data, feature_data);if(publisher_ptr_ != nullptr) {publisher_ptr_->SetState(Eigen::Vector3d(state_.p[0], state_.p[1], 1.0));}}
}

    SE2 current_wheel_pose = SE2(wheel_data.x, wheel_data.y, wheel_data.t);Eigen::Vector3d current_imu = TransformImuData(imu_data);float delta_time = imu_data.timestamp - last_imu_timestamp_;last_imu_timestamp_ = imu_data.timestamp;imu_preintegration_ptr_->Integrate(last_imu_, current_imu, delta_time);wheel_preintegration_ptr_->Integrate(last_wheel_pose_, current_wheel_pose);last_imu_ = current_imu;if((current_wheel_pose.translation() - last_wheel_pose_.translation()).norm() < 0.3) {return;}State last_state = state_;// std::cout << "before state_ " << state_.p.transpose() << std::endl;state_ = imu_preintegration_ptr_->Predict(last_state);LOG(INFO) << "predict state_ " << state_.p.transpose() << ", " << state_.R.log() << std::endl;LOG(INFO) << "current_wheel_pose " << current_wheel_pose.translation().transpose() << ", " << current_wheel_pose.so2().log() << std::endl;// LOG(INFO) << "wheel_data " << wheel_data.vehicle_speed << std::endl;SliddingWindow(imu_data, wheel_data, feature_data);imu_preintegration_ptr_ = std::make_shared<ImuPreIntegration>(imu_options_);wheel_preintegration_ptr_ = std::make_shared<WheelPreIntegration>(wheel_options_);last_wheel_pose_ = current_wheel_pose;frame_id_ += 1;

关于滑窗的所有处理函数，如上图所示，参考完整代码进行阅读吧

三. 总结

      本讲主要讲解了视觉惯性里程计的大体思路，实现也很简单，目前该版本还会持续更新，2D系列主要讲解了相对困难的imu直接积分与预积分部分，轮式里程计直接积分与预积分，2d激光概率优化，视觉与2D传感器融合，后续开发也可能会讲gps约束等，2D篇系列已耽误较长时间，后续会开发3D系列，敬请期待吧