opencv triangulatePoints, strange result - c++

Question

I am using the triagulatePoints function in opencv. Finally I have it working, after much despair, but the results do not look right. I have read some other questions on this, but I still don't understand it!
I am running:
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 and P2 are the calculated extrinsics from the stereo_calib function.
There are 5 points, a rough square with a point in each corner and one in the middle.
The resulting Matrix is:
[-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]
Which, when plotted in 3d, gives two positions that look almost correct, if completely scaled wrong, then three duplicates of those two.
Where am I going wrong here? DO I need to do something to the resulting matrix to get an xyz coordinate? Or have I implemented the function incorrectly?

Answer 1

cancel that, i managed to do it by ignoring the cv::triangulate function and using this method here:
http://www.morethantechnical.com/2012/01/04/simple-triangulation-with-opencv-from-harley-zisserman-w-code/
with a small change to fix some code that was in wrong place...
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;
}
and this:
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;
}