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

最新推荐文章于 2024-01-15 00:15:01 发布

scofield_ly

最新推荐文章于 2024-01-15 00:15:01 发布

阅读量234

点赞数

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

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

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、原理

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 rotation
		T w_R_i[9];
		YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
		// rotation transpose
		T i_R_w[9];
		RotationMatrixTranspose(w_R_i, i_R_w);
		// rotation matrix rotate point
		T 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);
......
}

scofield_ly

关注

0
点赞
踩
1

收藏

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

3. 4DOF pose_graph 详解(1). 原理<1>. sequentail edgeA 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.关键帧.
复制链接

扫一扫