pnp简单讲解

八倍体黑麦

已于 2024-02-18 11:03:25 修改

阅读量1k

点赞数 24

文章标签：数码相机计算机视觉人工智能 opencv

于 2024-02-17 22:26:49 首次发布

本文链接：https://blog.csdn.net/m0_73735838/article/details/136142048

版权

pnp简单讲解

一、solvepnp

bool solvePnP( InputArray objectPoints, InputArray imagePoints,
                            InputArray cameraMatrix, InputArray distCoeffs,
                            OutputArray rvec, OutputArray tvec,
                            bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );

objectPoints：世界坐标系下的3D点坐标数组，至少4个点

imagePoints：图像中对应3D点的成像点坐标数组 cameraMatrix：

相机内参矩阵 distCoeffs：相机畸变系数数组，NULL无畸变。

rvec和tvec：计算结果输出，rvec为旋转向量，tvec为平移向量，两者合并表达的是物体整体（即世界坐标系）在相机坐标系中的位姿

 solvePnPRansac
bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
                                  InputArray cameraMatrix, InputArray distCoeffs,
                                  OutputArray rvec, OutputArray tvec,
                                  bool useExtrinsicGuess = false, int iterationsCount = 100,
                                  float reprojectionError = 8.0, double confidence = 0.99,
                                  OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );

iterationsCount：迭代次数；

reprojectionError：RANSAC使用的内点阈值，即考虑作为内点的观察点与计算点投影之间的最大允许距离

confidence：算法得到有用结果的概率；

inliers：包含 objectPoints 和 imagePoints 中的内点索引的输出向量

二、四个坐标系

世界坐标系、相机坐标系、图像坐标系、像素坐标系。

世界坐标系：机器人或相机运动过程中，肯定需要知道它的位置，因此需要设定世界坐标系，认定固定不动，作为参考坐标系，描述世界中的任何一点P（Xw,Yw,Zw)。

相机坐标系：相机或机器人运动的一个坐标系，通过世界坐标系的变换（旋转R,平移T）计算得到。因此主要是将世界坐标系描述的点坐标P（Xw,Yw,Zw)转换成相机坐标系下描述P（Xc,Yc,Zc)，方便计算得到在成像坐标系的坐标。

图像（成像）坐标系：描述点在图像坐标系的成像点位置。

像素坐标：在相机中得到的是一个像素，因此主要将图像坐标系的点转换成像素坐标系下。

三、位姿

位姿”包含位置和姿态两部分内容，是位置(位移)+姿态(旋转)*的简称，不是打错的“位置”。一般情况下，旋转可以用*旋转矩阵R表示，平移可以用平移向量t表示

pnp：【【机器人学导论课程】台湾大学林沛群教授_人工智能AIoT(Robotics)学习参考教程，现整理分享给大家！】【机器人学导论课程】台湾大学林沛群教授_人工智能AIoT(Robotics)学习参考教程，现整理分享给大家！_哔哩哔哩_bilibili

线代：【【官方双语/合集】线性代数的本质 - 系列合集】【官方双语/合集】线性代数的本质 - 系列合集_哔哩哔哩_bilibili

四、坐标系变换

建立坐标系

建立一个新的右手坐标系——装甲板坐标系，以装甲板中心为原点，装甲板的四个顶点关于原点中心对称

format,png

vector<Point3f> point3d;
    float half_x = 13.4 / 2;
    float half_y = 5.7 / 2;
    point3d.push_back(Point3f(-half_x, -half_y, 0));
    point3d.push_back(Point3f(half_x, -half_y, 0));
    point3d.push_back(Point3f(half_x, half_y, 0));
    point3d.push_back(Point3f(-half_x, half_y, 0));

    point2d.push_back(tl);
    point2d.push_back(tr);
    point2d.push_back(br);
    point2d.push_back(bl);

 Mat cam_matrix = (Mat_<double>(3, 3) << 1.6041191539594568e+03, 0, 6.3983687194220943e+02,
                      0, 1.6047833493341714e+03, 5.1222951297937527e+02, 0, 0, 1); // 相机参数
    Mat distortion_coeff = (Mat_<double>(5, 1) << -6.4910709385278609e-01, 8.6914328787426987e-01,
                            5.1733428362687644e-03, -4.1111054148847371e-03, 0); // 畸变系数
    solvePnPRansac(point3d, point2d, cam_matrix, distortion_coeff, rvec, tvec);

    double dis = sqrt(tx * tx + ty * ty + tz * tz);
    double rx = (atan2(R.at<double>(2, 1), R.at<double>(2, 2))) * 180 / CV_PI;
    double ry = (atan2(-R.at<double>(2, 0), sqrt(pow(R.at<double>(1, 0), 2) + pow(R.at<double>(0, 0), 2)))) * 180 / CV_PI;
    double rz = (atan2(R.at<double>(1, 0), R.at<double>(0, 0))) * 180 / CV_PI;