一、原理

1. constraint edges

(1). sequential edge

A keyframe establishes several sequential edges to its previous keyframes. A sequential edge represents the relative transformation between two keyframes, which is taken directly from VIO.

关键帧 $j$ 到关键帧 $i$ , 观测：

translation: $\hat{p}^i_{ij}=\hat{R}^{w-1}_i(\hat{p}^w_j-\hat{p}^w_i)$

yaw: $\hat{\psi }_{ij}=\hat{\psi }_j -\hat{\psi }_i$

(2). Loop edge

the loop-closure edge only contains a 4-DOF relative pose transform that is defined the same as sequentail edge. The value of the loop-closure edge is obtained using results from relocalization

2. edge residual

$r_{i,j}(p^w_i, \psi _i, p^w_j, \psi _j)=\bigl(\begin{smallmatrix} R(\hat{\phi }_i, \hat{\theta }_i, \hat{\psi }_i)^{-1}(p^w_j-p^w_i) - \hat{p}^i_{ij}\\ \psi _j - \psi _i - \hat{\psi}_{ij} \end{smallmatrix}\bigr)$

其中 $\hat{\phi }_i, \hat{\theta }_i$ 是 vio 求出的 roll, pitch，认为是 fixed 值，不参与优化

3. overall cost

$\min_{p, \psi}(\sum_{(i,j)\in S}||r_{i,j}||^2 + \sum_{(i,j)\in L }\rho (||r_{i,j}||^2))$

对于 sequential edge, 直接使用最小二乘，对于 loop edge, 使用鲁棒核函数，避免 fp 的影响

Although the tightly coupled relocalization already helps with eliminating wrong loop closures, we add another Huber norm $\rho (\cdot )$ to further reduce the impact of any possible wrong loops. In contrast, we do not use any robust norms for sequential edges, as these edges are extracted from VIO, which already contain sufficient outlier rejection mechanisms

二、流程

PoseGraph::optimize4DoF()

1. 查找最新关键帧以及最老回环关键帧

cur_index: 最新加入的关键帧

first_looped_index: 最老的回环关键帧

int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty()) {cur_index = optimize_buf.front();first_looped_index = earliest_loop_index;optimize_buf.pop();
}
m_optimize_buf.unlock();

2. 二维数组对应优化变量内存

// w^t_i   w^q_i
double t_array[max_length][3];
Quaterniond q_array[max_length];
double euler_array[max_length][3];  // ypr
double sequence_array[max_length];

3. 添加 factors

(1). 忽略早于最早回环关键帧的关键帧

if ((*it)->index < first_looped_index)continue;
(*it)->local_index = i;

(2). 用 vio pose 作为优化初始值

Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
// translation
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
// quaternion
q_array[i] = tmp_q;
// ypr
Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
......
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);

(3). 设置最老回环关键帧 const

if ((*it)->index == first_looped_index || (*it)->sequence == 0) {          problem.SetParameterBlockConstant(euler_array[i]);problem.SetParameterBlockConstant(t_array[i]);
}

(4). 加入 sequential edge

观测：relative_t, relative_yaw

引入当前帧和前5帧的 delta odom 约束

for (int j = 1; j < 5; j++)  {if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])  {Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());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]);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]);}
}

(5). 加入 loop edge

引入 robust kernel function

if((*it)->has_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();double relative_yaw = (*it)->getLoopRelativeYaw();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]);
}

4. ceres 求解

ceres::Solve(options, &problem, &summary);

5. 求解结果更新

(1). 用优化的结果去更新参与优化的节点的 drift-free pose

忽略不参与优化的关键帧

i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++) {if ((*it)->index < first_looped_index)continue;Quaterniond tmp_q;tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);Matrix3d tmp_r = tmp_q.toRotationMatrix();(*it)-> updatePose(tmp_t, tmp_r);if ((*it)->index == cur_index)break;i++;
}

(2). 用当前关键帧计算 drift

xyz-yaw

 // cur_x: drift-free pose; vio_x: vio pose
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();

(3). 用 drift 去矫正未参与优化的关键帧

对于 index > cur_index 的关键帧，更新 drift-free 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);
}

三、pose-graph factor

pose_graph/src/pose_graph.h

1. local parameterization

class AngleLocalParameterization {public:template <typename T>bool operator()(const T* theta_radians, const T* delta_theta_radians,T* theta_radians_plus_delta) const {*theta_radians_plus_delta =NormalizeAngle(*theta_radians + *delta_theta_radians);  // 角度范围限制return true;}static ceres::LocalParameterization* Create() {return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,1, 1>);}
};

2. auto-diff factor

struct FourDOFError
{FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i):t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}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];// euler to rotationT w_R_i[9];YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);// rotation transposeT i_R_w[9];RotationMatrixTranspose(w_R_i, i_R_w);// rotation matrix rotate pointT t_i_ij[3];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;}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) {return (new ceres::AutoDiffCostFunction<FourDOFError, 4, 1, 3, 1, 3>(new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));}double t_x, t_y, t_z;double relative_yaw, pitch_i, roll_i;};

回环边加入 weight

struct FourDOFWeightError  {
......residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);
......
}

Vins-Mono 论文 Coding 一 7(3). pose_graph: 4DOF pose_graph相关推荐

m2dgr数据集在vins mono运行
m2dgr数据集在vins mono运行 1.代码的修改 1.1 yaml文件的修改 1.2修改VINS-mono轨迹保存代码 1.3修改launch文件 2 EVO显示轨迹 2.1运行比较结果参考 ...
SLAM之小觅相机跑开源方案（ORB_SLAM2，VINS MONO，VINS FUSION，RTAB-Map）
传感器: 小觅相机标准版开源SLAM方案: ORB_SLAM2,VINS MONO,VINS FUSION,RTAB-Map 测试地点: 室内大厅(光线不均)/ 露天阳台实现形式: 小觅相机 / ...
的环境下 qt 运行在_Ubuntu16.04环境下运行vins mono（环境配置及编译）之ROS kinetic的安装...
所需环境:ubuntu16.04+ROS kinetic+opencv 3.3.1+eigen3.3.3+ceres solver 1.14 1.ROS Kinetic 的安装 (1)设置source ...
VINS Fusion GPS融合部分
概述 VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 局部传感器(如相机,IMU,激光雷达 ...
VINS_FUSION
VINS_FUSION意义 VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖.此外,全局信息可 ...
【今日CV 计算机视觉论文速览第111期】Fri, 3 May 2019
今日CS.CV 计算机视觉论文速览 Fri, 3 May 2019 Totally 29 papers ?上期速览✈更多精彩请移步主页 Interesting: ?****Single Image P ...
【优秀论文解读】UV-SLAM: Unconstrained Line-based SLAM Using Vanishing Points for Structural Mapping
论文简介提出了一种UV-SLAM的算法,整体建立与VINS-MONO的基础上: 在VINS-MONO的基础上增加了线特征的约束和消影点的约束. 其中线特征的提取用的是line segment det ...
论文剽窃者“自爆家门”？CVPR 最后一天上演“一出好戏 ”！
6月24日,是CVPR 2022 最后一天了,全世界计算机视觉领域的大佬们齐聚一堂,翱翔在知识的海洋.一片祥和之下,突然发生了一件可以惊呼"厚礼蟹"的大事:有人举报CVPR Ora ...
ORBSLAM3论文翻译
ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM 摘要本文介绍了OR ...
[论文笔记|sonar+VIO]SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor
目录 Ⅰ. 摘要 Ⅱ. INTRODUCTION Ⅲ. PROPOSED METHOD A. Notations and States B. Tightly-coupled Non-Linear Op ...

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