SLAM中的图优化的边的构造
在SLAM问题中，使用图优化的方式可以直观的建立与运动观测问题契合的优化模型，相比于滤波的方式，不要求满足马尔科夫性，于是可以将多帧的信息融合优化。
个人在阅读SLAM领域优秀开源代码ORB_SLAM的时候，对于核心部分的Optimize.cpp 中使用的一些Vertex与Edge的定义，还有一些不明白，这里记录一下推导的过程。
G2O 自带类型
types_six_dof_expmap.h 以及 types_seven_dof_expmap.h 中给我们定义了许多的常用SE3类型与 Sim3优化类型。
下面主要介绍EdgeSE3ProjectXYZ 与 ORB_SLAM3中新添加的 EdgeSE3ProjectXYZOnlyPoseToBody 类型。
EdgeSE3ProjectXYZ
class EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EdgeSE3ProjectXYZ();
bool read(std::istream &is);
bool write(std::ostream &os) const;
void computeError()
{
const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]);
const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[0]);
Vector2d obs(_measurement);
_error = obs - cam_project(v1->estimate().map(v2->estimate()));
}
virtual void linearizeOplus()
{
VertexSE3Expmap *vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
SE3Quat T(vj->estimate());
VertexSBAPointXYZ *vi = static_cast<VertexSBAPointXYZ *>(_vertices[0]);
Vector3d xyz = vi->estimate();
Vector3d xyz_trans = T.map(xyz);
double x = xyz_trans[0];
double y = xyz_trans[1];
double z = xyz_trans[2];
double z_2 = z * z;
Matrix<double, 2, 3> tmp;
tmp(0, 0) = fx;
tmp(0, 1) = 0;
tmp(0, 2) = -x / z * fx;
tmp(1, 0) = 0;
tmp(1, 1) = fy;
tmp(1, 2) = -y / z * fy;
_jacobianOplusXi = -1. / z * tmp * T.rotation().toRotationMatrix();
_jacobianOplusXj(0, 0) = x * y / z_2 * fx;
_jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * fx;
_jacobianOplusXj(0, 2) = y / z * fx;
_jacobianOplusXj(0, 3) = -1. / z * fx;
_jacobianOplusXj(0, 4) = 0;
_jacobianOplusXj(0, 5) = x / z_2 * fx;
_jacobianOplusXj(1, 0) = (1 + y * y / z_2) * fy;
_jacobianOplusXj(1, 1) = -x * y / z_2 * fy;
_jacobianOplusXj(1, 2) = -x / z * fy;
_jacobianOplusXj(1, 3) = 0;
_jacobianOplusXj(1, 4) = -1. / z * fy;
_jacobianOplusXj(1, 5) = y / z_2 * fy;
}
bool isDepthPositive()
{
const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]);
const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[0]);
return (v1->estimate().map(v2->estimate()))(2) > 0.0;
}
Vector2d cam_project(const Vector3d &trans_xyz) const;
double fx, fy, cx, cy;
}
对于一个优化Edge来说，我们主要需要做的工作为，实现一个继承与 g2o::BaseBinaryEdge 或者 g2o::BaseUnaryEdge。
并且重要的是实现虚函数 computeError （对应于该边定义的误差项的表达式）和 linearizeOplus （对应于GN或者LM之类的梯度下降算法中的更新函数，也叫做线性化，
H
δ
x
=
b
H\delta x =b
Hδx=b）函数。
EdgeSE3ProjectXYZ 是一个二元边， 计算的是3D点投影到2D图像的误差。
误差函数
根据相机投影模型，我们知道:
e
r
r
=
o
b
s
−
π
(
T
c
w
P
w
)
err = obs - \pi (T_{cw}P_w)
err=obs−π(TcwPw)
与代码中的对应起来。
_error = obs - cam_project(v1->estimate().map(v2->estimate()));
雅克比矩阵（线性化函数）
线性化函数主要计算的就是误差函数关于当前边连接的定点的导数(雅克比矩阵).
对于上面的公式来说，我们需要求解的就是 $\frac{ \part err} {\part T_{cw}} $ 和KaTeX parse error: Undefined control sequence: \part at position 7: \frac{\̲p̲a̲r̲t̲ ̲err}{\part P_w}，分别是关于3D点的梯度，与相机位姿的梯度。
对位姿求导
对于
KaTeX parse error: Undefined control sequence: \part at position 8: \frac{\̲p̲a̲r̲t̲ ̲err} {\part T_{…
KaTeX parse error: Undefined control sequence: \part at position 8: \frac{\̲p̲a̲r̲t̲ ̲err}{\part T_{c…
根据链式法则。
P
′
=
T
c
w
P
w
P^{'} = T_{cw} P_w
P′=TcwPw
KaTeX parse error: Undefined control sequence: \part at position 8: \frac{\̲p̲a̲r̲t̲ ̲err}{\part T_{c…
先看前半部分 KaTeX parse error: Undefined control sequence: \part at position 7: \frac{\̲p̲a̲r̲t̲ ̲\pi(.)}{\part P…
我们知道相机模型：
$$
\left[ \begin{matrix}
u \ v \ s
\end{matrix} \right]
\left[ \begin{matrix}
f_x & 0 & c_x \0 & f_y & c_y \ 0 & 0 & 1
\end{matrix} \right]
\left[ \begin{matrix}
x \ y \ z
\end{matrix} \right]
归
一
化
尺
度
之
后
，
有
如
下
两
个
公
式
:
归一化尺度之后，有如下两个公式:
归一化尺度之后，有如下两个公式:
u = \frac{f_x}{z} x + c_x \
v = \frac {f_y} {z } y + c_y
$$
$$
\frac{\part \pi(.)}{\part P^{’}} = \frac{\part {\left[ \begin{matrix}
u & v
\end{matrix} \right] }}
{\part \left[ \begin{matrix}
x \ y \ z
\end{matrix} \right]} =
\left[ \begin{matrix}
\frac{\part u}{\part x} & \frac{\part u}{\part y} & \frac{\part u}{\part z} \ \frac{\part v}{\part x} & \frac{\part v}{\part y} & \frac{\part v}{\part z}
\end{matrix} \right]
\left[ \begin{matrix}
\frac{f_x}{z} & 0 & -\frac{{f_x} x}{z^2} \ 0 & \frac{f_y}{z} & -\frac{{f_y} y}{z^2}
\end{matrix} \right]
$$
毕。
对于KaTeX parse error: Undefined control sequence: \part at position 7: \frac{\̲p̲a̲r̲t̲ ̲P^{'}}{\part T_…, 我们需要使用李代数中扰动来对SE3进行求导。这里矩阵的第一行单位矩阵在后面，是因为代码中对于se3的定义为 旋转向量在前。
$$
\frac{\part P^{’}}{\part T_{cw}} = \frac{\part T_{cw} P_w}{\part \delta \theta} =
\left[ \begin{matrix}
- P^{’}{\times} & I \
0 & 0
\end{matrix} \right] \
KaTeX parse error: Can't use function '$' in math mode at position 9: 取出前前三行 $̲\left[ \begin{m…
\frac{\part P^{’}}{\part T{cw}} =
\left[ \begin{matrix}
1 & 0 & 0 & 0 & -z & y \
0 & 1 & 0 & z & 0 & -x \
0 & 0 & 1 & -y & x & 0
\end{matrix}\right]
$$
结合之前求出的$\frac{\part \pi(.)}{\part P^{’}} $
于是的到代码中的 _jacobianOplusXj
$$
\frac{\part err}{\part T_{cw}} = - \frac{\part \pi(.)}{\part P^{’}} \frac{\part P^{’}}{\part T_{cw}} = -
\left[
\begin{matrix}
\frac{f_x}{z} & 0 & -\frac{{f_x} x}{z^2} \
0 & \frac{f_y}{z} & -\frac{{f_y} y}{z^2}
\end{matrix}
\right]
\left[
\begin{matrix}
0 & z & -y & 1 & 0 & 0 \
-z & 0 & x & 0 & 1 & 0 \
y & -x & 0 & 0 & 0 & 1
\end{matrix}
\right]
\
\left[
\begin{matrix}
\frac{{f_x} x y}{z^2} & -f_x - \frac{f_x x2}{z2} & \frac{f_x y}{z} &-\frac{f_x}{z} & 0 & \frac{f_x x}{z}\
f_y + \frac{f_y y2}{z2} & -\frac{{f_y} x y}{z^2}& -\frac{f_y x}{ z} & 0 & -\frac{f_y}{z} & \frac{{f_y} y}{z^2}
\end{matrix}
\right]
$$
对路标点求导
KaTeX parse error: Undefined control sequence: \part at position 8: \frac{\̲p̲a̲r̲t̲ ̲err}{\part P_{w…
已知 $ P^{’} = T_{cw} P_w = R_{cw} P_w + t_{cw}$
$$
\frac{\part err}{\part P_{w}} = - \frac{\part \pi(.)}{\part P^{’}} R_{cw}\
-\left[
\begin{matrix}
\frac{f_x}{z} & 0 & -\frac{{f_x} x}{z^2} \
0 & \frac{f_y}{z} & -\frac{{f_y} y}{z^2}
\end{matrix}
\right] R_{cw}
$$
与代码中的
_jacobianOplusXi = -1. / z * tmp * T.rotation().toRotationMatrix();
以上推导完毕。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
