【ORBSLAM2点线融合】点特征图模型构建

最新推荐文章于 2023-10-28 17:38:12 发布

「已注销」

最新推荐文章于 2023-10-28 17:38:12 发布

阅读量500

点赞数

本文链接：https://blog.csdn.net/weixin_62266575/article/details/127304786

版权

本文介绍了在SLAM中如何构建基于点特征的图优化模型，详细推导了点特征重投影误差关于相机位姿增量和三维点坐标的雅克比矩阵，并探讨了在g2o中如何实现这些优化。

摘要由CSDN通过智能技术生成

在SLAM中，通常用BA（Bundle Adjustment）来实现多个三维点和不同相机位姿的优化
本文描述如何建立基于点特征的图优化，并推导相应的雅克比矩阵，并用g2o实现相应的类

1. 点特征误差及观测模型

假设相机位姿为 $T_{cw}$ 表示世界坐标系到相机坐标系的变换，或者表示成 $R_{cw},t_{cw}$ 。三维空间点表示成
世界坐标下 $P^{w}=\begin{bmatrix} P^{w}_{x}, P^{w}_{y}, P^{w}_{z}\end{bmatrix}^{T}$
相机坐标下 $P^{c}=\begin{bmatrix} P^{c}_{x}, P^{c}_{y}, P^{c}_{z}\end{bmatrix}^{T}$
相机内参为 $K$
重投影误差为 $e_{p}=p-p^{'}=\begin{bmatrix} u \\ v\end{bmatrix}-\begin{bmatrix} u^{'} \\ v^{'}\end{bmatrix}$
其中 $p$ 为当前图像中提取的点， $p^{'}$ 为与 $p$ 匹配的三维空间点的投影点，计算方式为
$\begin{aligned} P^{c}&=R_{cw}P^{w}+t_{cw} \\ u^{'} &= \frac {f_{x}P^{c}_{x}} {P^{c}_{z}}+c_{x} \\ v^{'} &= \frac {f_{y}P^{c}_{y}} {P^{c}_{z}}+c_{y} \end{aligned}$

2. 点特征重投影误差关于位姿增量的雅克比矩阵

利用链式法则，可以求得
$\frac{\partial e_{p}}{\partial \delta\xi}=\frac{\partial e_{p}}{\partial P^{c}}\frac{\partial P^{c}}{\partial \delta\xi }$
下面分别计算等式右边两项

这里注意，这里需要涉及到矩阵求导，矩阵求导通常分为分子布局和分母布局，这两种情况只是表达形式的不同，本身并没有两样，下面统一都用分母布局，具体求导结果可以看这篇文章

第一部分
$\begin{aligned} \frac{\partial e_{p}}{\partial P^{c}}&= \frac{\partial \begin{bmatrix} u-u^{'}\\ v-v^{'}\end{bmatrix}}{\partial P^{c}} \\ &= \begin{bmatrix} \frac{\partial(u-u^{'})}{\partial P_{x}^{c}} & \frac{\partial(u-u^{'})}{\partial P_{y}^{c}} & \frac{\partial(u-u^{'})}{\partial P_{z}^{c}} \\ \frac{\partial(v-v^{'})}{\partial P_{x}^{c}} & \frac{\partial(v-v^{'})}{\partial P_{y}^{c}} & \frac{\partial(v-v^{'})}{\partial P_{z}^{c}} \end{bmatrix} \\ &= -\begin{bmatrix} \frac{f_{x}}{P_{z}^{c}} & 0 & \frac{f_{x}P_{x}^{c}}{P_{z}^{c2}} \\ 0 & \frac{f_{y}}{P_{z}^{c}} & \frac{f_{y}P_{y}^{c}}{P_{z}^{c2}} \end{bmatrix}_{2 \times 3} \end{aligned}$