一、原理

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.

關(guān)鍵幀??到關(guān)鍵幀?, 觀測(cè)：

? ? ? ?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，認(rèn)為是 fixed 值，不參與優(yōu)化

3. overall cost

? ? ? ?

? ? ?對(duì)于 sequential edge, 直接使用最小二乘，對(duì)于 loop edge, 使用魯棒核函數(shù)，避免 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. 查找最新關(guān)鍵幀以及最老回環(huán)關(guān)鍵幀

cur_index: 最新加入的關(guān)鍵幀

first_looped_index: 最老的回環(huán)關(guān)鍵幀

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. 二維數(shù)組對(duì)應(yīng)優(yōu)化變量?jī)?nèi)存

// 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). 忽略早于最早回環(huán)關(guān)鍵幀的關(guān)鍵幀

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

(2). 用 vio pose 作為優(yōu)化初始值

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). 設(shè)置最老回環(huán)關(guān)鍵幀 const

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

(4). 加入 sequential edge

觀測(cè)：relative_t, relative_yaw

引入當(dāng)前幀和前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. 求解結(jié)果更新

(1). 用優(yōu)化的結(jié)果去更新參與優(yōu)化的節(jié)點(diǎn)的 drift-free pose

忽略不參與優(yōu)化的關(guān)鍵幀

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). 用當(dāng)前關(guān)鍵幀計(jì)算 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 去矯正未參與優(yōu)化的關(guān)鍵幀

對(duì)于 index > cur_index 的關(guān)鍵幀，更新 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;};

? 回環(huán)邊加入 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); ...... }

總結(jié)

以上是生活随笔為你收集整理的Vins-Mono 论文 Coding 一 7(3). pose_graph: 4DOF pose_graph的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。