深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第四章作业

Zcoasting

已于 2023-11-03 12:29:04 修改

阅读量226

点赞数

分类专栏：深蓝学院高翔《自动驾驶与机器人中的SLAM技术》作业文章标签：自动驾驶机器人 c++

于 2023-11-03 12:21:22 首次发布

本文链接：https://blog.csdn.net/Zcoasting/article/details/134200482

版权

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》作业专栏收录该内容

6 篇文章 5 订阅

订阅专栏

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第四章作业

第二章作业详解
 第三章作业详解

第一题

在这里插入图片描述

第二题

对本题的理解为，在处理GNSS与Odom数据时，都会进行图优化。
首先设定一些标志变量，在类GinsPreInteg中设定：

    /// 两帧GNSS观测
    GNSS last_gnss_;
    GNSS this_gnss_;
    bool last_gnss_set_ = false; // 最新的GNSS数据

    IMU last_imu_;    // 上时刻IMU
    Odom last_odom_;  // 上时刻odom
    Odom this_odom_;  // 当前odom
    bool last_odom_set_ = false;  // 最新的Odom数据

    /// 标志位
    bool gnss_opt = false;          
    bool odom_opt = false;           
    bool first_gnss_received_ = false;  // 是否已收到第一个gnss信号
    bool first_imu_received_ = false;   // 是否已收到第一个imu信号
    bool first_odom_received_ = false;  // 是否已收到第一个odom信号
    bool last_opt = false;              // 上一次优化是什么优化，false为gnss触发，true为odom触发

更改Odom回调函数，imu 与 gnss 均初始化后再处理odom数据，保证初始第一次处理odom数据时滑窗内含有一帧gnss数据，确定初始位置。

SetOdomProcessFunc([&](const sad::Odom& odom) {
            imu_init.AddOdom(odom);

            if (imu_inited && gnss_inited) {
                gins.AddOdom(odom);  // imu 与 gnss 均初始化后再处理odom数据
            }
            
            auto state = gins.GetState();
            save_result(fout, state);
            if (ui) {
                ui->UpdateNavState(state);
                usleep(1e3);
            }
        }

实现 AddOdom()，修改 AddGnss()

void GinsPreInteg::AddOdom(const sad::Odom& odom) {
    this_frame_ = std::make_shared<NavStated>(current_time_);
    this_odom_ = odom;

    // odom数据计算世界坐标系下速度
    auto odom_vel = [&](const sad::Odom& odom, std::shared_ptr<NavStated> last) {
        Vec3d vel_world = Vec3d::Zero();
        Vec3d vel_odom = Vec3d::Zero();
        double velo_l =
            options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
        double velo_r =
            options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
        double average_vel = 0.5 * (velo_l + velo_r);
        vel_odom = Vec3d(average_vel, 0.0, 0.0);
        vel_world = last->R_ * vel_odom;
        return vel_world;
    };

    if (!first_odom_received_ && first_gnss_received_) {
        // 此时上一帧 last_frame_ 为 gnss 优化的
        // 利用上一帧确定位置和本帧速度
        this_frame_->timestamp_ = odom.timestamp_;
        this_frame_->p_ = last_frame_->p_;
        this_frame_->R_ = last_frame_->R_;
        this_frame_->v_ = odom_vel(this_odom_, last_frame_);
        this_frame_->bg_ = last_frame_->bg_;   // imu_init 时的值
        this_frame_->ba_ = last_frame_->ba_;
        
        pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);

        last_frame_ = this_frame_;
        last_odom_ = this_odom_;
        first_odom_received_ = true;
        current_time_ = this_odom_.timestamp_;
        return;
    }

    // 积分到ODOM时刻
    pre_integ_->Integrate(last_imu_, odom.timestamp_ - current_time_);
    current_time_ = odom.timestamp_;

    // 从上一个时刻到当前 ODOM 时刻的预积分结果预测当前帧的状态
    *this_frame_ = pre_integ_->Predict(*last_frame_, options_.gravity_);

    odom_opt = true;    // 本次优化为odom触发优化
    Optimize();

    last_frame_ = this_frame_;
    last_odom_ = this_odom_;
    last_odom_set_ = true;     // 最新的Odom数据
}

void GinsPreInteg::AddGnss(const GNSS& gnss) {
    this_frame_ = std::make_shared<NavStated>(current_time_);
    this_gnss_ = gnss;

    if (!first_gnss_received_) {
        if (!gnss.heading_valid_) {
            // 要求首个GNSS必须有航向
            return;
        }

        // 首个gnss信号，将初始pose设置为该gnss信号
        this_frame_->timestamp_ = gnss.unix_time_;
        this_frame_->p_ = gnss.utm_pose_.translation();
        this_frame_->R_ = gnss.utm_pose_.so3();
        this_frame_->v_.setZero();
        this_frame_->bg_ = options_.preinteg_options_.init_bg_;   // imu_init 时的值
        this_frame_->ba_ = options_.preinteg_options_.init_ba_;

        pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);

        last_frame_ = this_frame_;
        last_gnss_ = this_gnss_;
        first_gnss_received_ = true;
        current_time_ = gnss.unix_time_;
        return;
    }

    // 积分到GNSS时刻
    pre_integ_->Integrate(last_imu_, gnss.unix_time_ - current_time_);

    current_time_ = gnss.unix_time_;
    // 从上一个时刻到当前 GNSS 时刻的预积分结果预测当前帧的状态
    *this_frame_ = pre_integ_->Predict(*last_frame_, options_.gravity_);
    
    // 添加标志位，本次优化为gnss触发优化
    gnss_opt = true;
    Optimize();

    last_frame_ = this_frame_;
    last_gnss_ = this_gnss_;
    last_gnss_set_ = true;  // 最新的GNSS数据
}

修改优化函数，只展示修改部分

void GinsPreInteg::Optimize() {
    if (pre_integ_->dt_ < 1e-2) {
        // 未得到积分  // gnss 与 odom 挨得太近了会造成发散 提高点时间阈值
        return;
    }

    // 初始化gnss与odom边
    EdgeGNSS* edge_gnss0 = nullptr;
    EdgeGNSS* edge_gnss1 = nullptr;
    EdgeEncoder3D* edge_odom0 = nullptr;
    EdgeEncoder3D* edge_odom1 = nullptr;
    Vec3d vel_world = Vec3d::Zero();
    Vec3d vel_odom = Vec3d::Zero();

    // 计算世界体系坐标系速度
    auto odom_vel = [&](const sad::Odom& odom, std::shared_ptr<NavStated> last) {
        Vec3d v_world = Vec3d::Zero();
        Vec3d v_odom = Vec3d::Zero();
        double velo_l =
            options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
        double velo_r =
            options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
        double average_vel = 0.5 * (velo_l + velo_r);
        v_odom = Vec3d(average_vel, 0.0, 0.0);
        v_world = last->R_ * v_odom;
        return v_world;
    };

    // 如果是gnss触发优化
    if (gnss_opt) {
        // 如果上一次优化是gnss触发，那么两帧gnss数据之间进行约束
        // 用上一帧last_gnss_来对v0_pose进行约束
        if (!last_opt) {
            edge_gnss0 = new EdgeGNSS(v0_pose, last_gnss_.utm_pose_);
            edge_gnss0->setInformation(options_.gnss_info_);
            optimizer.addEdge(edge_gnss0);
        }
        else {
            // 如果上一次是odom触发优化 
            // 用上一帧 R，p对v0_pose进行约束
            edge_gnss0 = new EdgeGNSS(v0_pose, SE3(last_frame_->R_, last_frame_->p_));
            edge_gnss0->setInformation(options_.gnss_info_);
            optimizer.addEdge(edge_gnss0);
        }

        // this_gnss_约束v1_pose
        edge_gnss1 = new EdgeGNSS(v1_pose, this_gnss_.utm_pose_);
        edge_gnss1->setInformation(options_.gnss_info_);
        optimizer.addEdge(edge_gnss1);

        // 最新的odom数据约束v1_vel
        if (last_odom_set_) {
            // velocity obs
            vel_world = odom_vel(last_odom_, this_frame_);
            edge_odom1 = new EdgeEncoder3D(v1_vel, vel_world);
            edge_odom1->setInformation(options_.odom_info_);
            optimizer.addEdge(edge_odom1);

            // 重置odom数据到达标志位，等待最新的odom数据
            last_odom_set_ = false;
        }
        gnss_opt = false;
        last_opt = false;
    }
    else if (odom_opt) {
        // 如果是odom触发优化
        if (last_gnss_set_) {
            // 如果最新gnss数据可用
            // 判断在两帧优化之间，gnss数据离哪边更近
            // 对更近的一帧进行位置约束
            if (this_frame_->timestamp_ - last_gnss_.unix_time_ > last_gnss_.unix_time_ - last_frame_->timestamp_) {
                edge_gnss1 = new EdgeGNSS(v0_pose, last_gnss_.utm_pose_);
                edge_gnss1->setInformation(options_.gnss_info_);
                optimizer.addEdge(edge_gnss1);
            }
            else {
                edge_gnss1 = new EdgeGNSS(v1_pose, last_gnss_.utm_pose_);
                edge_gnss1->setInformation(options_.gnss_info_);
                optimizer.addEdge(edge_gnss1);
            }
            last_gnss_set_ = false;
        }

        // 两帧odom分别对v0_vel，v1_vel约束
        vel_world = odom_vel(last_odom_, last_frame_);
        edge_odom0 = new EdgeEncoder3D(v0_vel, vel_world);
        edge_odom0->setInformation(options_.odom_info_);
        optimizer.addEdge(edge_odom0);

        vel_world = odom_vel(this_odom_, this_frame_);
        edge_odom1 = new EdgeEncoder3D(v1_vel, vel_world);
        edge_odom1->setInformation(options_.odom_info_);
        optimizer.addEdge(edge_odom1);
        
        odom_opt = false;
        last_opt = true;
    }
    

    optimizer.setVerbose(options_.verbose_);
    optimizer.initializeOptimization();
    optimizer.optimize(20);

    edge_inertial->checkJacobians();  // 第三题
}

优化结果，总体比较平滑，几处偏移较大的地方优化仍有遗憾

在这里插入图片描述

第三题

使用g2o的自动求导工具，将虚函数 linearizeOplus() 及其重载实现注释，在优化过程中就会调用基类中的 void BaseMultiEdge::linearizeOplus() 进行自动求导。
实现 void checkJacobians();

void EdgeInertial::checkJacobians() {
    linearizeOplus();

    auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
    auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
    auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
    auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
    auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
    auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);

    Vec3d bg = bg1->estimate();
    Vec3d ba = ba1->estimate();
    Vec3d dbg = bg - preint_->bg_;

    // 一些中间符号
    const SO3 R1 = p1->estimate().so3();
    const SO3 R1T = R1.inverse();
    const SO3 R2 = p2->estimate().so3();

    auto dR_dbg = preint_->dR_dbg_;
    auto dv_dbg = preint_->dV_dbg_;
    auto dp_dbg = preint_->dP_dbg_;
    auto dv_dba = preint_->dV_dba_;
    auto dp_dba = preint_->dP_dba_;

    // 估计值
    Vec3d vi = v1->estimate();
    Vec3d vj = v2->estimate();
    Vec3d pi = p1->estimate().translation();
    Vec3d pj = p2->estimate().translation();

    const SO3 dR = preint_->GetDeltaRotation(bg);
    const SO3 eR = SO3(dR).inverse() * R1T * R2;
    const Vec3d er = eR.log();
    const Mat3d invJr = SO3::jr_inv(eR); // 雅可比

    Eigen::Matrix3d dR_dR1 = -invJr * (R2.inverse() * R1).matrix();
    Eigen::Matrix3d dv_dR1 = SO3::hat(R1T * (vj - vi - grav_ * dt_));
    Eigen::Matrix3d dp_dR1 = SO3::hat(R1T * (pj - pi - v1->estimate() * dt_ - 0.5 * grav_ * dt_ * dt_));
    Eigen::Matrix3d dp_dp1 = -R1T.matrix();
    Eigen::Matrix3d dv_dv1 = -R1T.matrix();
    Eigen::Matrix3d dp_dv1 = -R1T.matrix() * dt_;
    Eigen::Matrix3d dR_dbg1 = -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;
    Eigen::Matrix3d dv_dbg1 = -dv_dbg;
    Eigen::Matrix3d dp_dbg1 = -dp_dbg;
    Eigen::Matrix3d dv_dba1 = -dv_dba;
    Eigen::Matrix3d dp_dba1 = -dp_dba;
    Eigen::Matrix3d dr_dr2 = invJr;
    Eigen::Matrix3d dp_dp2 = R1T.matrix();
    Eigen::Matrix3d dv_dv2 = R1T.matrix(); 

    LOG(INFO) << "dR_dR1 diff: " << (_jacobianOplus[0].block<3, 3>(0, 0) - dR_dR1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dv_dR1 diff: " << (_jacobianOplus[0].block<3, 3>(3, 0) - dv_dR1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dp_dR1 diff: " << (_jacobianOplus[0].block<3, 3>(6, 0) - dp_dR1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dp_dp1 diff: " << (_jacobianOplus[0].block<3, 3>(6, 3) - dp_dp1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dv_dv1 diff: " << (_jacobianOplus[1].block<3, 3>(3, 0) - dv_dv1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dp_dv1 diff: " << (_jacobianOplus[1].block<3, 3>(6, 0) - dp_dv1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dR_dbg1 diff: " << (_jacobianOplus[2].block<3, 3>(0, 0) - dR_dbg1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dv_dbg1 diff: " << (_jacobianOplus[2].block<3, 3>(3, 0) - dv_dbg1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dp_dbg1 diff: " << (_jacobianOplus[2].block<3, 3>(6, 0) - dp_dbg1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dv_dba1 diff: " << (_jacobianOplus[3].block<3, 3>(3, 0) - dv_dba1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dp_dba1 diff: " << (_jacobianOplus[3].block<3, 3>(6, 0) - dp_dba1).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dr_dr2 diff: " << (_jacobianOplus[4].block<3, 3>(0, 0) - dr_dr2).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dp_dp2 diff: " << (_jacobianOplus[4].block<3, 3>(6, 3) - dp_dp2).lpNorm<Eigen::Infinity>();
    LOG(INFO) << "dv_dv2 diff: " << (_jacobianOplus[5].block<3, 3>(3, 0) - dv_dv2).lpNorm<Eigen::Infinity>();
};