VINS-MONO代码解读6----pose_graph

开始pose_graph部分，本部分记住一句话无论是快速重定位还是正常重定位，求出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​就是终极目标。

还剩一个整体Pipeline~~

1. pose_graph_node.cpp

注意，定义全局变量时即实例化了一个对象

PoseGraph posegraph; //定义全局的位姿图优化对象

加载配置
订阅topic及对应的callback()
注册publisher
定义主线程和键盘线程，
若需要loop则
取出对应的msg，时间戳对齐
最重要的是新建KF，添加KF到posegraph中

2. pose_graph.cpp

2.1 回环检测

int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
相关的debug语句就不进行介绍。

  1. 查询字典，给出查询结果ret，并将描述子添加到数据库中，db.query()的传参见注释。
    //first query; then add this frame into database!
    QueryResults ret;
    TicToc t_query;
    //查询字典数据库，得到与每一帧的相似度评分ret
    //param：描述子；返回结果(主要是Entry id和相似度评分Score)；最多返回几个；<=max_id的才被返回
    db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);

    TicToc t_add;
    //描述子加到数据库中
    db.add(keyframe->brief_descriptors);
  1. 判定是否找到loop
    //至少两个，且第1个的score>0.05才可能是loop
    if (ret.size() >= 1 &&ret[0].Score > 0.05)
        for (unsigned int i = 1; i < ret.size(); i++)
        {
            //if (ret[i].Score > ret[0].Score * 0.3)
            //后面只要再有一个score>0.015的就判定找到了loop
            if (ret[i].Score > 0.015)
            {          
                find_loop = true;//
  1. 返回 50帧之后，最远的，score>0.015的结果
    //只有50帧之后才返回loop查询结果
    if (find_loop && frame_index > 50)
    {
        int min_index = -1;
        for (unsigned int i = 0; i < ret.size(); i++)
        {
            //找最远的，且score>0.015的结果
            if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
                min_index = ret[i].Id;
        }
        return min_index;
    }
    else
        return -1;

2.2 PnP 求解相对位姿

bool KeyFrame::findConnection(KeyFrame* old_kf)
其中关键函数是searchByBRIEFDes()和PnPRANSAC()
需要说明，searchByBRIEFDes()找到的是 i i i帧的2D点，而PnPRANSAC()最终输出在PnP_T_old, PnP_R_old中的是 T w 2 b i T_{w_2b_i} Tw2​bi​​

//关键帧WINDOW内的描述子与回环帧的描述子(brief_descriptors，不知道后面有没有把WINDOW内的描述子合并到这里面来)进行BRIEF描述子匹配，剔除匹配失败的点
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);

......

//如果能匹配的特征点能达到最小回环匹配个数，则用RANSAC PnP检测再去除误匹配的点，求出的是(PnP_T_old, PnP_R_old)=Tw2_bi
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
	status.clear();
    PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
    reduceVector(matched_2d_cur, status);//根据status去除outlier
    reduceVector(matched_2d_old, status);
    reduceVector(matched_2d_cur_norm, status);
    reduceVector(matched_2d_old_norm, status);
    reduceVector(matched_3d, status);
    reduceVector(matched_id, status);
......   
}

对 T w 2 b i T_{w_2b_i} Tw2​bi​​这个量是不是很熟悉？对，这就是estimator中的relo_Pose，有了这个量，就能求出 T b i b j T_{b_ib_j} Tbi​bj​​，存在loop_info中，后面就可以算出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​执行矫正了：

  //相对位姿检验
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
      //Tbi_bj=Tw2_bi^(-1) * Tw2_bj
   relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
   relative_q = PnP_R_old.transpose() * origin_vio_R;
   //Rbi_bj.yaw
   relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
   if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)
   {
   	has_loop = true;
   	loop_index = old_kf->index;
   	//更新this KFde loop_info  Tbi_bj，后面要用
   	loop_info << relative_t.x(), relative_t.y(), relative_t.z(),
   	             relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
   	             relative_yaw;
          //快速重定位:组装msg，发送match_points topic，estimator接收到该消息后进行快速重定位
   	if(FAST_RELOCALIZATION)
......balabala

在addKeyFrame()中，拿loop_info中的 T b i b j T_{b_ib_j} Tbi​bj​​计算出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​并执行矫正，全部变换到 w 1 w_1 w1​系下：

    //若存在回环候选帧
	if (loop_index != -1)
	{
        //printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
        //获得回环帧
        KeyFrame* old_kf = getKeyFrame(loop_index);
        //PnP 求解Tbi_w2，进而求出Tbi_bj存放于loop_info中
        if (cur_kf->findConnection(old_kf))
        {
            //更新最早的回环帧index
            if (earliest_loop_index > loop_index || earliest_loop_index == -1)
                earliest_loop_index = loop_index;

            Vector3d w_P_old, w_P_cur, vio_P_cur;
            Matrix3d w_R_old, w_R_cur, vio_R_cur;
            old_kf->getVioPose(w_P_old, w_R_old);//获得Tw1_bi
            cur_kf->getVioPose(vio_P_cur, vio_R_cur);//Tw2_bj

            //relative是Tbi_bj=Told_cur
            Vector3d relative_t;
            Quaterniond relative_q;
            //读取Tbi_bj
            relative_t = cur_kf->getLoopRelativeT();//这个loop_info是在findConnection()中更新的
            relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
            //Tw1_bj = Tw1_bi * Tbi_bj
            w_P_cur = w_R_old * relative_t + w_P_old;
            w_R_cur = w_R_old * relative_q;

            //回环得到的位姿和VIO位姿之间的偏移量Tw1_w2
            double shift_yaw;
            Matrix3d shift_r;
            Vector3d shift_t;
            //Tw1_w2
            shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
            shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
            //tw1_bj - Rw1_bj * Rw2_bj^(-1) * tw2_bj = tw1_w2
            shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;

            // shift vio pose of whole sequence to the world frame
            // 如果存在多个图像序列，则将所有图像序列都从w2系拉回到w1系下
            if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
            {  
                w_r_vio = shift_r;
                w_t_vio = shift_t;
                //Tw1_bj = Tw1_w2 * Tw1_bj，执行relocation
                vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                vio_R_cur = w_r_vio *  vio_R_cur;
                cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
                list<KeyFrame*>::iterator it = keyframelist.begin();
                for (; it != keyframelist.end(); it++)   
                {
                    //TODO：这sequence到底是啥意思？是说跟着loop上的帧后面还有一些帧，都需要relo回来吗？
                    if((*it)->sequence == cur_kf->sequence)
                    {
                        Vector3d vio_P_cur;
                        Matrix3d vio_R_cur;
                        (*it)->getVioPose(vio_P_cur, vio_R_cur);
                        vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                        vio_R_cur = w_r_vio *  vio_R_cur;
                        (*it)->updateVioPose(vio_P_cur, vio_R_cur);
                    }
                }
                sequence_loop[cur_kf->sequence] = 1;
            }

            //将当前帧放入优化队列中
            m_optimize_buf.lock();
            optimize_buf.push(cur_kf->index);
            m_optimize_buf.unlock();
        }
	}

3. keyframe.cpp/h

构建了两个类：

  • BriefExtractor：用于计算BRIEF描述子
  • KeyFrame：用于对KF进行相关管理(重点)
  1. BriefExtractor类主要使用node中加载的描述子的pattern来计算BRIEF描述子，其中重载了()运算符
class BriefExtractor
{
public:
    //读取 构建字典时使用的相同的Brief模板文件，构造BriefExtractord
    virtual void operator()(const cv::Mat &im, vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const;
    //运算符重载了“()”来计算描述子。
    BriefExtractor(const std::string &pattern_file);

    DVision::BRIEF m_brief;
};
  1. KeyFrame类定义了一些函数，包括计算描述子，进行匹配，PnP计算relative pose等。

构造时会分别调用以下函数计算WINDOW内已有的features的描述子，新提features、去畸变、计算描述子。均使用到了BriefExtractor中重载的()运算符，函数较简单，不进行进一步解释。

computeWindowBRIEFPoint();//计算窗口中所有特征点的描述子
computeBRIEFPoint();//对新KF再重新检出500个FAST角点，并计算所有特征点的描述子，用于loop detection

3.1 快速重定位(涉及pose_graph、estimator两个模块)

这部分稍微有些复杂，同时涉及到pose_graph和estimator两个模块，当时讲到estimator后端求解时在构建系统整体的ResidualBlock时遇到过，当时没有具体讲解，现在有了pose_graph的基础后对此部分进行详细探究。
rqt_graph中可以看出estimator接收了来自于pose_graph的topic：match_points
在这里插入图片描述
崔华坤的PDF：
在这里插入图片描述

本部分难点有二：

  1. 理解pose_graph和estimator中关于快速重定位msg的数据结构和组织形式；
  2. 理解estimator中的选点策略。

和前面一样，针对loop，old帧记为 i i i帧，cur帧记为 j j j帧。

在findConnection()函数中PnPRANSAC最后会输出 T w b i T_{wb_i} Twbi​​，存放于PnP_T_old, PnP_R_old中，在FAST_RELOCALIZATION之前会计算出relative pose T b i b j T_{b_ib_j} Tbi​bj​​，然后开始组装并pub match_points的msg：
其中

  • msg_match_points.header.stamp是 j j j帧的时间戳，用于重定位时与WINDOW内的
  • msg_match_points.points前两维存的是loop上的点在 i i i帧中的归一化坐标的xy，z是该feature的id
  • T,Q是 i i i帧的pose： T w b i T_{wb_i} Twbi​​，在setReloFrame/estimator.cpp中赋给prev_relo_t, prev_relo_q
//keyframe.cpp
    //快速重定位:组装msg，发送match_points topic，estimator接收到该消息后进行快速重定位
   	if(FAST_RELOCALIZATION)
   	{
	    sensor_msgs::PointCloud msg_match_points;//特征点之间的匹配
        //这个是cur帧的时间戳，estimator靠该时间戳在WINDOW中找到需要重定位的帧，通过该时间戳和old_frame的index来将old与cur建立联系
	    msg_match_points.header.stamp = ros::Time(time_stamp);
	    //msg的point是归一化的xy和landmark的id
	    for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
	    {
            geometry_msgs::Point32 p;
            p.x = matched_2d_old_norm[i].x;//i帧的归一化xy
            p.y = matched_2d_old_norm[i].y;
            p.z = matched_id[i];
            msg_match_points.points.push_back(p);
	    }
        //Tw_bi
	    Eigen::Vector3d T = old_kf->T_w_i;
	    Eigen::Matrix3d R = old_kf->R_w_i;
	    Quaterniond Q(R);
	    sensor_msgs::ChannelFloat32 t_q_index;
	    t_q_index.values.push_back(T.x());
	    t_q_index.values.push_back(T.y());
	    t_q_index.values.push_back(T.z());
	    t_q_index.values.push_back(Q.w());
	    t_q_index.values.push_back(Q.x());
	    t_q_index.values.push_back(Q.y());
	    t_q_index.values.push_back(Q.z());
	    t_q_index.values.push_back(index);
	    msg_match_points.channels.push_back(t_q_index);
	    pub_match_points.publish(msg_match_points);
   	}

在estimator_node.cpp中接收到match_points的msg之后执行回调relocalization_callback将消息存入relo_buf，在主线程process中处理relo_buf中的数据，并setReloFrame()设置重定位帧。其中

  • match_points中存的是 i , j i,j i,j两帧loop上的feature在 i i i帧中的归一化xy，z是feature id，在optimization中选点时需要使用。
//estimator_node.cpp
            // 取relo_buf中最新的帧，设置重定位用的回环帧
            // set relocalization frame
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;
            while (!relo_buf.empty())
            {
                relo_msg = relo_buf.front();
                relo_buf.pop();
            }
            if (relo_msg != NULL)
            {
                vector<Vector3d> match_points;
                double frame_stamp = relo_msg->header.stamp.toSec();
                for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                {
                    Vector3d u_v_id;
                    u_v_id.x() = relo_msg->points[i].x;//i帧归一化xy
                    u_v_id.y() = relo_msg->points[i].y;
                    u_v_id.z() = relo_msg->points[i].z;//feature id
                    match_points.push_back(u_v_id);
                }
                //Tw_bi（i就是old帧，理解为loop frame v）
                Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
                Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
                Matrix3d relo_r = relo_q.toRotationMatrix();
                int frame_index;
                frame_index = relo_msg->channels[0].values[7];//old frame的帧号
                estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);//设置重定位帧(relo包括loop detection等操作)
            }


//estimator.cpp
void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
    relo_frame_stamp = _frame_stamp;//与old frame loop上的WINDOW内的帧(j帧)的时间戳
    relo_frame_index = _frame_index;//j帧的帧号
    match_points.clear();
    match_points = _match_points;//i帧中与j帧中match上的点在i帧中的归一化(x,y,id)
    //Tw1_bi=Tw_b_old
    prev_relo_t = _relo_t;
    prev_relo_r = _relo_r;
    for(int i = 0; i < WINDOW_SIZE; i++)
    {
        if(relo_frame_stamp == Headers[i].stamp.toSec())//
        {
            relo_frame_local_index = i;//j帧在WINDOW中的下标
            relocalization_info = 1;
            for (int j = 0; j < SIZE_POSE; j++)
                relo_Pose[j] = para_Pose[i][j];//将WINDOW内用于relo的pose赋值给relo_Pose
        }
    }
}

在后端求解的optimization()函数中处理了上面设置的reloframe，其中，选点策略见下图，用到了match_points中z()保存的feature_id。

该部分的ResidualBlock目的是为了求 T w 1 b i T_{w_1b_i} Tw1​bi​​，这里pts_j是i帧的归一化平面上的点，这里理解relo_Pose极其重要，relo_Pose实际上是 T w 2 b i T_{w_2b_i} Tw2​bi​​，视觉重投影是从WINDOW内的start帧的camera(在w2系下)，投影到i帧(在w1系下)，relo_Pose中耦合了从 w 2 w_2 w2​系到 w 1 w_1 w1​系下的变换 T w 1 w 2 T_{w_1w_2} Tw1​w2​​，而我们在pose_graph中是需要求出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​的。无论是快速重定位还是正常重定位，求出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​就是终极目标。

在这里插入图片描述

    //4.添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化(pose graph)准备 或者是 快速重定位(崔华坤PDF7.2节)
    //这里注意relo_pose是Tw2_bi = Tw2_w1 * Tw1_bi
    if(relocalization_info)
    {
        //printf("set relocalization factor! \n");
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
        int retrive_feature_index = 0;
        int feature_index = -1;
        for (auto &it_per_id : f_manager.feature)
        {
            it_per_id.used_num = it_per_id.feature_per_frame.size();
            if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
                continue;
            ++feature_index;
            int start = it_per_id.start_frame;
            if(start <= relo_frame_local_index)//必须之前看到过
            {
                //1.先在i中match的点中找到可能是现在这个feature的id的index
                while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)//.z()存的是i，j两帧match上的feature的id
                {
                    retrive_feature_index++;
                }
                //2.如果是，则WINDOW内的it_per_id.feature_id这个id的landmark就是被loop上的landmark,取归一化坐标，
                if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
                {
                    //pts_j是i帧的归一化平面上的点，这里理解relo_Pose及其重要，relo_Pose实际上是Tw2_bi，视觉重投影是从WINDOW内的start帧的camera(在w2系下)，投影到i帧(在w1系下)，耦合了Tw1_w2
                    Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
                    Vector3d pts_i = it_per_id.feature_per_frame[0].point;//start中的点
                    
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    //relo_Pose是Tw2_bi
                    problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                    retrive_feature_index++;
                }     
            }
        }
    }

在double2vector()中计算了 T w 1 w 2 T_{w_1w_2} Tw1​w2​​和 T b i b j T_{b_ib_j} Tbi​bj​​，为什么叫快速重定位？就是因为此时在estimator中一轮优化之后得到了 T w 2 b i T_{w_2b_i} Tw2​bi​​（即relo_Pose），就可以用 T w 2 b i T_{w_2b_i} Tw2​bi​​和 T w 1 b i T_{w_1b_i} Tw1​bi​​（即prev_relo_r, prev_relo_t）来算出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​，求出了终极目标就可以执行重定位了(在pubOdometry()中)。

需要注意，pitch和roll因为是可观的，所以在estimator中一直都有优化，所以我们重定位只需要矫正yaw和t即可。代码注释和计算方法如下：

// relative info between two loop frame
if(relocalization_info)
{
    //按照WINDOW内第一帧的yaw角变化对j帧进行矫正
    Matrix3d relo_r;//j帧矫正之后的T
    Vector3d relo_t;
    relo_r = rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]).normalized().toRotationMatrix();
    relo_t = rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0],
                                 relo_Pose[1] - para_Pose[0][1],
                                 relo_Pose[2] - para_Pose[0][2]) + origin_P0;//保证第[0]帧不变之后，+origin_P0转为世界系下的t

    //由于pitch和roll是可观的，所以我们在BA中一直都在优化pitch和roll，但由于yaw不可观，
    //所以即使漂了，可能还是满足我们BA的最优解，所以需要手动进行矫正
    //prev_relo_r=Tw1_bi, relo_Pose=Tw2_bi，这两个pose间的yaw和t的漂移Rw1_w2,tw1_w2
    double drift_correct_yaw;
    //yaw drift, Rw1_bi.yaw() - Rw2_bi.yaw=Rw1_w2.yaw()
    drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x();
    drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));
    //tw1_w2
    drift_correct_t = prev_relo_t - drift_correct_r * relo_t;


    //Tw2_bi^(-1) * Tw2_bj = Tbi_bj
    relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);
    relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index];
    //Rw2_bj.yaw() - Rw2_bi.yaw() = Rbi_bj.yaw()
    relo_relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());
    relocalization_info = 0;    
}

在这里插入图片描述

在这里插入图片描述

pubOdometry()中的快速重定位

//重定位时计算出的Tw1_w2，把现在的拉回到之前的上面去
Vector3d correct_t;
Vector3d correct_v;
Quaterniond correct_q;
//Tdrift_correct_r = T_w1_w2把world系的漂移拉回去
correct_t = estimator.drift_correct_r * estimator.Ps[WINDOW_SIZE] + estimator.drift_correct_t;
correct_q = estimator.drift_correct_r * estimator.Rs[WINDOW_SIZE];
odometry.pose.pose.position.x = correct_t.x();
odometry.pose.pose.position.y = correct_t.y();
odometry.pose.pose.position.z = correct_t.z();
odometry.pose.pose.orientation.x = correct_q.x();
odometry.pose.pose.orientation.y = correct_q.y();
odometry.pose.pose.orientation.z = correct_q.z();
odometry.pose.pose.orientation.w = correct_q.w();

loop_info更新的几个地方：

  • KeyFrame()的第2个构造函数，在load pose garph时会使用
  • updateKeyFrameLoop中，在estimator完成FAST_RELOCATION后发送msg，pose_graph线程接收到msg后的回调函数中调用updateKeyFrameLoop更新
  • findConnection中更新，在找到KF之后求 T b i b j T_{b_ib_j} Tbi​bj​​时会更新

3.2 4DoF pose_graph优化

此部分和重定位跑在两个不同的线程

(1). 4DOF优化的残差边的构建，这里面包括两类，一是序列边(即每个关键帧和时序上前四帧之间的相对Pose)，二是回环边(即回环检测成功产生的相对Pose)；
(2). 另外需要注意的是，最早回环检测成功的回环关键帧之前的历史关键帧对此处的Pose_graph优化没有太大的帮助(很直观的理解)；
(3). 还需要注意的是要考虑线程异步的问题，从代码中可以发现，在Pose_graph优化后只是更新关键帧列表中各个关键帧的T_w_i和R_w_i，并不直接改变关键帧的vio_T_w_i和vio_R_w_i，主要是因为若直接修正关键帧的vio_T_w_i和vio_R_w_i可能会导致VIO后端优化时出现异常，特别是当存在比较大的位姿调整时这个问题更加明显。

4DoF PoseGraph可以这样理解：观测和预测实际上计算方法是一样的，观测使用了yaw,pitch,roll,tx,ty,tz，而预测中，pitch和roll不能动（因为estimator已经在优化了），所以只能调整yaw,tx,ty,tz来减小residual。这样来理解pose graph就好理解了。

factor构建部分，其中angle_local_parameterization是定义的yaw的计算更新方式（具体解释见ceres博客4.1.2节），本部分BA使用的是autodiff。

  problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
  problem.AddParameterBlock(t_array[i], 3);

  //第1次loop上的帧和sequence=0的帧的欧拉角和t不优化
  if ((*it)->index == first_looped_index || (*it)->sequence == 0)
  {   
      problem.SetParameterBlockConstant(euler_array[i]);
      problem.SetParameterBlockConstant(t_array[i]);
  }

  //add sequential edge 找i前面的4帧[i-j]，计算(b[i-j])tb[i-j]_b[i], (w)Rb[i-j]_b[i].yaw都是从[i-j]<-[i]和论文顺序相反
  for (int j = 1; j < 5; j++)
  {
    if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
    {
      //Rw_b[i-j]
      Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
      //tw_b[i] - tw_b[i-j] = (w)tb[i-j]_b[i]
      Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
      //计算观测t_diff：Rw_b[i-j]^(-1) * (w)tb[i-j]_b[i] = (b[i-j])tb[i-j]_b[i]
      relative_t = q_array[i-j].inverse() * relative_t;
      //计算观测yaw_diff：Rw_b[i].yaw - Rw_b[i-j].yaw = (w)Rb[i-j]_b[i].yaw*(是个标量，也无所谓参考系，只是知道从[i]到[i-j]的yaw即可)
      double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
      ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                     relative_yaw, euler_conncected.y(), euler_conncected.z());
      problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], 
                              t_array[i-j], 
                              euler_array[i], 
                              t_array[i]);
    }
  }

  //add loop edge
  
  if((*it)->has_loop)
  {
      //loop上的帧号肯定大于第一次loop的帧号
      assert((*it)->loop_index >= first_looped_index);
      int connected_index = getKeyFrame((*it)->loop_index)->local_index;
      Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
      Vector3d relative_t;
      relative_t = (*it)->getLoopRelativeT();//Tbibj的t
      double relative_yaw = (*it)->getLoopRelativeYaw();//Tbibj的yaw(i,j按照loop的理解)
      //构建pose_graph factor
      //注意，ceres里面构建的Rwi是由Create()中的euler_conncected.y(), euler_conncected.z()和AddResidualBlock中的euler_array[connected_index]构建的
      ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                                                 relative_yaw, euler_conncected.y(), euler_conncected.z());
      //分别是loop边和序列边(sequential edge)
      problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], 
                                                    t_array[connected_index], 
                                                    euler_array[i], 
                                                    t_array[i]);
      
  }

其中关于残差边的构建，我对于此处world系的理解：优化参数在给初值时的world是 w 2 w_2 w2​，在优化后由于保持了全局一致性，就变为 w 1 w_1 w1​，代码和论文中residual构建的方向相反，论文中translation是“小-大”，代码中确是“大-小”，不过只要外层观测relative_t，relative_yaw和内层operator()中的预测t_i_ij保持一致就没有问题(都是“大-小”)。

//4为residual，输入参数分别为1(yaw_i)，3(tw2_i)，1(yaw_j)，3(tw2_j)，注意这里的w在优化前是w2，在优化后由于保持了全局一致性，就变为w1
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
								   const double relative_yaw, const double pitch_i, const double roll_i) 
{
     //分别是sequential和loop边的yaw(1)和t(3)的residual
  return (new ceres::AutoDiffCostFunction<
          FourDOFError, 4, 1, 3, 1, 3>(
          	new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
}

优化完之后，就变为 T w 1 b j T_{w1_bj} Tw1b​j​，与优化前的 T w 2 b j T_{w_2b_j} Tw2​bj​​一起计算出 T w 1 w 2 T_{w_1w_2} Tw1​w2​​即可进行全局pose矫正，注意更新的是VIO pose还是非VIO pose。

  1. 这里仍然使用loop中的叫法： i i i帧为old帧， j 帧 j帧 j帧为WINDOW内的loop帧。
    solve之后更新了pose graph中所有的pose的成员T_w_i, R_w_i，并未更新vio_T_w_i， vio_R_w_i，这是因为优化后，即进行了重定位，如果有漂移的话，会被拉回去，导致world系改变，从 w 2 w_2 w2​变为 w 1 w_1 w1​，所以updatePose更新之后，此时*it中的T_w_i, R_w_i即为 T w 1 b j T_{w_1b_j} Tw1​bj​​，而vio_T_w_i， vio_R_w_i仍然是 T w 2 b j T_{w_2b_j} Tw2​bj​​，这样就可以更新 T w 1 w 2 T_{w_1w_2} Tw1​w2​​了。

  2. 更新 T w 1 w 2 T_{w_1w_2} Tw1​w2​​之后就可以矫正keyframelist中的当前优化帧之后的帧的非VIO pose。

  3. 最后updatePath，遍历keyframelist，获取矫正之后的非VIO pose，并pub出去。

//这里计算的应该是Tw1_w2
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);//pose graph优化后的pose Tw1_bj
cur_kf->getVioPose(vio_t, vio_r);//优化前的pose  Tw2_bj
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();//Rw1_bj.yaw - Rw2_bj.yaw= Rw1_w2.yaw
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;//(w1)tw1_bj - Rw1_w2 * (w2)tw2_bj = (w1)tw1_w2
m_drift.unlock();


//更新重定位帧之后的KF的非VIO pose
it++;
for (; it != keyframelist.end(); it++)
{
    Vector3d P;
    Matrix3d R;
    (*it)->getVioPose(P, R);
    P = r_drift * P + t_drift;
    R = r_drift * R;
    (*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();

至此，pose_graph的整体工作就完成，而VINS-MONO的整个框架解读也就完成了。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/247992.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

26种主流的神经网络偏微分方程求解方法汇总

26种主流的神经网络偏微分方程求解方法汇总

偏微分方程&#xff08;PDE&#xff09;是数学中一门重要的分支&#xff0c;应用范围广泛涉及自然科学、工程技术、生物学领域等。然而我们都知道&#xff0c;偏微分方程的求解过程异常艰难&#xff0c;如果碰上了特别复杂的&#xff0c;传统的计算方法可能需要数百万个CPU小时…
阅读更多...
[多线程]一篇文章带你看懂Java中的synchronized关键字（线程安全）锁的深入理解

[多线程]一篇文章带你看懂Java中的synchronized关键字（线程安全）锁的深入理解

目录 1.前言 2.synchronized的特性 2.1synchronized前言 2.2乐观锁和悲观锁 2.3重量级锁和轻量级锁 重量级锁 &#xff1a; 轻量级锁&#xff1a; 2.4自旋锁和挂起等待锁 2.5 公平锁和非公平锁 公平锁&#xff1a; 非公平锁&#xff1a; 2.6可重入锁和不可重入锁 可…
阅读更多...
bat 脚本的常用特殊符号

bat 脚本的常用特殊符号

1、 命令行回显屏蔽符 2、% 批处理变量引导符 3、> 重定向符 4、>> 重定向符 5、<、>&、<& 重定向符 6、| 命令管道符 7、^ 转义字符 8、& 组合命令 9、&& 组合命令 10、|| 组合命令 11、"" 字符串界定符 12、, 逗号…
阅读更多...
报错“找不到mfc100u.dll,程序无法继续执行”的解决方法，完美解决

报错“找不到mfc100u.dll,程序无法继续执行”的解决方法，完美解决

在软件操作过程中&#xff0c;部分用户可能遇到"计算机缺失mfc140u.dll导致无法启动程序"的困扰。这种情况常常发生在启动某特定应用&#xff0c;特别是需要VC Redistributable支持的软件时。以下为详尽解决策略&#xff0c;让用户轻松应对这类技术难题&#xff0c;重…
阅读更多...
React中的setState执行机制

React中的setState执行机制

我这里今天下雨了&#xff0c;温度一下从昨天的22度降到今天的6度&#xff0c;家里和学校已经下了几天雪了&#xff0c;还是想去玩一下的&#xff0c;哈哈&#xff0c;只能在图片里看到了。 一. setState是什么 它是React组件中用于更新状态的方法。它是类组件中的方法&#x…
阅读更多...
人工智能与天文：技术前沿与未来展望

人工智能与天文：技术前沿与未来展望

人工智能与天文&#xff1a;技术前沿与未来展望 一、引言 随着科技的飞速发展&#xff0c;人工智能&#xff08;AI&#xff09;在各个领域的应用越来越广泛。在天文领域&#xff0c;AI也发挥着越来越重要的作用。本文将探讨人工智能与天文学的结合&#xff0c;以及这种结合带…
阅读更多...
SpringBoot之JSON参数,路径参数的详细解析

SpringBoot之JSON参数,路径参数的详细解析

1.6 JSON参数 在学习前端技术时&#xff0c;我们有讲到过JSON&#xff0c;而在前后端进行交互时&#xff0c;如果是比较复杂的参数&#xff0c;前后端通过会使用JSON格式的数据进行传输。 &#xff08;JSON是开发中最常用的前后端数据交互方式&#xff09; 我们学习JSON格式参…
阅读更多...
在VS2010上使用C#调用非托管C++生成的DLL文件（图文讲解）

在VS2010上使用C#调用非托管C++生成的DLL文件（图文讲解）

背景 在项目过程中&#xff0c;有时候你需要调用非C#编写的DLL文件&#xff0c;尤其在使用一些第三方通讯组件的时候&#xff0c;通过C#来开发应用软件时&#xff0c;就需要利用DllImport特性进行方法调用。本篇文章将引导你快速理解这个调用的过程。 步骤 1. 创建一个CSharp…
阅读更多...
散点图，盒须图，折线图混放在一个echarts

散点图，盒须图，折线图混放在一个echarts

散点图&#xff0c;何须图&#xff0c;折线图混放在一个echarts option {tooltip: {trigger: axis,axisPointer: {type: cross,crossStyle: {color: #999}}},legend: {data:[盒须图1,盒须图2,折线图,散点图]},xAxis: [{type: category,data: [周一,周二,周三,周四,周五,周六…
阅读更多...
Win11 TensorRT环境部署

Win11 TensorRT环境部署

一、CUDA和CUDNN安装 cuda和cudnn网上有很多安装教程&#xff0c;这里列举了一些&#xff0c;就不详细说了&#xff0c;具体链接如下&#xff1a; csdn.net - CUDA安装教程&#xff08;超详细&#xff09; 原创 zhihu.com - 深度学习之CUDACUDNN详细安装教程 tencent.com - C…
阅读更多...
Sqoop安装与配置-shell脚本一键安装配置

Sqoop安装与配置-shell脚本一键安装配置

文章目录 前言一、使用shell脚本一键安装1. 复制脚本2. 增加执行权限3. 执行脚本4. 加载用户环境变量5. 查看是否安装成功 总结 前言 本文介绍了如何使用Shell脚本一键安装Sqoop。Sqoop是一个用于在Apache Hadoop和结构化数据存储&#xff08;如关系数据库&#xff09;之间传输…
阅读更多...
优先考虑泛型

优先考虑泛型

Java中的泛型&#xff08;Generics&#xff09;提供了一种参数化类型的机制&#xff0c;使得你可以编写更灵活、类型安全的代码。下面是一个例子&#xff0c;说明在Java中优先考虑泛型的好处&#xff1a; 考虑一个简单的容器类&#xff0c;它可以存储任意类型的元素&#xff0…
阅读更多...
【分享】WinRAR解压缩软件的超详细使用攻略

【分享】WinRAR解压缩软件的超详细使用攻略

WinRAR是一款常见的解压缩软件&#xff0c;它使用方便&#xff0c;界面友好&#xff0c;在压缩率和速度方面都有很好的表现。 除了解压缩功能&#xff0c;WinRAR还有很多好用的功能&#xff0c;今天小编就来分享一下&#xff0c;WinRAR几个常用功能的具体使用方法。 1. 解压缩…
阅读更多...
Axure之动态面板轮播图

Axure之动态面板轮播图

目录 一.介绍 二.好处 三.动态面板轮播图 四.动态面板多方式登录 五.ERP登录 六.ERP的左侧菜单栏 七.ERP的公告栏 今天就到这了哦&#xff01;&#xff01;&#xff01;希望能帮到你了哦&#xff01;&#xff01;&#xff01; 一.介绍 Axure中的动态面板是一个非常有用的组…
阅读更多...
可观测性是什么？新手入门指南！

可观测性是什么？新手入门指南！

如果您之前对可观测性重要性&#xff0c;益处&#xff0c;以及组成不甚了解&#xff0c;本文是一个合适的指南手册。 什么是可观测性&#xff1f; 可观测性被定义为根据系统产生的输出数据&#xff08;如日志&#xff0c;指标和链路追踪&#xff09;来衡量当前系统运行状态的…
阅读更多...
Python学习开发mock接口

Python学习开发mock接口

#1.测试为什么要开发接口&#xff1f; 1)在别的接口没有开发好的时候, mock接口(模拟接口) 2)查看数据, 避免直接操作数据库 #2.开发接口的顺序 1)安装flask flask是一个轻量级开发框架 pip install flask 2)开发一个接口 开发步骤&#xff1a; 1.实例化一个服务server:f…
阅读更多...
【MySQL学习之基础篇】多表查询

【MySQL学习之基础篇】多表查询

文章目录 1. 多表关系1.1. 一对多1.2. 多对多1.3. 一对一 2. 多表查询概述2.1. 数据准备2.2. 概述 3. 查询的分类3.1. 内连接查询3.2. 外连接查询3.3. 自连接3.3.1. 自连接查询3.3.2. 联合查询 3.4. 子查询3.4.1. 概述3.4.2. 标量子查询3.4.3. 列子查询3.4.4. 行子查询3.4.5. 表…
阅读更多...
gRPC框架

gRPC框架

1、gRPC 与 Protobuf 介绍 微服务架构中&#xff0c;由于每个服务对应的代码库是独立运行的&#xff0c;无法直接调用&#xff0c;彼此间 的通信就是个大问题gRPC 可以实现微服务&#xff0c; 将大的项目拆分为多个小且独立的业务模块&#xff0c; 也就是服务&#xff0c; 各服…
阅读更多...
[pluginviteimport-analysis] vite 提示jsx语法报错

[pluginviteimport-analysis] vite 提示jsx语法报错

参考文章 https://segmentfault.com/q/1010000043499356https://blog.csdn.net/kkkkkkgg/article/details/131168224 报错内容 内容类似如下&#xff1a; 03:16:26 [vite] Internal server error: Failed to parse source for import analysis because the content contains…
阅读更多...
C语言刷题数组------数组交换

C语言刷题数组------数组交换

输入一个长度为 10的整数数组 X[10]&#xff0c;将里面的非正整数全部替换为 1&#xff0c;输出替换完成后的数组。 输入格式 输入包含 10个整数&#xff0c;每个整数占一行。输出格式 输出新数组中的所有元素&#xff0c;每个元素占一行。输出格式为 X[i] x&#xff0c;其中…
阅读更多...
最新文章

Copyright @ 2022~2023