一、原理

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.

關鍵幀??到關鍵幀?, 觀測：

? ? ? ?translation:?

? ? ? ?yaw:?

(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

? ? ?

? ? ?其中??是 vio 求出的 roll, pitch，認為是 fixed 值，不參與優化

3. overall cost

? ? ? ?

? ? ?對于 sequential edge, 直接使用最小二乘，對于 loop edge, 使用魯棒核函數，避免 fp 的影響

? ? ??Although the tightly coupled relocalization already helps with eliminating wrong loop closures, we add another Huber norm? ? 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的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。