OpenCV: stima 3D Pose degli indicatori di colore utilizzando il sistema StereoCamera

Question

Ho un sistema di telecamere stereo e lo calibro correttamente utilizzando entrambi, cv::calibrateCamera e cv::stereoCalibrate. Il mio reprojection error sembra essere a posto:

• Cam0: 0,401427
• Cam1: 0,388200
• Stereo: 0,399642

controllo il mio calibrazione chiamando cv::stereoRectify e trasformando le mie immagini utilizzando cv::initUndistortRectifyMap e cv::remap. Il risultato è mostrato sotto (Qualcosa di strano che ho notato è quando si visualizzano le immagini raddrizzate di solito ci sono manufatti in forma di una copia deformata dell'immagine originale su uno o talvolta anche entrambe le immagini):

anche io stimare correttamente la posizione dei miei marcatori in coordinate pixel usando cv::findContours su un'immagine HSV con soglia.

Purtroppo, quando io ora cerco di cv::triangulatePoints i miei risultati sono molto scarsa rispetto alle mie coordinate stimate, soprattutto in direzione 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) 

Quelli sono i risultati in millimetri di coordinate della macchina fotografica . Entrambe le telecamere sono posizionate a circa 550 mm dalla scacchiera e le dimensioni quadrate sono 13 mm. Apparentemente, i miei risultati non sono nemmeno vicini a quello che mi aspetto (coordinate z e negative).

Quindi le mie domande sono:

1. ho seguito il campione stereo_calib.cpp abbastanza da vicino e mi sembra di ottenere almeno visivamente buon risultato (vedi errore di riproiezione). Perché i risultati della triangolazione sono così bassi?
2. Come posso trasformare i risultati nel sistema di coordinate del mondo reale in modo da poter controllare i risultati in modo quantitativo? Devo farlo manualmente come mostrato in over here o ci sono alcune funzioni OpenCV?

---

Ecco il mio codice:

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

Infine, resultPoints si suppone che contengono vettori colonna delle mie posizioni 3D in coordinate macchina fotografica.

Edit: ho tolto alcune conversioni non necessari per ridurre il codice

Edit2: I risultati che ottengo utilizzando @marols soluzione suggerita per la triangolazione

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

Answer 1

La mia risposta si concentrerà sul suggerire un'altra soluzione per triangulatePoints. In caso di visione stereo, è possibile utilizzare matrice Q restituito da rettificazione stereo in modo seguente:

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

Si prega di adattarsi seguente frammento di codice, potrei fatto qualche errore, ma il punto è quello di creare un array di cv :: Vec3f, ognuno con la seguente struttura: (point.x, point.y, disparità tra punto sulla seconda immagine) e passarlo al metodo perspectiveTransform (vedere docs per ulteriori dettagli). Se desideri approfondire i dettagli su come viene creata la matrice Q (in pratica rappresenta una proiezione "inversa" da un'immagine a un punto del mondo reale) consulta il libro "Learning OpenCV", pagina 435.

Nella visione stereo Il sistema che ho sviluppato, descritto metodo funziona bene e dà risultati corretti su errori di calibrazione ancora più grandi (come 1.2).

Answer 2

Per proiettare nel sistema di coordinate del mondo reale, è necessaria la matrice della telecamera di proiezione. Questo può essere fatto come:

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