PnP 位姿解算学习笔记

PnP（Perspective-n-Point）的作用#

已知：

平面图像上的三个点，或者更多
物理世界中对应点之间的三维位置关系
相机的内参矩阵

能够求出：

平面图像上的点在三维空间中的位置（相对于相机坐标系）

cv::solvePnP#

1
bool cv::solvePnP(
2
    InputArray objectPoints, // 物体坐标系下的三维点
3
    InputArray imagePoints,  // 图像坐标系下的二维点
4
    InputArray cameraMatrix, // 相机内参矩阵
5
    InputArray distCoeffs,   // 相机畸变参数
6
    OutputArray rvec,        // 输出旋转向量
7
    OutputArray tvec,        // 输出平移向量
8
    bool useExtrinsicGuess = false, // 是否使用外参初值
9
    int flags = SOLVEPNP_ITERATIVE // 求解方法
10
);

objectPoints#

是物体坐标系下的三维点，通常是一个 N x 3 的矩阵，表示 N 个三维点的坐标。

对于识别甲板而言，就是左上角、右上角、右下角、左下角四个点的三维坐标。例如：

那么就有：

1
static const double ARMOR_WIDTH = 0.135;  // 装甲板宽度
2
static const double LIGHTBAR_LENGTH = 0.15; // 灯条长度
3

4
static const std::vector<cv::Point3f> objectPoints = {
5
    {-ARMOR_WIDTH / 2, LIGHTBAR_LENGTH / 2, 0},   // 左上角
6
    {ARMOR_WIDTH / 2, LIGHTBAR_LENGTH / 2, 0},   // 右上角
7
    {ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2, 0},  // 右下角
8
    {-ARMOR_WIDTH / 2, -LIGHTBAR_LENGTH / 2, 0}   // 左下角
9
};

在这里我们把甲板的几何中心当做原点，那么剩下的点就可以定位了。

solvePnP 解出的东西#

旋转向量 rvec，即目标平面的朝向
平移向量 tvec，即目标平面到相机的距离

这里 rvec 和 tvec 是直接被 solvePnP 函数地址引用了，所以在函数调用后就可以直接使用。

拿到具体参数#

1
tvec.at<double>(0) // x
2
tvec.at<double>(1) // y
3
tvec.at<double>(2) // z
4

5
rvec.at<double>(0) // 绕 x 轴旋转角度
6
rvec.at<double>(1) // 绕 y 轴旋转角度
7
rvec.at<double>(2) // 绕 z 轴旋转角度

旋转的表示方法#

旋转向量 rvec 到旋转矩阵 rmat

1
cv::Mat rmat;
2
cv::Rodrigues(rvec, rmat); // 罗德里格斯公式

旋转矩阵 rmat 到欧拉角

1
cv::Vec3f euler = rotationMatrixToEulerAngles(rmat);

欧拉角到四元数

1
cv::Vec4f quaternion = eulerAnglesToQuaternion(euler);

旋转向量、旋转矩阵、欧拉角、四元数有什么区别？#

旋转向量：用一个三维向量表示旋转，向量的方向表示旋转轴，向量的长度表示旋转角度。
旋转矩阵：用一个 3x3 的矩阵表示旋转，矩阵的每一列代表旋转后坐标轴的方向。
欧拉角：用三个角度表示旋转，通常表示绕 x、y、z 轴的旋转角度。
四元数：用四个数表示旋转，可以避免万向节死锁问题，计算效率高。

PnP（Perspective-n-Point）的作用#

cv::solvePnP#

objectPoints#

solvePnP 解出的东西#

拿到具体参数#

旋转的表示方法#

旋转向量、旋转矩阵、欧拉角、四元数有什么区别？#

支持与分享

评论区

目录