Vins-Mono 论文 && Coding 一 7(1). pose_graph: pose_graph 流程

最新推荐文章于 2023-12-14 21:18:23 发布

scofield_ly

最新推荐文章于 2023-12-14 21:18:23 发布

阅读量312

点赞数

分类专栏： # vio 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40072161/article/details/115284232

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、概述

1. 作用

(1). 回环检测，将回环的关键帧加入滑窗优化

(2). 4dof pose-graph

2. 流程

1. 载入字典

posegraph.loadVocabulary(vocabulary_file);

2. 读取 pose-graph 数据

posegraph.loadPoseGraph();

3. subscriber, publisher 初始化

4. 开启两个线程

 measurement_process = std::thread(process);
 keyboard_command_process = std::thread(command);

二、Subscriber && Publisher

1. Subscriber

ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);

(1). "/vins_estimator/imu_propagate"

topic: imu 频率预测的 imu pose

callback: 可视化更新预测的 camera pose

......

Vector3d vio_t_cam;
Quaterniond vio_q_cam;
vio_t_cam = vio_t + vio_q * tic;
vio_q_cam = vio_q * qic;

cameraposevisual.reset();
cameraposevisual.add_pose(vio_t_cam, vio_q_cam);
cameraposevisual.publish_by(pub_camera_pose_visual, forward_msg->header);

(2). "/vins_estimator/odometry"

topic: window[WINDOW_SIZE](最新帧) 对应 pose

callback: 将 vio pose 变换至回环世界坐标系下，同时获得 camera pose，并压入 odomertry_buf

......
Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
Quaterniond vio_q;
vio_q.w() = pose_msg->pose.pose.orientation.w;
vio_q.x() = pose_msg->pose.pose.orientation.x;
vio_q.y() = pose_msg->pose.pose.orientation.y;
vio_q.z() = pose_msg->pose.pose.orientation.z;

// 变换至 回环参考世界坐标系
vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;
vio_q = posegraph.w_r_vio *  vio_q;

// 考虑 drift
vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;
vio_q = posegraph.r_drift * vio_q;

// 获得 camera pose
Vector3d vio_t_cam;
Quaterniond vio_q_cam;
vio_t_cam = vio_t + vio_q * tic;
vio_q_cam = vio_q * qic;        

// 压入 buffer
odometry_buf.push(vio_t_cam);
if (odometry_buf.size() > 10) {
    odometry_buf.pop();
}

(3). "/cam0/image_raw"

topic: sensor 原始图像

callback: image_buf.push(image_msg);

(4). "/vins_estimator/keyframe_pose"

topic: window[WINDOW_SIZE - 2](次新帧) 对应 pose （IMU 在 world 下的 pose）

callback: pose_buf.push(pose_msg);

(5). "/vins_estimator/extrinsic"

topic: imu-camera 外参，tic, ric

callback: 更新回环线程对应的外参 tic, qic

m_process.lock();
tic = Vector3d(pose_msg->pose.pose.position.x,
               pose_msg->pose.pose.position.y,
               pose_msg->pose.pose.position.z);
qic = Quaterniond(pose_msg->pose.pose.orientation.w,
                  pose_msg->pose.pose.orientation.x,
                  pose_msg->pose.pose.orientation.y,
                  pose_msg->pose.pose.orientation.z).toRotationMatrix();
m_process.unlock();

(6). "/vins_estimator/keyframe_point"

topic: 在 WINDOW_SIZE - 2(次新帧) 被观测的 featrues

callback: point_buf.push(point_msg);

(7). "/vins_estimator/relo_relative_pose"

topic：窗口内的回环关键帧重定位信息

callback: 将重定位优化结果更新于 pose-graph

Vector3d relative_t = Vector3d(pose_msg->pose.pose.position.x,
                               pose_msg->pose.pose.position.y,
                               pose_msg->pose.pose.position.z);
Quaterniond relative_q;
relative_q.w() = pose_msg->pose.pose.orientation.w;
relative_q.x() = pose_msg->pose.pose.orientation.x;
relative_q.y() = pose_msg->pose.pose.orientation.y;
relative_q.z() = pose_msg->pose.pose.orientation.z;
double relative_yaw = pose_msg->twist.twist.linear.x;
int index = pose_msg->twist.twist.linear.y;
//printf("receive index %d \n", index );

Eigen::Matrix<double, 8, 1 > loop_info;
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;
posegraph.updateKeyFrameLoop(index, loop_info);

2. Publisher

pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
pub_key_odometrys = n.advertise<visualization_msgs::Marker>("key_odometrys", 1000);
pub_vio_path = n.advertise<nav_msgs::Path>("no_loop_path", 1000);
pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);

三、坐标系相关

1. keyframe 的两种 pose

vio 中下 pose： $T_{wi_{\_vio}}$

回环优化的 pose： $T_{wi_{\_loop}}$

(1). vio 中的 pose

Eigen::Vector3d vio_T_w_i; 
Eigen::Matrix3d vio_R_w_i;

void KeyFrame::getVioPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i) {
    _T_w_i = vio_T_w_i;
    _R_w_i = vio_R_w_i;
}

void KeyFrame::updateVioPose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i) {
	vio_T_w_i = _T_w_i;
	vio_R_w_i = _R_w_i;
	T_w_i = vio_T_w_i;
	R_w_i = vio_R_w_i;
}

(2). 回环优化的 pose

Eigen::Vector3d T_w_i;
Eigen::Matrix3d R_w_i;

void KeyFrame::getPose(Eigen::Vector3d &_T_w_i, Eigen::Matrix3d &_R_w_i) {
    _T_w_i = T_w_i;
    _R_w_i = R_w_i;
}

void KeyFrame::updatePose(const Eigen::Vector3d &_T_w_i, const Eigen::Matrix3d &_R_w_i) {
    T_w_i = _T_w_i;
    R_w_i = _R_w_i;
}

2. 滑窗帧 , 数据库帧

滑窗中的当前帧， F_i

数据库中的历史参考帧， F_j

3. camera 坐标系, imu 坐标系，外参

$p^i = T_{ic}\cdot p^c$

$p^c = T_{ci}\cdot p^i$

四、process 线程

1. 同步 img, point, pose

从 image_buf，point_buf，pose_buf 中获取时间戳同步的 msg(时间戳完全一致)

// 更新 
sensor_msgs::ImageConstPtr image_msg = NULL;
sensor_msgs::PointCloudConstPtr point_msg = NULL;
nav_msgs::Odometry::ConstPtr pose_msg = NULL;

2. 关键帧判断

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

3. 构造关键帧

(1). 关键帧关联 vio 结果

// vio pose
Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
                      pose_msg->pose.pose.position.y,
                      pose_msg->pose.pose.position.z);
Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
                         pose_msg->pose.pose.orientation.x,
                         pose_msg->pose.pose.orientation.y,
                         pose_msg->pose.pose.orientation.z).toRotationMatrix();

vector<cv::Point3f> point_3d; 
vector<cv::Point2f> point_2d_uv; 
vector<cv::Point2f> point_2d_normal;
vector<double> point_id;

for (unsigned int i = 0; i < point_msg->points.size(); i++) {
    // 3d 坐标
    cv::Point3f p_3d;
    p_3d.x = point_msg->points[i].x;
    p_3d.y = point_msg->points[i].y;
    p_3d.z = point_msg->points[i].z;
    point_3d.push_back(p_3d);

    cv::Point2f p_2d_uv, p_2d_normal;
    double p_id;
    // 归一化像素坐标(坐标值: 0.0-1.0)
    p_2d_normal.x = point_msg->channels[i].values[0];
    p_2d_normal.y = point_msg->channels[i].values[1];
    // 像素坐标(原始像素坐标)
    p_2d_uv.x = point_msg->channels[i].values[2];
    p_2d_uv.y = point_msg->channels[i].values[3];
    // feature id
    p_id = point_msg->channels[i].values[4];
    point_2d_normal.push_back(p_2d_normal);
    point_2d_uv.push_back(p_2d_uv);
    point_id.push_back(p_id);
}

KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image, point_3d, point_2d_uv, point_2d_normal, point_id, sequence);   
posegraph.addKeyFrame(keyframe, 1);

(2). 构造关键帧

// create keyframe online
KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,
		           vector<cv::Point3f> &_point_3d, vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_norm,
		           vector<double> &_point_id, int _sequence)
{
    time_stamp = _time_stamp;
    // index 是回环构造的关键帧索引，自然增长
	index = _index;  
    // 用 vio pose 初始化
	vio_T_w_i = _vio_T_w_i;
	vio_R_w_i = _vio_R_w_i;
	T_w_i = vio_T_w_i;
	R_w_i = vio_R_w_i;
	origin_vio_T = vio_T_w_i;		
	origin_vio_R = vio_R_w_i;
	image = _image.clone();
	cv::resize(image, thumbnail, cv::Size(80, 60));
	point_3d = _point_3d;
	point_2d_uv = _point_2d_uv;
	point_2d_norm = _point_2d_norm;
	point_id = _point_id;
	has_loop = false;
	loop_index = -1;
	has_fast_point = false;
	loop_info << 0, 0, 0, 0, 0, 0, 0, 0;
	sequence = _sequence;
    // 复用 vio 提取的特征点，计算对应的feature brief descriptor
    // 更新 window_keypoints，window_brief_descriptors
	computeWindowBRIEFPoint();
    // 对整幅图像重新提取特征点，并计算 feature brief descriptor
    // 更新 keypoints，keypoints_norm，brief_descriptor
	computeBRIEFPoint();
	......
}

4. 处理关键帧

m_process.lock();
......
posegraph.addKeyFrame(keyframe, 1);
m_process.unlock();
......

Vins-Mono 论文 && Coding 一 7(2). pose_graph: 回环检测 && 重定位

Vins-Mono 论文 && Coding 一 7(3). pose_graph: 4DOF pose_graph

scofield_ly

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
Vins-Mono 论文 && Coding 一 7(1). pose_graph: pose_graph 流程

ttt
复制链接

扫一扫

专栏目录