OpenCV: 3D Szacowanie pozycji znaczników kolorów za pomocą systemu StereoCamera

Question

Mam system kamer stereo i prawidłowo skalibruję go przy użyciu obu, cv::calibrateCamera i cv::stereoCalibrate. My reprojection error wydaje się być w porządku:

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

mogę sprawdzić kalibrację wywołując cv::stereoRectify i przekształcając moje obrazy przy użyciu cv::initUndistortRectifyMap i cv::remap. Wynik jest pokazany poniżej (Coś dziwnego zauważyłem jest podczas wyświetlania rektyfikowanego obrazy są zazwyczaj artefakty w postaci zdeformowanej kopią oryginalnego obrazu na jednym lub czasem nawet oba obrazy):

ja również poprawnie oszacuj pozycję moich znaczników we współrzędnych pikseli, używając cv::findContours na progowym obrazie HSV.

Niestety, kiedy już spróbować cv::triangulatePoints moje wyniki są bardzo słabe w porównaniu do moich szacunkowych współrzędnych, zwłaszcza w kierunku 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) 

Są to wyniki w mm w układzie współrzędnych kamery . Obie kamery znajdują się w odległości około 550 mm od szachownicy, a rozmiar kwadratu wynosi 13 mm. Najwyraźniej moje wyniki nie są nawet zbliżone do moich oczekiwań (ujemne i ogromne współrzędne Z).

Więc moje pytania to:

1. Śledziłem próbkę stereo_calib.cpp dość blisko i wydaje mi się, przynajmniej wizualnie uzyskać dobry wynik (patrz błąd ponowne odwzorowanie). Dlaczego moje wyniki triangulacji są tak słabe?
2. W jaki sposób mogę przekształcić moje wyniki w rzeczywisty układ współrzędnych, aby móc właściwie sprawdzić moje wyniki ilościowo? Czy muszę to zrobić ręcznie, jak pokazano na rysunku over here, czy jest tam kilka funkcji OpenCV?

---

Oto mój kod:

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

Wreszcie resultPoints ma zawierać wektory kolumnowe moich pozycjach 3D we współrzędnych kamery.

Edit: usunąłem kilka niepotrzebnych konwersji skrócić kod

Edit2: wyniki I uzyskać stosując @marols sugerowane rozwiązanie dla triangulacji

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

Answer 1

Moja odpowiedź skupi się na sugerowaniu innego rozwiązania triangulacji punktów. W przypadku stereo wizji, można użyć macierz Q zwrócony przez stereo sprostowania w następujący sposób:

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

Proszę dostosowania następujący fragment do kodu, to może ja popełniłem kilka błędów, ale chodzi o to, aby stworzyć tablicę cv :: Vec3f, każdy z nich ma następującą strukturę: (point.x, point.y, rozbieżność między punktami na drugim obrazie) i przekazuje go do metody perspectiveTransform (zobacz docs po więcej szczegółów). Jeśli chcesz dowiedzieć się więcej na temat tworzenia macierzy Q (w zasadzie reprezentuje odwróconą projekcję od obrazu do punktu rzeczywistego) zobacz książkę "Uczenie się OpenCV", strona 435.

W stereo wizji opracowany przeze mnie system, opisana metoda działa dobrze i daje właściwe wyniki przy jeszcze większych błędach kalibracji (jak 1.2).

Answer 2

Do projekcji w rzeczywisty układ współrzędnych potrzebny jest macierz kamery projekcyjnej. Można to zrobić jako:

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