学习笔记

浅谈PnP的算法原理与实践

发布于 2023/01/14 · 7 分钟阅读

学习笔记#数学#SLAM#RoboMaster
文章目录
  1. PnP问题概述
  2. 针孔相机模型
  3. DLT方法（Direct Linear Transform）
  4. 实践：检测二维码与相机的相对位置

PnP问题概述

PnP (Perspective-n-Point) 是求解3D到2D点对运动的方法，目的是求解相机坐标系相对世界坐标系的位姿。已知n个3D点的坐标 (相对世界坐标系) 以及这些点的像素坐标时，如何估计相机的位姿(即求解世界坐标系到相机坐标系的旋转矩阵 $R$ 和平移向量 $t$ )

定义 表达符号
相对世界坐标 $P^W= \begin{bmatrix}X^W&Y^W&Z^W\end{bmatrix}^T$
相对相机坐标 $P^C= \begin{bmatrix}X&Y&Z\end{bmatrix}^T$
像素坐标（齐次化） $P^{u,v}= \begin{bmatrix}u&v&1\end{bmatrix}^T$
归一化坐标 $P’= \begin{bmatrix}X\over{Z}&Y\over{Z}&1\end{bmatrix}^T$
相机内参 $K=\begin{pmatrix}f_x&0&c_x\0&f_y&c_y\0&0&1\end{pmatrix}$
深度信息 $ω = Z^C $

已知：n个点在世界坐标系下的坐标$P_1^W,P_2^W,…,P_n^W$，这些点相应在像素坐标系下的坐标$p_1^{u,v},p_2^{u,v},…,p_n^{u,v}$，相机内参矩阵 $K$。求解：相机坐标系相对于世界坐标系的位姿，即$P^C=R_{CW}×P_W+t_{WC}$中的$R_{CW}$和$t_{CW}$，或记作$T = \begin{bmatrix}R|T\end{bmatrix}$

针孔相机模型

基本法则

通过针孔相机模型，有以下公式： $$ \omega P^{u,v}=Z\begin{pmatrix}u\v\1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\0&f_y&c_y\0&0&1\end{pmatrix}\begin{pmatrix}X\Y\Z\end{pmatrix}=KP_C=KTP_W $$ 对于$T=\begin{pmatrix}R|t\end{pmatrix}=\begin{pmatrix}t_1&t_2&t_3&t_4\t_5&t_6&t_7&t_8\t_9&t_{10}&t_{11}&t_{12}\end{pmatrix}$ ，共有12个未知数，则至少需要6个点对来求解方程

归一化处理

将物体的坐标减少一维处理： $$ P^C=\omega P’ $$ $$ P^C=(RP^W+t)=\begin{bmatrix}X&Y&Z\end{bmatrix}^T \rightarrow P’=\begin{bmatrix}X\over{Z}&Y\over{Z}&1\end{bmatrix}^T \overset{def}{=} \begin{bmatrix}u’&v’&1\end{bmatrix}^T $$ 可以看作把点转化为$Z=1$这一平面上

DLT方法（Direct Linear Transform）

DLT是最直接求解PnP问题的方法，通过对线性等式进行化简，转化为线性方程组的形式求解变换矩阵

算法介绍

  1. 对等式进行化简： $$ \omega P^{u,v}=K\begin{bmatrix}R|t\end{bmatrix}P^W $$ $$ \omega K^{-1}P^{u,v}=TP^W $$ $$ \because wP^{u,v}=KP^C \therefore \omega K^{-1}P^{u,v} = P_C $$ $$ P^C = TP^W $$

  2. 归一化处理： $$ P^C=\omega P’=TP^W \ \omega \begin{pmatrix}u’\v’\1\end{pmatrix} = \begin{pmatrix}t_1&t_2&t_3&t_4\t_5&t_6&t_7&t_8\t_9&t_{10}&t_{11}&t_{12}\end{pmatrix} \begin{pmatrix}X^W\Y^W\Z^W\1\end{pmatrix} $$ 观察$u’和v’$ ，可以得到 $$ \begin{cases} u’=\frac{t_1X+t_2Y+t_3Z+t_4}{t_9X+t_{10}Y+t_{11}Z+t_{12}} \ v’=\frac{t_5X+t_6Y+t_7Z+t_8}{t_9X+t_{10}Y+t_{11}Z+t_{12}} \end{cases} $$ 将转换矩阵$T$的行向量重新写为 $$ t_{r1}=\begin{bmatrix}t_1\t_2\t_3\t_4\end{bmatrix}
    t_{r1}=\begin{bmatrix}t_5\t_6\t_7\t_8\end{bmatrix}
    t_{r1}=\begin{bmatrix}t_9\t_{10}\t_{11}\t_{12}\end{bmatrix} $$ 把上述方程组看作是向量点乘并且移项，化简；再写作为矩阵形式，由于$t_{r1},t_{r2},t_{r3}$为未知量，写成$At=0$ $$ \begin{cases} t_{r1}^T\ P^W - t_{r3}^T\ P^W\ u’ = 0 \ t_{r2}^T\ P^W - t_{r3}^T\ P^W\ v’ = 0 \end{cases} \iff \begin{pmatrix} P^{W\ T} & 0 & -u’^T\ P^W \ 0& P^{W\ T} & -v’^T\ P^W \end{pmatrix} \begin{pmatrix} t_{r1} \ t_{r2} \ t_{r3} \end{pmatrix} =0 $$

  3. 构造方程组

    将n个点对代入上式，可以得到： $$ \begin{pmatrix} P_1^{W\ T} & 0 & -u_1’^T\ P_1^W \ 0& P_1^{W\ T} & -v_1’^T\ P_1^W \ \vdots & \vdots & \vdots \ P_N^{W\ T} & 0 & -u_N’^T\ P_N^W \ 0& P_N^{W\ T} & -v_N’^{T}\ P_N^W \end{pmatrix} \begin{pmatrix} t_{r1} \ t_{r2} \ t_{r3} \end{pmatrix} =0 $$ 通过消元求解即可

多个点优化处理

$ t $一共有12维，最少通过6对匹配点即可实现增广矩阵$T=[R|t]$的线性求解。当匹配点的数量大于6对时，可以使用SVD等方法求超定方程的最小二乘解。针对SVD求超定方程的最小二乘解，说明如下：当匹配点数量大于6对时，可以获得一个在$|t|=1$约束下的最小二乘解$t^∗=argmint||At||$。具体的，令$A=UDV^T$，则最小二乘解$t^∗$为$V$的最后一列$t^v$，再利用SVD确定最优旋转矩阵近似以及相应的尺度，便可以确定最终的相机姿态

实践：检测二维码与相机的相对位置

在PnP问题中，如果把物体看作是世界坐标的原点，那么求解相机相对于世界坐标原点的位姿就等价于求解了相机与物体的相对位置。对于一个典型的PnP问题，可以利用opencv4中自带的solvePnP()函数实现，在实践中，需要获得物体的像素坐标，并且以物体为中心，建立世界坐标系。

作者在实践中是在ROS系统中获取了Realsense D455摄像头的topic来获取的摄像头画面。以下代码都需要：

#include<iostream>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace cv;

完整代码请见：https://github.com/Phoenizard/SolvePnP

获取角点坐标

使用二维码的目的是为了更简便的获取二维码的角点数据（最小正接旋转矩形的四个顶点），利用opencv4中的QRCodeDetector内置对象，构造了一个二维码检测器，返回值为vector<cv::Point2f>类型

vector<Point2f> get2DPoint(Mat rawImage)
{
    QRCodeDetector qrDecoder = QRCodeDetector();
    // 构造二维码检测器
    vector<Point2f> bbox;
    qrDecoder.detect(rawImage,bbox);
    // detect(src, output array) 
    // output array可以为vector和Mat
    if(!bbox.empty()) {
        string tag[4] = {"A","B","C","D"};
        for(int i=0;i<4;++i){
            line(rawImage, bbox[i], bbox[(i+1)%4], Scalar(124,252,0),4,8);
            // 绘制轮廓
            putText(rawImage, tag[i],bbox[i],FONT_HERSHEY_DUPLEX,2,Scalar(0,0,255),2,8);
        }
    }
    return bbox;
}

效果图如下，可以看到二维码被框出，系统输出四个角点坐标：

注意是从左上顺指针输出角点坐标

getPoint

构造世界坐标系

由于二维码是2D的，以正方形的中心，二维码平面为$xy$平面建系，构造三维点集，用vector<cv::Point3f>存储，需要与得到的2d点一一对应。

#define HALF_LENGTH 65
// 定义二维码边长的一边
// 注意：单位为毫米mm
vector<Point3f> point_in_3d = vector<Point3f>{
            Point3f(-HALF_LENGTH, HALF_LENGTH, 0),
            Point3f(HALF_LENGTH, HALF_LENGTH, 0),
            Point3f(HALF_LENGTH, -HALF_LENGTH, 0),
            Point3f(-HALF_LENGTH, -HALF_LENGTH, 0)
        };

调用solvePnP()函数

solvePnP() 在官方文档中为：

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)
  • InputArray objectPoints ：世界坐标系中点的三维坐标
  • InputArray imagePoints ：点的像素坐标
  • InputArray cameraMatrix ：相机内参
  • InputArray distCoeffs ：畸变系数
  • OutputArray rvec ：输出旋转矩阵
  • OutputArray tvec ：输出平移矩阵
  • useExtrinsicGuess ：仅用于flags=SOLVEPNP_ITERATIVE，此值如果为true 需要rvec和tvec有输入值，以便函数把输入值作为旋转和平移的估计初始值
  • flags ：求解方法，默认SOLVEPNP_ITERATIVE，可选SOLVEPNP_P3P、SOLVEPNP_EPNP等等

由于技术条件限制，使用了BSSN在Ego-plannerforIntelligentUAVChampionshipSimulator库中的相机参数（同为Realsense D455摄像头）

const Mat cameraMatrix = (Mat_<double>(3, 3) <<
        268.5118713378906, 0.0,               320.0,
        0.0,               268.5118713378906, 240.0,
        0.0,               0.0,               1.0
    );
const Mat distCoeffs = (Mat_<double>(5, 1) << 0, 0, 0, 0, 0);

一般来说：解算PnP，最少需要4个物体点与其成像点构成的点对，直接使用solvePnP求解：

void SubPuber::solvePnP2_3(vector<Point2f> pnt2d, vector<Point3f> obj3d, Mat rVec, Mat tVec)
{
    solvePnP(obj3d, pnt2d, cameraMatrix, distCoeffs, rVec, tVec, false, SOLVEPNP_ITERATIVE);
    cout << "RVec" << endl << rVec << endl;
    cout << "tVec" << endl << tVec << endl;
}

利用cmake编译

ROS统一运用cmake进行编译，重点在于调用opencv4

cmake_minimum_required(VERSION 3.0.2)
project(object-camera-transform)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  geometry_msgs
  image_transport
  roscpp
  rospy
  sensor_msgs
  std_msgs
)
find_package(OpenCV REQUIRED)
catkin_package(
)
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)
add_executable(solvepnp2d3d src/solvepnp2d3d.cpp src/mainsolve2d3d.cpp)
target_link_libraries(solvepnp2d3d
  ${catkin_LIBRARIES}
  ${OpenCV_LIBS}
)