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

news2024/12/25 13:21:57

开始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的整个框架解读也就完成了。

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

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

开关电源这些纹波噪声如何抑制？

开关电源这些纹波噪声如何抑制？

电源工程师都知道&#xff0c;开关电源在运行过程中会产生多种纹波噪声&#xff0c;对电路性能和稳定性产生一定影响&#xff0c;所以要针对各种纹波噪声采取合理的措施来解决&#xff0c;那么如何做&#xff1f; 1、低频纹波噪声主要是由于开关管开关状态的快速变化导致的。在…
阅读更多...
C++异步网络库workflow系列教程(3)Series串联任务流

C++异步网络库workflow系列教程(3)Series串联任务流

往期教程 如果觉得写的可以,请给一个点赞关注支持一下 观看之前请先看,往期的两篇博客教程,否则这篇博客没办法看懂 workFlow c异步网络库编译教程与简介 C异步网络库workflow入门教程(1)HTTP任务 C异步网络库workflow系列教程(2)redis任务 简介 首先,workflow是任务流的意…
阅读更多...
图解python | 字典

图解python | 字典

1.Python字典(Dictionary) 字典是另一种可变容器模型&#xff0c;且可存储任意类型对象。 字典的每个键值 key>value 对用冒号 : 分割&#xff0c;每个键值对之间用逗号 , 分割&#xff0c;整个字典包括在花括号 {} 中 ,格式如下所示&#xff1a; d {key1 : value1, key…
阅读更多...
软件测试职业规划

软件测试职业规划

软件测试人员的发展误区【4】 公司开发的产品专业性较强&#xff0c;软件测试人员需要有很强的专业知识&#xff0c;现在软件测试人员发展出现了一种测试管理者不愿意看到的景象&#xff1a; 1、开发技术较强的软件测试人员转向了软件开发(非测试工具开发)&#xff1b; 2、业务…
阅读更多...
题目:区间或 (蓝桥OJ 3691)

题目:区间或 (蓝桥OJ 3691)

题目描述: 解题思路: 本题采用位运算.先求出全部数组每一位各自的前缀和,然后再判断区间内每一位区间和是否为0,不为0则乘上相应的2^n并将各个为的2^n相加,得ans. 实现原理图 题解: #include<bits/stdc.h> using namespace std;const int N 1e5 9;int a[N], prefix[35…
阅读更多...
2023AI Agent智能体HR商用落地的案例汇集

2023AI Agent智能体HR商用落地的案例汇集

过去一周在各类智能体产品不断呈现新发展态势的情况下&#xff0c;我们将注意力继续放回AI Agent智能体在大型和超大型企业不同领域商用落地的可能性探索上面去。 本着这一初衷&#xff0c;我们会继续把注意力转向探索AI Agent智能体在HR领域的商用落地所面临的挑战和最可能实…
阅读更多...
ida脚本环境开发配置idapythonidacpp三端环境(win,mac,linux)

ida脚本环境开发配置idapythonidacpp三端环境(win,mac,linux)

ida脚本也有一段时间了,一直有个痛点是找不到比较好的方法热重载脚本来实时改动生效,导致开发效率老慢了。固总结下比较友好的环境搭配 使用ida热加载插件让你开发脚本更高效 github地址: GitHub - 0xeb/ida-qscripts: An IDA plugin to increase productivity when developi…
阅读更多...
PyTorch: 基于【VGG16】处理MNIST数据集的图像分类任务【准确率98.9%+】

PyTorch: 基于【VGG16】处理MNIST数据集的图像分类任务【准确率98.9%+】

目录 引言在Conda虚拟环境下安装pytorch步骤一&#xff1a;利用代码自动下载mnist数据集步骤二&#xff1a;搭建基于VGG16的图像分类模型步骤三&#xff1a;训练模型步骤四&#xff1a;测试模型运行结果后续模型的优化和改进建议完整代码结束语 引言 在本博客中&#xff0c;小…
阅读更多...
MySQL数据库卸载-Windows

MySQL数据库卸载-Windows

目录 1. 停止MySQL服务 2. 卸载MySQL相关组件 3. 删除MySQL安装目录 4. 删除MySQL数据目录 5. 再次打开服务&#xff0c;查看是否有MySQL卸载残留 1. 停止MySQL服务 winR 打开运行&#xff0c;输入 services.msc 点击 "确定" 调出系统服务。 2. 卸载MySQL相关组…
阅读更多...
国标级联/流媒体音视频平台EasyCVR设备录像下载异常该如何解决？

国标级联/流媒体音视频平台EasyCVR设备录像下载异常该如何解决？

视频监控TSINGSEE青犀视频平台EasyCVR能在复杂的网络环境中&#xff0c;将分散的各类视频资源进行统一汇聚、整合、集中管理&#xff0c;在视频监控播放上&#xff0c;视频安防监控汇聚平台可支持1、4、9、16个画面窗口播放&#xff0c;可同时播放多路视频流&#xff0c;也能支…
阅读更多...
一、win10+yolov8+anaconda环境部署

一、win10+yolov8+anaconda环境部署

1、安装anaconda &#xff08;1&#xff09;打开aonconda下载地址&#xff1a;https://www.anaconda.com/download&#xff0c;点击download下载。 2、下载完成后&#xff0c;双击打开&#xff0c;点击Next&#xff0c;I Agree&#xff0c;选择just me&#xff1b; 3、勾选…
阅读更多...
SQL进阶理论篇（五）：什么是Hash索引

SQL进阶理论篇（五）：什么是Hash索引

文章目录 简介MySQL中的Hash索引与B树的区别总结参考文献 简介 hash&#xff0c;即哈希&#xff0c;也被称为是散列函数。 Hash在数据库中的应用&#xff0c;可以帮助我们大幅度提升检索数据的效率。 大名鼎鼎的MD5其实就是Hash函数的一种变体。 Hash算法&#xff0c;是通过…
阅读更多...
ArkTS编译时遇到arkts-no-obj-literals-as-types错误【Bug已解决-鸿蒙】

ArkTS编译时遇到arkts-no-obj-literals-as-types错误【Bug已解决-鸿蒙】

文章目录 项目场景：问题描述原因分析：解决方案：解决方案1解决方案2此Bug解决方案总结项目场景： 在开发鸿蒙项目过程中，遇到了arkts-no-obj-literals-as-types，总结了自己和网上人的解决方案，故写下这篇文章。 遇到问题： rkTS编译时遇到arkts-no-obj-literals-as-type…
阅读更多...
操作系统中的作业管理

操作系统中的作业管理

从用户的角度看&#xff0c;作业是系统为完成一个用户的计算任务&#xff08;或一次事务处理&#xff09;所做的工作总和。例如&#xff0c;对于用户编制的源程序&#xff0c;需经过对源程序的编译、连接编辑或连接装入及运行产生计算结果。这其中的每一个步骤&#xff0c;常称…
阅读更多...
解锁知识的新大门：自建知识付费小程序的技术指南

解锁知识的新大门：自建知识付费小程序的技术指南

在数字化时代&#xff0c;知识付费小程序的崛起为创作者和学习者提供了全新的学习和分享方式。本文将以“知识付费小程序源码”为关键词&#xff0c;从技术角度出发&#xff0c;为你展示如何搭建一个独具特色的知识付费平台。 步骤1&#xff1a;选择适用的知识付费小程序源码…
阅读更多...
知识库SEO：提升网站内容质量与搜索引擎排名的策略

知识库SEO：提升网站内容质量与搜索引擎排名的策略

随着搜索引擎算法的不断更新和优化&#xff0c;单纯依靠关键词堆砌和外部链接的时代已经过去。现在的SEO&#xff08;搜索引擎优化&#xff09;已经转向了以提供高质量、有价值内容为核心的阶段。知识库SEO便是这个新阶段的重要策略之一。 | 一、知识库SEO的概念与意义 1.定义…
阅读更多...
《儿童绘本》期刊杂志发表论文投稿

《儿童绘本》期刊杂志发表论文投稿

《儿童绘本》杂志是由国家新闻出版管理部门批准&#xff0c;由吉林省舆林报刊发展有限责任公司主管主办&#xff0c;国内外公开发行的全国优秀期刊。办刊宗旨&#xff1a;以“普及绘本知识、推动儿童阅读”为理念&#xff0c;带动家庭亲子阅读&#xff0c;推动阅读教育及图画书…
阅读更多...
一文解析数据结构是如何装入 CPU 寄存器的？

一文解析数据结构是如何装入 CPU 寄存器的？

我们在之前很多文章的讲解中涉及了CPU与寄存器&#xff0c;然后有同学问了这样一个问题&#xff1a;既然CPU内部的寄存器数量有限&#xff0c;容量有限&#xff0c;那么我们使用的庞大的数据结构是怎样装入寄存器供CPU计算的呢&#xff1f;这篇文章就为你讲解一下这个问题。 内…
阅读更多...
交叉销售与场景业务销售运营

交叉销售与场景业务销售运营

交叉销售 交叉销售的定义 交叉销售是一种从横向角度开发产品市场的方式,是营销人员在完成本职工作以后,主动积极的向现有客户、市场等销售其他的、额外的产品或服务。 交叉销售的类型 补充销售 搭配销售个性化推荐奖励推荐 捆绑销售 交叉销售的意义 通过增加客户的转移成本…
阅读更多...
VMP泄露编译的一些注意事项

VMP泄露编译的一些注意事项

VMP编译教程 鉴于VMP已经在GitHub上被大佬强制开源&#xff0c;特此出一期编译教程。各位熟悉的可以略过&#xff0c;不熟悉的可以参考一下。 环境&#xff08;软件&#xff09; Visual Studio 2015 - 2022 &#xff08;建议使用VS2019&#xff0c;Qt插件只有这个版本及以上…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023