3D reconstruction from multiple images with one camera - c++

Question

So, I've been trying to get a 3D cloud point from a sequence of images of an object. I have successfully obtained a decent point cloud with two images. I got that from matching features on both images, finding the fundamental matrix and from that, extracting P' (the camera matrix for the second view). For the first view, I set P = K(I | 0), where K is the matrix for the camera intrinsics. But I haven't been able to extend this approach to several images. My idea was to do this sliding the two image window through the sequence of images(e.g. match image1 with image2, find 3d points, match image2 with image3 and then find the more 3d points, and so on). For the following image pairs, P would be made of a cumulative rotation matrix and a cumulative translation vector (this would allow me to keep bringing the points to the first camera coordinate system). But this is not working at all. I'm using OpenCV. What I wanna know is if this approach makes sense at all.
In the code, P_prev is P and Pl is P'. This is just the part that I think it's relevant.
Mat combinedPointCloud;
Mat P_prev;
P_prev = (Mat_<double>(3,4) << cameraMatrix.at<double>(0,0), cameraMatrix.at<double>(0,1), cameraMatrix.at<double>(0,2), 0,
cameraMatrix.at<double>(1,0), cameraMatrix.at<double>(1,1), cameraMatrix.at<double>(1,2), 0,
cameraMatrix.at<double>(2,0), cameraMatrix.at<double>(2,1), cameraMatrix.at<double>(2,2), 0);
for(int i = 1; i < images.size(); i++) {
Mat points3D;
image1 = images[i-1];
image2 = images[i];
matchTwoImages(image1, image2, imgpts1, imgpts2);
P = findSecondProjectionMatrix(cameraMatrix, imgpts1, imgpts2);
P.col(0).copyTo(R.col(0));
P.col(1).copyTo(R.col(1));
P.col(2).copyTo(R.col(2));
P.col(3).copyTo(t.col(0));
if(i == 1) {
Pl = P;
triangulatePoints(P_prev, Pl, imgpts1, imgpts2, points3D); //points3D is 4xN
//Transforming to euclidean by hand, because couldn't make
// opencv's convertFromHomogeneous work
aux.create(3, points3D.cols, CV_64F);// aux is 3xN
for(int i = 0; i < points3D.cols; i++) {
aux.at<float>(0, i) = points3D.at<float>(0, i)/points3D.at<float>(3, i);
aux.at<float>(1, i) = points3D.at<float>(1, i)/points3D.at<float>(3, i);
aux.at<float>(2, i) = points3D.at<float>(2, i)/points3D.at<float>(3, i);
}
points3D.create(3, points3D.cols, CV_64F);
aux.copyTo(points3D);
}
else {
R_aux = R_prev * R;
t_aux = t_prev + t;
R_aux.col(0).copyTo(Pl.col(0));
R_aux.col(1).copyTo(Pl.col(1));
R_aux.col(2).copyTo(Pl.col(2));
t_aux.col(0).copyTo(Pl.col(3));
triangulatePoints(P_prev, Pl, imgpts1, imgpts2, points3D);
//Transforming to euclidean by hand, because couldn't make
// opencv's convertFromHomogeneous work
aux.create(3, points3D.cols, CV_64F);// aux is 3xN
for(int i = 0; i < points3D.cols; i++) {
aux.at<float>(0, i) = points3D.at<float>(0, i)/points3D.at<float>(3, i);
aux.at<float>(1, i) = points3D.at<float>(1, i)/points3D.at<float>(3, i);
aux.at<float>(2, i) = points3D.at<float>(2, i)/points3D.at<float>(3, i);
}
points3D.create(3, points3D.cols, CV_64F);
aux.copyTo(points3D);
}
Pl.col(0).copyTo(R_prev.col(0));
Pl.col(1).copyTo(R_prev.col(1));
Pl.col(2).copyTo(R_prev.col(2));
Pl.col(3).copyTo(t_prev.col(0));
P_prev = Pl;
if(i==1) {
points3D.copyTo(combinedPointCloud);
} else {
hconcat(combinedPointCloud, points3D, combinedPointCloud);
}
}
show3DCloud(comninedPointCloud);