opencv triangulatePoints, странный результат

Question

Я использую функцию triagulatePoints в opencv. Наконец, у меня есть работа, после долгого отчаяния, но результаты не выглядят правильно. Я прочитал некоторые другие вопросы по этому поводу, но я до сих пор этого не понимаю!

Я бегу:

cv::Mat Q,P1,P2,LP,RP,D1,D2,M1,M2; 
    char *CALIB_FILE = (char *)"extrinsics.xml"; 
    FileStorage fs(CALIB_FILE, FileStorage::READ); 
    fs["Q"] >> Q; 
    fs["P1"] >> P1; 
    fs["P2"] >> P2; 


    cv::Mat cam0pnts(1, 5, CV_64FC2); //681 432 479 419 550 320 682 274 495 254 
    cv::Mat cam1pnts(1, 5, CV_64FC2); //800 466 587 451 657 352 791 311 592 283 

    cv::Mat points_3D(1, 5, CV_64FC4); 


    cv::triangulatePoints(P1, P2, cam0pnts, cam1pnts, points_3D); 

P1 и P2 рассчитанные extrinsics из функции stereo_calib.

Есть 5 баллов, грубый квадрат с точкой в ​​каждом углу и один посередине.

В результате Матрица:

[-0.6620691274599629, 0.6497615623177577, -0.6585234150236594, 0.6529909432980171, -0.6604373884239706; 
    -0.7091492226203088, 0.7208075295879011, -0.7119285643550911, 0.7174438199266364, -0.710244308941275; 
    0.242429054072024, -0.2413429417514131, 0.2439357048056051, -0.2426462227979475, 0.2436708320163396; 
    -6.52928664505207e-005, -4.348043360405063e-005, -5.515313727475824e-005, -6.149577656504346e-005, -5.668087253108842e-005] 

Который, при построении графика в 3d, дает две позиции, которые выглядят почти правильно, если полностью масштабируются неправильно, то три дубликата этих двух.

В каком месте я ошибаюсь? Я должен что-то сделать с полученной матрицей, чтобы получить координату xyz? Или я неправильно выполнил эту функцию?

Answer 1

отменить, что я сумел сделать это, не обращая внимания на функцию резюме :: триангуляции и используя этот метод здесь:

http://www.morethantechnical.com/2012/01/04/simple-triangulation-with-opencv-from-harley-zisserman-w-code/

с небольшим изменением, чтобы установить некоторый код, который был в неправильном месте .. .

Mat_<double> IterativeLinearLSTriangulation(Point3d u, //homogenous image point (u,v,1) 
    Matx34d P,   //camera 1 matrix 
    Point3d u1,   //homogenous image point in 2nd camera 
    Matx34d P1   //camera 2 matrix 
    ) { 

    double wi = 1, wi1 = 1; 
    Mat_<double> X(4, 1); 



    for (int i = 0; i < 10; i++) { //Hartley suggests 10 iterations at most 
     Mat_<double> X_ = LinearLSTriangulation(u, P, u1, P1); 
    X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X(3) = 1.0; 
     //recalculate weights 
     double p2x = Mat_<double>(Mat_<double>(P).row(2)*X)(0); 
     double p2x1 = Mat_<double>(Mat_<double>(P1).row(2)*X)(0); 

     //breaking point 
     if (fabsf(wi - p2x) <= EPSILON && fabsf(wi1 - p2x1) <= EPSILON) break; 

     wi = p2x; 
     wi1 = p2x1; 

     //reweight equations and solve 
     Matx43d A((u.x*P(2, 0) - P(0, 0))/wi, (u.x*P(2, 1) - P(0, 1))/wi, (u.x*P(2, 2) - P(0, 2))/wi, 
      (u.y*P(2, 0) - P(1, 0))/wi, (u.y*P(2, 1) - P(1, 1))/wi, (u.y*P(2, 2) - P(1, 2))/wi, 
      (u1.x*P1(2, 0) - P1(0, 0))/wi1, (u1.x*P1(2, 1) - P1(0, 1))/wi1, (u1.x*P1(2, 2) - P1(0, 2))/wi1, 
      (u1.y*P1(2, 0) - P1(1, 0))/wi1, (u1.y*P1(2, 1) - P1(1, 1))/wi1, (u1.y*P1(2, 2) - P1(1, 2))/wi1 
      ); 
     Mat_<double> B = (Mat_<double>(4, 1) << -(u.x*P(2, 3) - P(0, 3))/wi, 
      -(u.y*P(2, 3) - P(1, 3))/wi, 
      -(u1.x*P1(2, 3) - P1(0, 3))/wi1, 
      -(u1.y*P1(2, 3) - P1(1, 3))/wi1 
      ); 

     solve(A, B, X_, DECOMP_SVD); 
     X(0) = X_(0); X(1) = X_(1); X(2) = X_(2); X(3) = 1.0; 
    } 

    return X; 
} 

и это:

Mat_<double> LinearLSTriangulation(Point3d u,  //homogenous image point (u,v,1) 
    Matx34d P,  //camera 1 matrix 
    Point3d u1,  //homogenous image point in 2nd camera 
    Matx34d P1  //camera 2 matrix 
    ) 
{ 
    //build matrix A for homogenous equation system Ax = 0 
    //assume X = (x,y,z,1), for Linear-LS method 
    //which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1 
    Matx43d A(u.x*P(2, 0) - P(0, 0), u.x*P(2, 1) - P(0, 1), u.x*P(2, 2) - P(0, 2), 
     u.y*P(2, 0) - P(1, 0), u.y*P(2, 1) - P(1, 1), u.y*P(2, 2) - P(1, 2), 
     u1.x*P1(2, 0) - P1(0, 0), u1.x*P1(2, 1) - P1(0, 1), u1.x*P1(2, 2) - P1(0, 2), 
     u1.y*P1(2, 0) - P1(1, 0), u1.y*P1(2, 1) - P1(1, 1), u1.y*P1(2, 2) - P1(1, 2) 
     ); 
    Mat_<double> B = (Mat_<double>(4, 1) << -(u.x*P(2, 3) - P(0, 3)), 
     -(u.y*P(2, 3) - P(1, 3)), 
     -(u1.x*P1(2, 3) - P1(0, 3)), 
     -(u1.y*P1(2, 3) - P1(1, 3))); 

    Mat_<double> X; 
    solve(A, B, X, DECOMP_SVD); 

    return X; 
}