VINS-MONO——Pose_Graph重定位和位姿图优化

VINS-MONO系列代码解读


VINS-MONO——Pose_Graph重定位和位姿图优化

  • VINS-MONO系列代码解读
  • 前言——无功利做事,努力达到“心流”状态
  • 一、重定位
    • 1、数据对齐,找到同一帧pose、point、image数据
    • 2、判断是否为关键帧
    • 3、创建关键帧并计算描述子
      • (1)computeWindowBRIEFPoint()计算窗口内已经存在的关键点描述子
      • (2)computeBRIEFPoint()
    • 4、回环检测
    • 5、PNP 求回环相对位姿Tbi<-bj
  • 二、4DoF位姿优化——两种类型
    • 1:Sequential Edge
    • 2:loop_edge
    • 代码实现
      • *非闭环关键帧*(直接复制过来的,空格没删,谅解)
      • 闭环关键帧构造边
  • 参考

前言——无功利做事,努力达到“心流”状态

重定位和4自由度位姿优化在pose_graph_node节点同时双线程并行,它和后端优化的关系可以参见崔神的注解1。上图中,蓝线为正常的闭环优化流程,即通过后端的非线性优化来更新滑窗内所有相机的位姿。紫线为闭环检测模块, 当后端优化完成后,会将滑窗内的次新帧进行闭环检测,即首先提取新角点并进行描述,然后与数据库进行检索,寻找闭环帧,并将该帧添加到数据库中。红线为快速重定位模块,当检测到闭环帧后,会将闭环约束添加到后端的整体目标函数中进行非线性优化,得到第 i 帧(注意这里的帧为闭环帧中的老帧)经过滑窗优化后的位姿,从而计算出累积的偏移误差,进而对滑窗内的位姿进行修正。绿线为发布闭环优化后消除累积误差的帧Tw2<-bj,更新滑窗的Tw2<-bj。
VINS-MONO——Pose_Graph重定位和位姿图优化_第1张图片

提示:以下是本篇文章正文内容

一、重定位

1、数据对齐,找到同一帧pose、point、image数据

VINS-MONO——Pose_Graph重定位和位姿图优化_第2张图片
时间顺序上 img.heder.stamp = point.heder.stamp >= pose.header.stamp,原因在于IMU的频率大于图像采集频率,在IMU预积分时(在estimator节点进行),采用线性插值的方法使得在如图所示的WINDOW_SIZE - 2 处,IMU位姿刚好和图像的位姿一样,但是最后积分出来的时间戳还是用的在WINDOW_SIZE - 2 内的最后一帧IMU时间戳,这就导致位姿时间戳小于等于图像时间戳。point的时间戳和image一样,因为point是从image内提取出来的,赋值也是直接用的image时间戳。

2、判断是否为关键帧

判断和上一帧位姿之差大于0即可

if((T - last_t).norm() > SKIP_DIS)

3、创建关键帧并计算描述子

(1)computeWindowBRIEFPoint()计算窗口内已经存在的关键点描述子

void KeyFrame::computeWindowBRIEFPoint()
{
	BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
	for(int i = 0; i < (int)point_2d_uv.size(); i++)
	{
	    cv::KeyPoint key;
	    key.pt = point_2d_uv[i];
	    window_keypoints.push_back(key);
	}
	extractor(image, window_keypoints, window_brief_descriptors);
}

(2)computeBRIEFPoint()

临时生成500个Harris或者FAST关键点,需要说明的是,作者论文中说的是500个Harris关键点,但是代码中却用的是FAST关键点,而且没有指定数量。生成关键点仍然计算描述子。

//额外检测新特征点并计算所有特征点的描述子,为了回环检测
void KeyFrame::computeBRIEFPoint()
{
	BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
	const int fast_th = 20; // corner detector response threshold
	//TODO 作者论文中提到,检测500个Harris角点作为特征,而这里却是FAST特征,并且没有规定数量,有出入,尝试注释掉看看效果
	if(1)
		cv::FAST(image, keypoints, fast_th, true);
	else
	{
		vector<cv::Point2f> tmp_pts;
		//检测500个新的特征点并将其放入keypoints
		cv::goodFeaturesToTrack(image, tmp_pts, 500, 0.01, 10);
		for(int i = 0; i < (int)tmp_pts.size(); i++)
		{
		    cv::KeyPoint key;
		    key.pt = tmp_pts[i];
		    keypoints.push_back(key);
		}
	}
	//计算keypoints中所有特征点的描述子
	extractor(image, keypoints, brief_descriptors);

.

4、回环检测

主要的功能就是这个语句完成,利用brief描述子索引是否有评分和当前帧描述子最大的,这在ORB_SLAM2已经见识过了,也不多讲,只要注意一点,赋值给loop_index是最老帧的id。其他的处理图像拼接的函数这里都没有用上,检测到回环确实会拼接两个回环帧,但不是在这里,是在下一步findconnection()函数执行的

//查询字典数据库,得到与每一帧的相似度评分ret
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);

5、PNP 求回环相对位姿Tbi<-bj

描述子匹配,窗口中所有特征点的描述子与回环帧的所有描述子匹配,通过计算Hammings距离,阈值大于80的认为描述子匹配成功,若匹配上,返回在老帧中的3D、2D坐标。随后利用reduceVector()剔除没有匹配上的新老帧中的2D、3D点和ID。

//描述子匹配,窗口中所有特征点的描述子与回环帧的所有描述子匹配,通过计算Hammings距离,阈值大于80的认为描述子匹配成功,若匹配上,返回在老帧中的3D 2D坐标,为下面的PNP求解做准备
void KeyFrame::searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old, std::vector<cv::Point2f> &matched_2d_old_norm,
                                std::vector<uchar> &status, const std::vector<BRIEF::bitset> &descriptors_old,
                                const std::vector<cv::KeyPoint> &keypoints_old, const std::vector<cv::KeyPoint> &keypoints_old_norm)
{
    for(int i = 0; i < (int)window_brief_descriptors.size(); i++)
    {
        cv::Point2f pt(0.f, 0.f);
        cv::Point2f pt_norm(0.f, 0.f);
        if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm))
          status.push_back(1);
        else
        status.push_back(0);
        matched_2d_old.push_back(pt);
        matched_2d_old_norm.push_back(pt_norm);
    }
}

用PnPRANSAC()计算回环帧和滑窗参考坐标洗的变换矩阵Tw2<-bi,同时取出误匹配点。

PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);//TODO PNP_T返回imu到世界坐标的变换Tw2<-bi

经过RANSAC剔除误匹配点仍然具有足够的匹配点,说明真的构成回环,发布出来看看(也就是第4步说的在这拼接回环帧图像)

if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
	cv::Mat thumbimage;
	cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2));
	sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage).toImageMsg();
	msg->header.stamp = ros::Time(time_stamp);
	pub_match_img.publish(msg);
}

随后计算回环帧之间的相对误差Tbi<-bj ,并跟新到loop_info里去

//若达到最小回环匹配点数
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
		//Tw2<-bi.inverse() * Tw2<-bj = Tbi<-bj
	    relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
	    relative_q = PnP_R_old.transpose() * origin_vio_R;
	    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;
	    	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;
}

最后,是一段相当绕的关于累计误差Tw1<-w2求解,并且利用该累计误差更新关键帧序列中的位姿,一定要注意区别:t_drift, r_drift是相对位姿(两回环之间的位姿Tbi<-bj),w_r_vio, w_t_vio是累计位姿误差Tw1<-w2,具体的说明已经注释到代码里了,看着代码会更加清晰。

if (loop_index != -1)
	{
        //获取回环候选帧
        KeyFrame* old_kf = getKeyFrame(loop_index);

        //当前帧与回环候选帧进行描述子匹配
        if (cur_kf->findConnection(old_kf))//PNP求相对位姿,RANSAC剔除误匹配点,计算累积误差和相对误差
        {
            //earliest_loop_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); //w_P_cur、w_R_cur: Tw1<-bi
            cur_kf->getVioPose(vio_P_cur, vio_R_cur); //vio_P_cur、vio_R_cur: Tw2<-bj

            //获取当前帧与回环帧的相对位姿relative_q、relative_t
            Vector3d relative_t;
            Quaterniond relative_q;
            relative_t = cur_kf->getLoopRelativeT(); //得到Tbi<-bj
            relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();

            //重新计算当前帧位姿w_P_cur、w_R_cur
            w_P_cur = w_R_old * relative_t + w_P_old; //Tw1<-bi * Tbi<-bj = Tw1<-bj 对应崔神图23
            w_R_cur = w_R_old * relative_q;
            
            //回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t
            double shift_yaw;
            Matrix3d shift_r;
            Vector3d shift_t; 
            shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); //偏差
            //只更新yaw轴的偏差,其他pitch、roll仍保持初始值,因为只有yaw是不可观测的,只要优化它就可以
            //w_R_cur*vio_R_cur.transpose() = Tw1<-bj*Tw2<-bj.transpose() = Tw1<-w2;
            shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
            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
            //将所有图像序列都合并到世界坐标系下
            if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
            {
                //Tw1<-w2
                w_r_vio = shift_r;
                w_t_vio = shift_t;
                //Tw1-
                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++)   
                {
                    if((*it)->sequence == cur_kf->sequence)
                    {
                        Vector3d vio_P_cur;
                        Matrix3d vio_R_cur;
                        (*it)->getVioPose(vio_P_cur, vio_R_cur); //Tw2<-bj
                        //变换成Tw1<-bj
                        vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                        vio_R_cur = w_r_vio *  vio_R_cur;
                        //TODO 上面将Tw1<-bj跟新过了呀!! 解:一个是当前帧,一个是关键帧序列,不一样的
                        (*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();
        }
	}

至此,重定位流程就结束了,相较于ORB_SLAM2用到个中匹配、旋转不变性等方法,VINS-MONO中的回环检测已经十分简单了,原因在于VINS-MONO有两个角度pitch and roll是可观的并不用构建复旋转不变直方图,而且ORB_SLAM2采用的是共视图法,每次比较特征点都要考虑到共视帧中的特征点,这样一延伸看起来代码量就大了,而VINS采用滑动窗口法,只要窗口内的地图和当前帧比较接近都拿来比较,容易选取,代码量就小了。第三点VINS构建BRIEF描述子是通过第三方库实现回环关键帧打分,ORB还自己算了BRIEF描述子。

二、4DoF位姿优化——两种类型

VINS-MONO——Pose_Graph重定位和位姿图优化_第3张图片
四自由度位姿图优化不考虑landmarks、bias等其他变量,只考虑相机的位姿(更准确的说是imu的位姿),并且由于pitch和roll为可观量,只需优化yaw和平移四个自由度即可,其主要有两种策略:

  1. 非闭环关键帧和其他关键帧形成边
  2. 另一是闭环关键帧和最早的闭环关键帧形成边(多次经过同一地点,会检测到多个和当前帧形成闭环的关键帧)。

这里需要区分当中用到的local_indexlocal_index表示将大于最早闭环(first_looped_index)提出来并依次用i对其复制,与此同时将大于最早闭环帧的所有帧位姿取出。为什么一定要大于最早闭环帧呢?因为在最早闭环帧之前的帧都看做不动,不做优化。所以要注意local_index不是从keyframelist.begin()开始赋值。]

1:Sequential Edge

所有关键帧序列的每一个关键帧和其前4个关键帧形成一条边,也就对应着以j为变量的4次循环啦!这里还有个判断语句if,第一个条件告诉我们不要选择local_index等于0的关键帧(也就是最早关键帧),第二个条件表示是要在同一个序列的关键帧。

2:loop_edge

这里只要理解

int connected_index = getKeyFrame((*it)->loop_index)->local_index;

到底得到的是哪一帧的local_index就豁然开朗了,loop_index在addKeyFrame函数存入的就是最老帧的id,那么local_index就是对应着最老帧(图中的绿色块0),所以这里就是闭环帧之间形成的边!

接下来都是一些位姿的更新,需要说明一下,由于执行过4DoF位姿优化,关键帧序列的位姿都发生了变化,累积误差r_drift, t_drift是当前帧和最老帧之间产生的,就需要更新,并且用该累积误差对最老帧到最新帧之间所有帧位姿做误差修正(相当于做个缝合工作)。

另外,要注意这边有两个函数容易混淆,getPose() 和 getVioPose()。在执行完4Dof位姿优化会更新各个帧的位姿,getPose()就是得到那个更新后的位姿,而getVioPose()是没有更新过的位姿,他们两个取的变量也是不同的(分别是T_w_i, vio_T_w_i)。

最后,执行完一次4DOF优化,还要暂停2秒,给重定位留时间,而且闭环很久才会检测到一次,也不用一直工作,不然就是在做while(1)死循环,不执行任何东西。

代码实现

首先看看残差4自由度残差方程,估计值 - 观测值 = 残差
在这里插入图片描述
optimize4DOF在PoseGraph()初始化就中打开线程,而观测值在传入CostFunction之前就已经构造好了,也就是relative_t 和 relative_yaw

非闭环关键帧(直接复制过来的,空格没删,谅解)

				for (int j = 1; j < 5; j++)
                {
                    /*
                     * i-j >= 0 : 该帧不能是最早的闭环关键帧, 取当前帧和比当前帧小j帧的进行4自由度位姿优化
                     * sequence_array[i] == sequence_array[i-j] : 数据集序列要是一样的
                     * */
                  if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
                  {
                    Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
                      //Pw<-ij 世界坐标系下的Pi - Pj
                    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]);
                      //Pbi<-ij = Rw<-bi.inverse() * pw<-ij ,转换到bi坐标系下  对应作者论文式28 带尖号的Pi<-ij测量值
                    relative_t = q_array[i-j].inverse() * relative_t;//
                    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]); //这里调用了FourDOFError的operator()重载括号运算符
                  }
                }

闭环关键帧构造边

				 //add loop edge
                //当前可能有好几帧闭环关键帧,和最早的进行4自由度位姿优化
                if((*it)->has_loop)
                {
                    assert((*it)->loop_index >= first_looped_index); //为真不终止
                    //loop_index为最早的闭环id, connected_inedx为0
                    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();
                    double relative_yaw = (*it)->getLoopRelativeYaw(); //yaw
                    ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                                                               relative_yaw, euler_conncected.y(), euler_conncected.z());
                    problem.AddResidualBlock(cost_function, loss_function,
                                             euler_array[connected_index],
                                             t_array[connected_index],
                                             euler_array[i],
                                             t_array[i]);
                    
                }

loop_edge求relative_t比较简单,之前重定位更新过一次相对误差,那个就是闭环关键帧之间的relative_t,接下来让我们进入FourDOFWeightError内的重载括号运算符看看这个残差是如何求出来的(雅可比矩阵没有求解析解,直接用ceres自动求导的方法):

template <typename T>
	bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
	{
		T t_w_ij[3];
		t_w_ij[0] = tj[0] - ti[0];
		t_w_ij[1] = tj[1] - ti[1];
		t_w_ij[2] = tj[2] - ti[2];

		//式28的估计值 - 测量值 = residuals
		// euler to rotation
		T w_R_i[9]; //Rw<-bi
		YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
		// rotation transpose
		T i_R_w[9];//Rw<-bi.inverse()
		RotationMatrixTranspose(w_R_i, i_R_w);
		// rotation matrix rotate point
		T t_i_ij[3];  //Rw<-bi.invesr() * (Pj-Pi);
		RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);

		residuals[0] = (t_i_ij[0] - T(t_x));
		residuals[1] = (t_i_ij[1] - T(t_y));
		residuals[2] = (t_i_ij[2] - T(t_z));
		residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));

		return true;
	}

参考


  1. https://github.com/StevenCui/VIO-Doc ↩︎

你可能感兴趣的:(VINS-Mono,slam,c++)