OpenCV: 使用立体相机系统进行彩色标记的3D姿态估计

Question

我有一个立体相机系统，并使用cv::calibrateCamera和cv::stereoCalibrate进行正确校准。我的重投影误差似乎没问题：

• Cam0: 0.401427
• Cam1: 0.388200
• Stereo: 0.399642

我通过调用cv::stereoRectify检查我的校准，并使用cv::initUndistortRectifyMap和cv::remap转换我的图像。结果如下所示（我注意到的一些奇怪的事情是，当显示矫正后的图像时，通常会在一个或两个图像中出现原始图像的变形副本）： 我还使用阈值HSV图像上的cv::findContours正确估计了标记在像素坐标中的位置。 不幸的是，当我现在尝试cv::triangulatePoints时，我的结果与我的估计坐标相比非常差，特别是在x方向上。

P1 = {   58 (±1),  150 (±1), -90xx (±2xxx)  } (bottom)
P2 = {  115 (±1),  -20 (±1), -90xx (±2xxx)  } (right)
P3 = { 1155 (±6),  575 (±3), 60xxx (±20xxx) } (top-left)

这些是相机坐标系下以毫米为单位的结果。两个相机距离棋盘大约550毫米，方格尺寸为13毫米。显然，我的结果与我预期的相差甚远（z坐标为负且极大）。

所以我的问题是：

1. 我非常密切地遵循了stereo_calib.cpp示例，并且至少在视觉上获得了良好的结果（参见重投影误差）。为什么我的三角测量结果如此糟糕？
2. 如何将我的结果转换到真实世界的坐标系中，以便我可以定量地检查我的结果？我需要像这里所示手动完成吗？还是OpenCV有相关函数可供使用？

---

以下是我的代码：

std::vector<std::vector<cv::Point2f> > imagePoints[2];
std::vector<std::vector<cv::Point3f> > objectPoints;

imagePoints[0].resize(s->nrFrames);
imagePoints[1].resize(s->nrFrames);
objectPoints.resize( s->nrFrames );

// [Obtain image points..]
// cv::findChessboardCorners, cv::cornerSubPix

// Calc obj points
for( int i = 0; i < s->nrFrames; i++ )
    for( int j = 0; j < s->boardSize.height; j++ )
        for( int k = 0; k < s->boardSize.width; k++ )
            objectPoints[i].push_back( Point3f( j * s->squareSize, k * s->squareSize, 0 ) );


// Mono calibration
cv::Mat cameraMatrix[2], distCoeffs[2];
cameraMatrix[0] = cv::Mat::eye(3, 3, CV_64F);
cameraMatrix[1] = cv::Mat::eye(3, 3, CV_64F);

std::vector<cv::Mat> tmp0, tmp1;

double err0 = cv::calibrateCamera( objectPoints, imagePoints[0], cv::Size( 656, 492 ),
    cameraMatrix[0], distCoeffs[0], tmp0, tmp1,    
    CV_CALIB_FIX_PRINCIPAL_POINT + CV_CALIB_FIX_ASPECT_RATIO );
std::cout << "Cam0 reproj err: " << err0 << std::endl;

double err1 = cv::calibrateCamera( objectPoints, imagePoints[1], cv::Size( 656, 492 ),
    cameraMatrix[1], distCoeffs[1], tmp0, tmp1,
    CV_CALIB_FIX_PRINCIPAL_POINT + CV_CALIB_FIX_ASPECT_RATIO );
std::cout << "Cam1 reproj err: " << err1 << std::endl;

// Stereo calibration
cv::Mat R, T, E, F;

double err2 = cv::stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
    cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1],
    cv::Size( 656, 492 ), R, T, E, F,
    cv::TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
    CV_CALIB_USE_INTRINSIC_GUESS + // because of mono calibration
    CV_CALIB_SAME_FOCAL_LENGTH +
    CV_CALIB_RATIONAL_MODEL +
    CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
std::cout << "Stereo reproj err: " << err2 << std::endl;

// StereoRectify
cv::Mat R0, R1, P0, P1, Q;
Rect validRoi[2];
cv::stereoRectify( cameraMatrix[0], distCoeffs[0],
            cameraMatrix[1], distCoeffs[1],
            cv::Size( 656, 492 ), R, T, R0, R1, P0, P1, Q,
            CALIB_ZERO_DISPARITY, 1, cv::Size( 656, 492 ), &validRoi[0], &validRoi[1]);


// [Track marker..]
// cv::cvtColor, cv::inRange, cv::morphologyEx, cv::findContours, cv::fitEllipse, *calc ellipsoid centers*

// Triangulation
unsigned int N = centers[0].size();


cv::Mat pnts3D;
cv::triangulatePoints( P0, P1, centers[0], centers[1], pnts3D );


cv::Mat t = pnts3D.t();
cv::Mat pnts3DT = cv::Mat( N, 1, CV_32FC4, t.data );

cv::Mat resultPoints; 
cv::convertPointsFromHomogeneous( pnts3DT, resultPoints );

最后，resultPoints 应该包含我的相机坐标系下的三维位置的列向量。

编辑：我删除了一些不必要的转换以缩短代码

编辑2：使用@marols建议的三角测量方法得到的结果如下图所示：

P1 = { 111,  47, 526 } (bottom-right)
P2 = {  -2,   2, 577 } (left)
P3 = {  64, -46, 616 } (top)

- pdresselhaus

Answer 1

我的回答将着重于建议triangulatePoints的另一种解决方案。在立体视觉的情况下，您可以使用立体校正返回的矩阵Q以以下方式进行：

std::vector<cv::Vec3f> surfacePoints, realSurfacePoints;

unsigned int N = centers[0].size();
for(int i=0;i<N;i++) {
    double d, disparity;
    // since you have stereo vision system in which cameras lays next to 
    // each other on OX axis, disparity is measured along OX axis
    d = T.at<double>(0,0);
    disparity = centers[0][i].x - centers[1][i].x;

    surfacePoints.push_back(cv::Vec3f(centers[0][i].x, centers[0][i].y, disparity));
}

cv::perspectiveTransform(surfacePoints, realSurfacePoints, Q);

请将以下代码片段适应到您的代码中，我可能会犯一些错误，但重点是创建一个 cv::Vec3f 数组，每个数组元素都具有以下结构：(point.x, point.y, 第二张图像上的点之间的视差)，并将其传递给 perspectiveTransform 方法（有关更多详细信息，请参见文档）。如果您想深入了解矩阵 Q 的创建方式（基本上它表示从图像到真实世界点的“反向”投影），请参阅《学习 OpenCV》书籍第 435 页。

在我开发的立体视觉系统中，所描述的方法运行良好，即使校准误差较大（如 1.2），也能得到正确的结果。

- marol

---

谢谢，你的解决方案非常有效！不过，我还想看看关于我的第二个问题的一些想法。你有什么办法可以轻松地转换为真实世界坐标吗？ - pdresselhaus

@Random-I-Am提供的代码片段返回的值是世界坐标系中的点。据我所知，cv::triangulatePoints()函数返回的是齐次坐标系下的点(x,y,z,w)，因此你只需要将所有坐标除以w即可得到世界坐标系下的点(x/w, y/w, z/w)。 - marol

现在这可能取决于“世界坐标”的定义。我已经更新了我的问题，并提供了使用您建议的解决方案得到的结果。我得到的值是左侧相机的“相机坐标”（尽管我不明白为什么将点移动到图像顶部时，“y”会减少）。我想将这些坐标转换为“对象坐标”。例如，当在我的棋盘的左上角特征上方放置一个标记时，我期望得到的结果是P = {0,0,0}。 - pdresselhaus

既然您有一个国际象棋棋盘，您可以相当容易地估计它的姿态。您只需检测您的棋盘，然后找到其4个角点的3D位置。通过一些简单的几何学，您可以很容易地定义一个附加到棋盘左上角的框架，并将您的标记3D位置与该棋盘平面中的该框架进行比较。唯一的问题是，这种方法存在歧义，因为您的棋盘具有2个对称轴。 - Ben

@Random-I-Am：我认为Ben的评论非常正确。因此，使用我的答案，您可以找到3D位置，然后使用简单的数学方法来完成其余部分。我认为这就是您第二个问题的答案。 - marol

Answer 2

为了将物体投影到真实世界的坐标系中，您需要使用投影相机矩阵。具体操作如下：

cv::Mat KR = CalibMatrix * R;
cv::Mat eyeC = cv::Mat::eye(3,4,CV_64F);
eyeC.at<double>(0,3) = -T.at<double>(0);
eyeC.at<double>(1,3) = -T.at<double>(1);
eyeC.at<double>(2,3) = -T.at<double>(2);

CameraMatrix = cv::Mat(3,4,CV_64F);
CameraMatrix.at<double>(0,0) = KR.at<double>(0,0) * eyeC.at<double>(0,0) + KR.at<double>(0,1) * eyeC.at<double>(1,0) + KR.at<double>(0,2) * eyeC.at<double>(2,0);
CameraMatrix.at<double>(0,1) = KR.at<double>(0,0) * eyeC.at<double>(0,1) + KR.at<double>(0,1) * eyeC.at<double>(1,1) + KR.at<double>(0,2) * eyeC.at<double>(2,1);
CameraMatrix.at<double>(0,2) = KR.at<double>(0,0) * eyeC.at<double>(0,2) + KR.at<double>(0,1) * eyeC.at<double>(1,2) + KR.at<double>(0,2) * eyeC.at<double>(2,2);
CameraMatrix.at<double>(0,3) = KR.at<double>(0,0) * eyeC.at<double>(0,3) + KR.at<double>(0,1) * eyeC.at<double>(1,3) + KR.at<double>(0,2) * eyeC.at<double>(2,3);
CameraMatrix.at<double>(1,0) = KR.at<double>(1,0) * eyeC.at<double>(0,0) + KR.at<double>(1,1) * eyeC.at<double>(1,0) + KR.at<double>(1,2) * eyeC.at<double>(2,0);
CameraMatrix.at<double>(1,1) = KR.at<double>(1,0) * eyeC.at<double>(0,1) + KR.at<double>(1,1) * eyeC.at<double>(1,1) + KR.at<double>(1,2) * eyeC.at<double>(2,1);
CameraMatrix.at<double>(1,2) = KR.at<double>(1,0) * eyeC.at<double>(0,2) + KR.at<double>(1,1) * eyeC.at<double>(1,2) + KR.at<double>(1,2) * eyeC.at<double>(2,2);
CameraMatrix.at<double>(1,3) = KR.at<double>(1,0) * eyeC.at<double>(0,3) + KR.at<double>(1,1) * eyeC.at<double>(1,3) + KR.at<double>(1,2) * eyeC.at<double>(2,3);
CameraMatrix.at<double>(2,0) = KR.at<double>(2,0) * eyeC.at<double>(0,0) + KR.at<double>(2,1) * eyeC.at<double>(1,0) + KR.at<double>(2,2) * eyeC.at<double>(2,0);
CameraMatrix.at<double>(2,1) = KR.at<double>(2,0) * eyeC.at<double>(0,1) + KR.at<double>(2,1) * eyeC.at<double>(1,1) + KR.at<double>(2,2) * eyeC.at<double>(2,1);
CameraMatrix.at<double>(2,2) = KR.at<double>(2,0) * eyeC.at<double>(0,2) + KR.at<double>(2,1) * eyeC.at<double>(1,2) + KR.at<double>(2,2) * eyeC.at<double>(2,2);
CameraMatrix.at<double>(2,3) = KR.at<double>(2,0) * eyeC.at<double>(0,3) + KR.at<double>(2,1) * eyeC.at<double>(1,3) + KR.at<double>(2,2) * eyeC.at<double>(2,3);

- madduci