OpenCV：使用StereoCamera系統進行色彩標記的3D姿態估計

Question

我有一個立體攝像頭系統，並使用它們正確校準，分別使用cv::calibrateCamera和cv::stereoCalibrate。我reprojection error似乎是好的：

• 的Cam0：0.401427
• 段1：0.388200
• 立體聲：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. 如何將結果轉換爲實際座標系，以便實際檢查我的結果？我是否必須手動執行，如over here所示，或者是否有一些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應該包含在攝像機座標我的3D位置的列向量。

編輯：我刪除一些不必要的轉換，縮短了代碼

EDIT2：結果我開始使用@marols建議的解決方案爲三角

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

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); 

請適應下面的代碼片段到你的代碼，我可能犯了一些錯誤，但關鍵是要建立簡歷的數組:: Vec3f，它們每個都具有以下結構：（point.x，point.y，第二個圖像上的點之間的不一致）並將其傳遞給perspectiveTransform方法（有關更多詳細信息，請參見docs）。如果您想深入瞭解如何創建矩陣Q的細節（基本上它代表從圖像到現實世界點的「反向」投影），請參閱「學習OpenCV」一書，第435頁。

在立體視覺我開發的系統，描述的方法工作正常，並在更大的校準誤差（如1.2）上給出適當的結果。

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);