1_4 3D点与2D匹配计算相机pose_2d图像和3d点云匹配的-优快云博客

本文介绍了计算相机位姿的两种主要方法：直接法，如OpenCV的solvePnP函数，以及基于BA（BundleAdjustment）的优化方法，包括高斯-牛顿法和g2o库的应用。通过最小化误差来优化相机姿态，展示了从像素坐标到相机坐标转换的过程，并提供了相关代码示例。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

对于已知3D点和2D点的匹配关系，进而计算相机位姿的方法有两大类，一类是直接求解法，包括DLS、PNP等；一类是基于BA的优化方法；

1、直接法

直接法可以利用opencv中已有的函数solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false)进行求解；

2、BA的方法

对于BA方法实际上是通过优化最小误差进行实现，而优化方法可以有多种方式，包括采用高斯-牛顿法、g2o、ceres等工具库，这里介绍高斯-牛顿法和g2o两种方法；

无论哪种方法首先需要给出优化误差，具体如下：

$\xi ^{*}=argmin\frac{1}{2}\sum_{i=1}^{n}\left \| p_{i}-\frac{1}{s_{i}}Kexp(\xi^{\wedge})P_{i} \right \|_{2}^{2}$

为了简化计算，这里将 $exp(\xi ^{\wedge})P_{i}=P^{'}$

考虑单个投影点误差： $e_{i}=u_{i}-\frac{1}{s_{i}}Kexp(\xi ^{\wedge})P_{i}$

对其线性化后： $e_{i}(x+\Delta x)\approx e_{i}(x)+J(x)\Delta x$

雅可比的形式如下： $\frac{\partial e}{\partial \delta \xi}=\lim_{\delta \xi \to 0} \frac{e(\delta \xi \bigoplus \xi)}{\delta \xi}=\frac{\partial e}{\partial P^{'}} \frac{\partial P^{'}}{\partial \delta \xi}$

这里的P'为P在相机坐标系下的坐标： $P^{'}=(exp(\xi ^{\wedge})P)_{1:3}=[X^{'},Y{'},Z{'}]^{T}$

对P'进行投影 $su=KP^{'}$

这里 $\begin{bmatrix} su &\\sv&\\s \end{bmatrix} =\begin{bmatrix} f_{x}&0 &c_{x} \\ 0& f_{x} &c_{y} \\ 0 &0 &1 \end{bmatrix}\begin{bmatrix} X^{'}\\ Y^{'}\\ Z^{'} \end{bmatrix}$