Hello, I am trying to solve a pose estimation problem for a multiple camera system where one camera acts as the origin using OpenCV. So let's assume a case where camera 1 is at (0,0,0) and camera 2 exactly to the left of it by 4 meters; I build the 3D model of the scene using this metric information. Now camera 2 has displaced to the left by another 4 m, so the total is now 8 m, but we consider the change unknown to the code. Now I am running solvePnP on the new set of image points and the previously reconstructed 3D scene in the hopes of getting 8m as the answer: because the triangulation is done with P1 = [I|0], everything that opencv does should be with respect to camera 1, but I am getting extremely bad results. (please note that units are in cm)
CV_EPNP: Translation matrix is [223.1857790065309;
-45.44370476199799;
357.0838060856186]
CV_ITERATIVE: Translation matrix is [142.2985654354176;
1001.722097074433;
-6323.072276118809]
CV_ITERATIVE RANSAC: Translation matrix is [0; 0; 0]
According to essential matrix based estimation, the translation vector turns up as [-0.9999999, 0.000001, 0.000002] etc. so the triangulation is being done properly. Also if I compare the scale of the reconstructed scene in both cases between the two cameras, the ratio of the scale factor is being computed as 0.5, so it should be obvious to OpenCV that the cameras' baseline has doubled. I have checked and rechecked my code and I am at my wits' end, I was hoping someone can throw some light on what might be happening here as I am clearly missing something. I am posting a snippet of my code here for reference. Thanks a lot for your time!
CODE: Assume trainOld and trainNew are from camera 1 (static) from time instant 1 and 2, queryOld and queryNew are from camera 2 that's moving. I take trainNew into account and check for what points are still visible from the old set, and run PnP on the 3D points that are still visible and their corresponding projections in the camera frame. pointsObj[0] is the list of 3D points from time instant 1.
for(unsigned int i = 0; i < (unsigned int)trainOld.size(); i++)
{
vector<Point2d>::iterator iter = find(trainNew.begin(), trainNew.end(), trainOld[i]);
size_t index = distance(trainNew.begin(), iter);
if(index != trainNew.size())
{
P3D.push_back(pointsObj[0][i]);
P2D.push_back(queryNew[index]);
}
}
Mat R, t;
solvePnP(P3D, P2D, K, d, rvec, tvec, false, CV_ITERATIVE);