c++ - 我想要使​​用棋盘的外在矩阵，但solvePnP的结果很奇怪

Question

我正在使用棋盘来获取外在矩阵。 findChessboardCorners正确找到图像点，并且应正确设置物点。但是当捕捉相同的场景时，我得到了非常奇怪的结果。

• 我正在使用 OpenCV 2.1.0。
• 我正在捕获的一些屏幕截图。
• 样本结果（注意 nan...）

    +2,2267e+35 -1,8254e+00 -1,4695e+07 +3,8279e+08
    +1,5230e+19 -1,5452e+00 -1,4752e-21 +3,5204e+16
    -1,3286e+13 +1,7682e+00 -2,9379e-15 +2,6464e-34
    +0,0000e+00 +0,0000e+00 +0,0000e+00 +1,0000e+00

    -南 -1,7778e+00 +5,9374e-20 +3,4842e+17
    +2,5102e+36 +1,6198e+00 -1,1908e-31 +2,6853e+16
    -4,1601e+13 +1,8132e+00 +1,7271e+27 +7,3568e-01
    +0,0000e+00 +0,0000e+00 +0,0000e+00 +1,0000e+00

所以我显然做错了什么。什么？

代码：

#include <cv.h>
#include <highgui.h>
#include <vector>
#include <iostream>
#include <cstdio>

class Camera
{
    cv::VideoCapture* cam;
    cv::Mat_<float> intr, extr;
public:
    Camera (int n = 0)
    {
        intr = cv::Mat_<float>(3, 3, 0.f);

        intr.at<float>(0, 0) = 2.7105506628580332e+02;
        intr.at<float>(0, 1) = 0;
        intr.at<float>(0, 2) = 1.5950000000000000e+02;

        intr.at<float>(1, 0) = 0;
        intr.at<float>(1, 1) = 2.7105506628580332e+02;
        intr.at<float>(1, 2) = 1.1950000000000000e+02;

        intr.at<float>(2, 0) = 0;
        intr.at<float>(2, 1) = 0;
        intr.at<float>(2, 2) = 1;

        cam = new cv::VideoCapture(n);
        if(!cam->isOpened())
            std::cerr << "arg\n";


    }

    void calib_extrinsic ()
    {
        cv::namedWindow("w");
        std::vector<cv::Point3f> obj_points;
        std::vector<cv::Point2f> img_points;
        std::vector<cv::Point2f> corners;
        cv::Size size(4, 3);
        float cell_size = 1;

        for(int i = 0; i < size.height; ++i)
            for(int j = 0; j < size.width; ++j)
                obj_points.push_back(cv::Point3f(float(j*cell_size),
                                                 float(i*cell_size), 0.f));

        cv::Mat img, gray;

        while(1) {
            *cam >> img;
            cv::cvtColor(img, gray, CV_BGR2GRAY);

            img_points.clear();
            corners.clear();
            bool found = cv::findChessboardCorners(gray, size, corners,
                                                   CV_CALIB_CB_ADAPTIVE_THRESH
                                                   | CV_CALIB_CB_FILTER_QUADS);

            if(found) {
                // cv::cornerSubPix(gray, corners,
                //                  cv::Size(11, 11),
                //                  cv::Size(-1, -1),
                //                  cv::TermCriteria(CV_TERMCRIT_EPS
                //                                   | CV_TERMCRIT_ITER, 30, 0.1));
                cv::drawChessboardCorners(img, size, cv::Mat(corners), found);
            }

            cv::imshow("w", img);
            int key = cv::waitKey(15) & 0xff;
            //std::cout << found << " " << key << "\n";
            if(key == ' ' && found) {

                cv::Mat_<float> distCoeffs (4, 1, 0.f);
                cv::Mat_<float> r (3, 3, 0.f);
                cv::Mat_<float> rvecs (3, 1, 0.f);
                cv::Mat_<float> tvecs (3, 1, 0.f);

                cv::solvePnP(cv::Mat(obj_points), cv::Mat(corners), intr, distCoeffs, rvecs, tvecs);
                cv::Rodrigues(rvecs, r);
                extr = cv::Mat_<float>(4, 4, 0.f);

                for(int y = 0; y < 3; y++) {
                    for(int x = 0; x < 3; x++)
                        extr.at<float>(y, x) = r.at<float>(y, x);

                    extr.at<float>(y, 3) = tvecs.at<float>(y, 0);
                }

                extr.at<float>(3, 0) = 0.f;
                extr.at<float>(3, 1) = 0.f;
                extr.at<float>(3, 2) = 0.f;
                extr.at<float>(3, 3) = 1.f;

                for(int y = 0; y < 4; y++) {
                    for(int x = 0; x < 4; x++) {
                        printf("%+.4e ", extr.at<float>(y, x));
                    }
                    putchar('\n');
                }
                putchar('\n');

                //break;
            }
        }


    }
};

int main ()
{
    Camera cam;
    cam.calib_extrinsic();
}

Answer 1

这是一个类型问题。SolvePNP 创建双矩阵，而不是浮点数。

Answer 2

看起来旋转和平移向量没有被正确访问。例如，您可能需要在 Rodrigues 函数中使用 rvecs[0]（对平移向量执行相同操作）。您可以尝试运行此代码吗？确保将 model_points 和 image_points push_backs 替换为您自己各自的 3D 模型和 2D 图像点。这对我有用并显示重新投影的输出，以便您验证结果：

Mat frame = imread("path/to/image.png");

vector<vector<Point3f> > model_points(1);
model_points[0].push_back(Point3f(FILL HERE AND ADD MORE));

vector<vector<Point2f> > image_points(1);
image_points[0].push_back(Point2f(FILL HERE AND ADD MORE));

Mat camera_matrix;
Mat distortion(1, 5, CV_32FC1);
distortion.setTo(0);
vector<Mat> rvecs, tvecs;
calibrateCamera(model_points, 
              image_points, 
              frame.size(), 
              camera_matrix, 
              distortion, 
              rvecs, tvecs, 
              CV_CALIB_FIX_K1 | CV_CALIB_FIX_K2 |
              CV_CALIB_FIX_K3 | CV_CALIB_FIX_K4 |
              CV_CALIB_FIX_K5);

Mat rotation;
Rodrigues(rvecs[0], rotation);

vector<Point2f> new_points;
projectPoints(model_points[0], rvecs[0], tvecs[0], camera_matrix, distortion, new_points); 

for (int i = 0; i < new_points.size(); ++i) {
    circle(frame, new_points[i], 3, Scalar(0, 0, 255), -1);
}

namedWindow("temp");
imshow("temp", frame);
waitKey(0);