外部校准使用cv :: SolvePnP [英] Extrinsic Calibration With cv::SolvePnP

问题描述

我目前正在尝试使用外部跟踪系统来实施基于网络摄像头的AR的替代方法。我有我的环境中的一切配置保存为外在校准。我决定使用 cv :: solvePnP（），因为它应该是几乎完全是我想要的，但两个星期后，我拉我的头发，试图让它的工作。下图显示了我的配置。 c1是我的相机，c2是我使用的光学跟踪器，M是附加到相机的跟踪标记，ch是棋盘。

I'm currently trying to implement an alternate method to webcam-based AR using an external tracking system. I have everything in my environment configured save for the extrinsic calibration. I decided to use cv::solvePnP() as it supposedly does pretty much exactly I want, but after two weeks I am pulling my hair out trying to get it to work. A diagram below shows my configuration. c1 is my camera, c2 is the optical tracker I'm using, M is the tracked marker attached to the camera, and ch is the checkerboard.

它传递我的图像像素坐标与 cv :: findChessboardCorners c $ c>。参考附加到照相机c1的跟踪标记M获取世界点（外部因此是从该标记的帧到照相机原点的变换）。我已经测试这与数据集达50点，以减轻局部最小值的可能性，但现在我测试只有四个2D / 3D点对。从 cv :: solvePnP（）返回的rvec和tvec得到的外在的I是从平移和旋转相对于一个基本事实创建以及一个基本的视觉分析（翻译意味着一个1100mm的距离，而相机最多10mm）。

As it stands I pass in my image pixel coordinates acquired with cv::findChessboardCorners(). The world points are acquired with reference to the tracked marker M affixed to the camera c1 (The extrinsic is thus the transform from this marker's frame to the camera origin). I have tested this with data sets up to 50 points to mitigate the possibility of local minima, but for now I'm testing with only four 2D/3D point pairs. The resulting extrinsic I get from the rvec and tvec returned from cv::solvePnP() is massively off in terms of both translation and rotation relative to both a ground truth extrinsic I manually created as well as a basic visual analysis (The translation implies a 1100mm distance while the camera is at most 10mm away).

最初我认为问题是，我的董事会的姿态有一些歧义，但现在我确定不是这样。数学看起来很简单，在我完成了系统设置的工作之后，对于一个基本上是一个单元的人来说，这是一个巨大的挫折。我真的耗尽了选择，所以如果任何人都可以帮助我将巨大的债务。我的测试代码在下面发布，和我的实现减去一些渲染调用相同。我有与我的程序工作的地面实况外在如下（基本上围绕一个轴和一个小翻译纯旋转）：

Initially I thought the issue was that I had some ambiguities in how my board pose was determined, but now I'm fairly certain that's not the case. The math seems pretty straightforward and after all my work on setting the system up, getting caught up on what is essentially a one-liner is a huge frustration. I'm honestly running out of options, so if anyone can help I would be hugely in your debt. My test code is posted below and is the same as my implementation minus some rendering calls. The ground truth extrinsic I have that works with my program is as follows (Basically a pure rotation around one axis and a small translation):

1     0     0     29
0   .77  -.64   32.06
0   .64   .77  -39.86
0     0     0     1

谢谢！

#include <opencv2\opencv.hpp>
#include <opencv2\highgui\highgui.hpp>

int main()
{
    int imageSize     = 4;
    int markupsSize   = 4;
    std::vector<cv::Point2d> imagePoints;
    std::vector<cv::Point3d> markupsPoints;
    double tempImage[3], tempMarkups[3]; // Temp variables or iterative data construction  

    cv::Mat CamMat = (cv::Mat_<double>(3,3) <<  (566.07469648019332), 0, 318.20416967732666, 
        0,  (565.68051204299513), -254.95231997403764, 0,  0, 1);  

    cv::Mat DistMat = (cv::Mat_<double>(5,1) <<  -1.1310542849120900e-001, 4.5797249991542077e-001,
    7.8356355644908070e-003, 3.7617039978623504e-003, -1.2734302146228518e+000);

    cv::Mat rvec = cv::Mat::zeros(3,1, cv::DataType<double>::type);
    cv::Mat tvec = cv::Mat::zeros(3,1,cv::DataType<double>::type);
    cv::Mat R;
    cv::Mat extrinsic = cv::Mat::eye(4, 4, CV_64F);

    // Escape if markup lists aren't equally sized
    if(imageSize != markupsSize)
    {
    //TODO: Replace with try, throw error code, and catch in qSlicerLocalizationModuleWidget
    return 0;
    }

    // Four principal chessboard corners only
    imagePoints.push_back(cv::Point2d(368.906, 248.123));
    imagePoints.push_back(cv::Point2d(156.583, 252.414));
    imagePoints.push_back(cv::Point2d(364.808, 132.384));
    imagePoints.push_back(cv::Point2d(156.692, 128.289));

    markupsPoints.push_back(cv::Point3d(495.115, 39.106, 93.79));  
    markupsPoints.push_back(cv::Point3d(463.143, -86.286, -51.178));  
    markupsPoints.push_back(cv::Point3d(500.236, 121.988, 24.056));  
    markupsPoints.push_back(cv::Point3d(471.276, -3.23, -127.809));    

    // Larger data set
    /*imagePoints.push_back(cv::Point2d(482.066, 233.778));
    imagePoints.push_back(cv::Point2d(448.024, 232.038));
    imagePoints.push_back(cv::Point2d(413.895, 230.785));
    imagePoints.push_back(cv::Point2d(380.653, 229.242 ));
    imagePoints.push_back(cv::Point2d(347.983, 227.785));
    imagePoints.push_back(cv::Point2d(316.103, 225.977));
    imagePoints.push_back(cv::Point2d(284.02, 224.905));
    imagePoints.push_back(cv::Point2d(252.929, 223.611));
    imagePoints.push_back(cv::Point2d(483.41, 200.527));
    imagePoints.push_back(cv::Point2d(449.456, 199.406));
    imagePoints.push_back(cv::Point2d(415.843, 197.849));
    imagePoints.push_back(cv::Point2d(382.59, 196.763));
    imagePoints.push_back(cv::Point2d(350.094, 195.616));
    imagePoints.push_back(cv::Point2d(317.922, 194.027));
    imagePoints.push_back(cv::Point2d(286.922, 192.814));
    imagePoints.push_back(cv::Point2d(256.006, 192.022));
    imagePoints.push_back(cv::Point2d(484.292, 167.816));
    imagePoints.push_back(cv::Point2d(450.678, 166.982));
    imagePoints.push_back(cv::Point2d(417.377, 165.961));

    markupsPoints.push_back(cv::Point3d(457.132, 59.822, 89.247));  
    markupsPoints.push_back(cv::Point3d(451.634, 42.015, 69.719));  
    markupsPoints.push_back(cv::Point3d(447.06, 22.927, 48.635));  
    markupsPoints.push_back(cv::Point3d(442.424, 4.454, 28.659));  
    markupsPoints.push_back(cv::Point3d(437.621, -14.395, 7.495));  
    markupsPoints.push_back(cv::Point3d(433.386, -33.034, -12.009));  
    markupsPoints.push_back(cv::Point3d(429.227, -51.001, -32.269));  
    markupsPoints.push_back(cv::Point3d(424.291, -70.266, -52.667));  
    markupsPoints.push_back(cv::Point3d(460.300, 79.769, 69.948));  
    markupsPoints.push_back(cv::Point3d(455.020, 61.379, 49.306));  
    markupsPoints.push_back(cv::Point3d(450.501, 43.288, 30.250));  
    markupsPoints.push_back(cv::Point3d(446.062, 24.572, 8.713));  
    markupsPoints.push_back(cv::Point3d(441.346, 5.823, -10.997));  
    markupsPoints.push_back(cv::Point3d(436.670, -13.135, -31.428));  
    markupsPoints.push_back(cv::Point3d(432.367, -31.428, -51.785));  
    markupsPoints.push_back(cv::Point3d(427.745, -50.016, -72.519));  
    markupsPoints.push_back(cv::Point3d(464.824, 101.129, 52.251));  
    markupsPoints.push_back(cv::Point3d(458.628, 81.864, 30.810));  
    markupsPoints.push_back(cv::Point3d(454.120, 63.546, 10.458));  */


    // Calculate camera pose
    cv::solvePnP(cv::Mat(markupsPoints), cv::Mat(imagePoints), CamMat, DistMat, rvec, tvec);
    cv::Rodrigues(rvec, R);

    // Invert results of Rodrigues by transposing rotation matrix and calculating inverted tvec
    R = R.t();  
    tvec = -R * tvec; // translation of inverse

    std::cout << "Tvec = " << std::endl << tvec << std::endl;

    // Copy R and tvec into the extrinsic matrix
    extrinsic( cv::Range(0,3), cv::Range(0,3) ) = R * 1; 
    extrinsic( cv::Range(0,3), cv::Range(3,4) ) = tvec * 1; 

    // Fill last row of homogeneous transform (0,0,0,1)
    double *p = extrinsic.ptr<double>(3);
    p[0] = p[1] = p[2] = 0; p[3] = 1;

    std::cout << "Extrinsic = " << std::endl << extrinsic << std::endl << std::endl;
    std::cout << "Extrinsic (inv) = " << std::endl << extrinsic.inv() << std::endl;
    std::cin >> tempImage[0];
    return 0;
}

编辑1：我尝试使用Chi Xu的方法（x-cx）/ f，yn =（y-cy）/ f）。没有运气：（

EDIT 1: I tried normalizing the pixel values using Chi Xu's method (xn = (x-cx)/f, yn = (y-cy)/f). No luck :(

编辑2：似乎几乎每个使用solvePnP的人都使用一种方法，他们将棋盘角标记为它们的世界框架和原点的向量，我将尝试将我的跟踪器注册到棋盘，如果这个工作按预期，第一个坐标I标记将大约<0,0>。从solvePnP得到的变换然后将被乘以世界 - to-camera-marker转换，从而产生（希望）外部矩阵。

EDIT 2: As it seems that almost everyone who uses solvePnP uses a method where they mark the checkerboard corners as the vectors for their world frame and origin, I'm going to try registering my tracker to the checkerboard. If this works as expected, the first coordinate I mark will be approximately <0,0>. The resulting transform from solvePnP will then be multiplied by the inverse of the world-to-camera-marker transformation, thus resulting in (hopefully) the extrinsic matrix.

编辑3：我执行了步骤2中描述的步骤。棋盘，我计算了从棋盘空间到世界空间的变换cTw，并在我的渲染环境中验证它，然后计算表示从相机空间到棋盘空间的变换的外在mTc。摄像机标记变换wTr是从最后，我把所有的变换和乘以它们如下，以将我的变换从相机原点移动到相机标记：

EDIT 3: I performed the steps described in step 2. After establishing a coordinate system on the checkerboard, I calculated the transform cTw from the checkerboard space to the world space and verified it in my rendering environment. I then calculated the extrinsic mTc representing the transform from the camera space to the checkerboard space. The camera marker transform wTr was acquired from the tracking system. Finally, I took all of the transforms and multiplied them as follows to move my transformation all the way from camera origin to camera marker:

mTc * cTw * wTr

Lo，看到它给出了完全相同的结果。在这里死亡的任何迹象，我做错了。如果有人有任何线索，我请求你帮助。

Lo and behold it gave the the exact same result. I'm dying here for any sign of what I'm doing wrong. If anyone has any clue, I beg of you to help.

编辑4：今天我意识到，我已经配置了我的棋盘轴，左手坐标系。由于OpenCV使用标准的右手形式操作，因此我决定使用右手配置的棋盘框架轴重试测试。虽然结果大致相同，但我注意到，世界到棋盘格变换现在是一个非标准的变换格式，即3x3旋转矩阵不再大致对称围绕对角线，因为它应该是。这表明我的跟踪系统没有使用右手坐标系统（如果他们记录了这个系统，这将是伟大的，或者任何事情）。虽然我不知道如何解决这个问题，我相信它是问题的一部分。我会继续打击它，希望有人在这里知道该怎么办。我还在OpenCV板上添加了一个图表，由 Eduardo 。感谢Eduardo！

EDIT 4: Today I realized that I had configured my checkerboard axes in such a way that they were in a left-handed coordinate system. Since OpenCV operates using standard right-hand form, I decided to retry the tests with my checkerboard frame axes configured in a right-hand fashion. While the results were about the same, I did notice that the world-to-checkerboard transform was now a non-standard transform format, i.e. the 3x3 rotation matrix was no longer roughly symmetrical around the diagonal as it should be. This indicates that my tracking system is not using a right-hand coordinate system (Would have been great if they had documented that. Or anything, for that matter). While I'm not sure how to resolve this, I am certain that it is part of the issue. I'll keep hammering at it and hope someone here knows what to do. I've also added a diagram provided to me on the OpenCV boards by Eduardo. Thanks Eduardo!

推荐答案

所以我想出了问题。不出所料，它是在实施，而不是理论。当项目最初设计时，我们使用了基于网络摄像头的跟踪器。该跟踪器具有从标记平面出来的z轴。当我们移动到光学跟踪器时，代码被移植大多是原样。不幸的是我们（RIP的2个月的我的生活），我们从来没有检查z轴是否仍然出来的标记平面（它没有）。因此，渲染流水线被分配了错误的视图向上和向外观察向量到场景摄像机。它大多是现在工作，除了翻译是关闭的某些原因。完全不同的问题虽然！

So I figured out the issue. Unsurprisingly, it was in the implementation, not the theory. When the project was initially designed, we utilized a webcam-based tracker. This tracker had the z-axis coming out of the marker plane. When we moved to an optical tracker, the code was ported mostly as-is. Unfortunately for us (RIP 2 months of my life), we never checked if the z-axis still came out of the marker plane (It didn't). So the render pipeline was assigned the wrong view up and view out vectors to the scene camera. It's mostly working now, except that the translations are off for some reason. Entirely different problem though!

这篇关于外部校准使用cv :: SolvePnP的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！