OPENCV Realization SFM (III): Multi-mesh three-dimensional reconstruction

Note: The code in this article must be compiled with OpenCV3.0 or later, because many functions are added after 3.0.
Directory:

• • Problem simplification
  • Find the transformation matrix of the third camera
  • Add more images
  • Code implementation
  • Test
  • Thinking
  • Download

Problem simplification

Finally has the time to pits, this one breath expands the binocular reconstruction to the multi-eye reconstruction. First of all, to simplify the problem, we have to make an important assumption: the image used for multi-mesh reconstruction is orderly, that is, the location of the adjacent image is adjacent to the shooting. Multi-purpose reconstruction itself is more complex, I will try to speak clearly, if there is unclear place, please forgive and welcome the question.

Find the transformation matrix of the third camera

From the previous article we know that the transformation matrix between the two cameras can be achieved by Findessentialmat and the Recoverpose function, set the first camera's coordinate system as a world coordinate system, and now add the third image (camera), How to determine the third camera (later called Camera three) to the world coordinate system transformation matrix?

The simplest idea is to use the binocular reconstruction method, which extracts the feature points between the third image and the first image, and then calls Findessentialmat and Recoverpose. So join the fourth, fifth, and even more? With the increase of the number of images, the difference between the newly added image and the first image may be more and more large, the extraction of feature points becomes extremely difficult, and the method of binocular reconstruction can not be used.

So can we use the newly added image and the adjacent image for feature matching? For example, the third and the second match, the fourth and the third match, and so on. Of course, but you can not continue to use Findessentialmat and recoverpose to find the camera's transformation matrix, because these two functions to find a relative transformation, such as camera three to camera two transformation, and we need is camera three to camera one transformation. Some people say, now that you know the camera two to camera a transformation, but also know the camera to three to the camera two transformation, can not be able to find the camera three to the camera one transformation? In fact, in this way, you can only find the camera three to the camera one rotation transformation (rotation matrix R), and their displacement vector T, is impossible to find out. This is because the above two function to find the displacement vector, are the unit vector, lost the proportional relationship between the displacement of the camera.

Having said so much, how are we going to solve these problems? Now please out the protagonist of this article--solvepnp and Solvepnpransac. According to OpenCV's official explanation, the function solves the position of the camera in space according to the correspondence between the points in the space and the points in the image. That is, I know some of the coordinates of the dots in the space, and I know the pixel coordinates of the dots in the image, so solvepnp can tell me the coordinates of the camera in space. The SOLVEPNP and Solvepnpransac have the same functionality, except that the latter uses random consistent sampling to make it more robust to noise, which is used in this article.

Well, how about a good function for our three-dimensional reconstruction? First of all, using binocular reconstruction method, the two images are reconstructed, so that some space points, add the third image, so that it and the second image of the feature matching, these matching points, there must be a part of image two and image one of the matching point, that is, The spatial coordinates of these matches are known, and the pixel coordinates of the points in the third image, and, well, the information required for the SOLVEPNP is available, and the space position of the third camera is naturally found. Since the coordinates of the spatial point are all in the world coordinate system (i.e. the coordinate system of the first camera), the camera position obtained by SOLVEPNP is also in the world coordinate system, that is, the transformation matrix of camera three to camera one.

Add more images

Using the above method to get the transformation matrix of camera three, we can use the Triangulatepoints method mentioned in the previous article to triangulate the matching point between image three and image two, and get its spatial coordinates. In order for the subsequent images to be able to solve the transformation matrix using the above method, we also need to merge the newly obtained spatial points with the previous three-dimensional point clouds. Existing space points, there is no need to add, only add in the image of two and three matches, but in the image one and the image of the three points do not match. So repeated.

To facilitate the convergence of point clouds and future expansions, we need to store the corresponding points in the space for each feature point in the image. In the code I used a two-dimensional list, named Correspond_struct_idx,correspond_struct_idx[i][j], which represents the index of the space point in the point cloud for the J feature point of the image of section I, if the index is less than 0, Indicates that the feature point has no corresponding point in space. With this structure, QUERYIDX and trainidx in feature matching can query the location of a feature point in space.

Code implementation

Many of the previous articles do not have to be modified, but can continue to use, but the process of the program has changed greatly. The first is to initialize the point cloud, that is, the first two images of the image sequence are reconstructed by the binocular reconstruction method, and the CORRESPOND_STRUCT_IDX is initialized.

voidInit_structure (Mat K, vector<vector<KeyPoint>>& Key_points_for_all, vector<vector<Vec3b>>& Colors_for_all, vector<vector<DMatch>>& Matches_for_all, vector<Point3f>& Structure, vector<vector<int>>& Correspond_struct_idx, vector<Vec3b>& Colors, vector<Mat>& Rotations, vector<Mat>& Motions) {//Calculate the transformation matrix between the first two images     vector<Point2f>P1, p2; vector<Vec3b>C2; Mat R, T;//rotation matrix and shift vectorMat Mask;points greater than 0 in//mask represent match points, equals zero represents mismatch pointsGet_matched_points (key_points_for_all[0], key_points_for_all[1], matches_for_all[0], p1, p2); Get_matched_colors (colors_for_all[0], colors_for_all[1], matches_for_all[0], colors, C2); Find_transform (K, p1, p2, R, T, mask);//three-dimensional reconstruction of two imagesMaskout_points (P1, mask);    Maskout_points (P2, mask);    Maskout_colors (colors, mask); Mat R0 = Mat::eye (3,3, CV_64FC1); Mat T0 = Mat::zeros (3,1, CV_64FC1); Reconstruct (K, R0, T0, R, T, p1, p2, structure);//Save transformation matrixrotations = {R0, R}; Motions = {T0, T};//Initialize the size of the CORRESPOND_STRUCT_IDX to exactly the same as the Key_points_for_allCorrespond_struct_idx.clear (); Correspond_struct_idx.resize (Key_points_for_all.size ()); for(inti =0; I < key_points_for_all.size (); ++i) {correspond_struct_idx[i].resize (Key_points_for_all[i].size (),-1); }//Fill in the structure index of the first two images    intIDX =0; vector<DMatch>& matches = matches_for_all[0]; for(inti =0; I < matches.size (); ++i) {if(mask.at<uchar> (i) = =0)Continue; correspond_struct_idx[0][MATCHES[I].QUERYIDX] = idx; correspond_struct_idx[1][MATCHES[I].TRAINIDX] = idx;    ++idx; }}

Once the initial point cloud is obtained, the remaining images can be reconstructed incrementally, noting that the feature matching between all the images is completed in advance in the code for ease of implementation and is saved in the Matches_for_all list. The key to incremental reconstruction is to call SOLVEPNPRANSAC, and this function requires space point coordinates and corresponding pixel coordinates as parameters, with CORRESPOND_STRUCT_IDX, it is convenient to find the corresponding relationship, as follows.

voidGet_objpoints_and_imgpoints ( vector<DMatch>& Matches, vector<int>& Struct_indices, vector<Point3f>& Structure, vector<KeyPoint>& Key_points, vector<Point3f>& Object_points, vector<Point2f>& image_points) {object_points.clear (); Image_points.clear (); for(inti =0; I < matches.size (); ++i) {intQuery_idx = Matches[i].queryidx;intTrain_idx = Matches[i].trainidx;intStruct_idx = Struct_indices[query_idx];if(Struct_idx <0)Continue;        Object_points.push_back (Structure[struct_idx]);    Image_points.push_back (key_points[train_idx].pt); }}

After calling Solvepnpransac to get the rotation vectors and displacements of the camera, since we are using a rotation matrix, we call OpenCV's Rodrigues function to transform the rotation vector into a rotation matrix. After that, the matching points were reconstructed (triangulated) using the reconstruct function used in the previous article, but some simple modifications were made for the purpose of multi-mesh reconstruction.

voidReconstruct (mat& K, mat& R1, mat& T1, mat& R2, mat& T2, vector<Point2f>& P1, vector<Point2f>& P2, vector<Point3f>& structure) {//two camera projection matrix [R t],triangulatepoints only supports float typeMat proj1 (3,4, CV_32FC1); Mat Proj2 (3,4, CV_32FC1); R1.convertto (Proj1 (Range (0,3), Range (0,3)), CV_32FC1); T1.convertto (Proj1.col (3), CV_32FC1); R2.convertto (Proj2 (Range (0,3), Range (0,3)), CV_32FC1); T2.convertto (Proj2.col (3), CV_32FC1);    Mat FK;    K.convertto (FK, CV_32FC1);    proj1 = fk*proj1; Proj2 = Fk*proj2;//Triangular reconstructionMat s;    Triangulatepoints (proj1, Proj2, p1, p2, s);    Structure.clear (); Structure.reserve (S.cols); for(inti =0; i < S.cols; ++i) {mat_<float> col = s.col (i); Col/= Col (3);//homogeneous coordinates, dividing the last element to be the true coordinate valueStructure.push_back (point3f (Col (0), Col (1), Col (2))); }}

Finally, the reconstructed structure is fused with the previous point cloud.

voidFusion_structure ( vector<DMatch>& Matches, vector<int>& Struct_indices, vector<int>& Next_struct_indices, vector<Point3f>& Structure, vector<Point3f>& Next_structure, vector<Vec3b>& Colors, vector<Vec3b>& Next_colors) { for(inti =0; I < matches.size (); ++i) {intQuery_idx = Matches[i].queryidx;intTrain_idx = Matches[i].trainidx;intStruct_idx = Struct_indices[query_idx];if(Struct_idx >=0)//If the point is already present in space, then the spatial point corresponding to the match point should be the same, the index should be the same{Next_struct_indices[train_idx] = Struct_idx;Continue; }//If the point already exists in space, add the point to the structure, and the spatial point index of the matching point is the index of the newly added pointStructure.push_back (Next_structure[i]);        Colors.push_back (Next_colors[i]); STRUCT_INDICES[QUERY_IDX] = Next_struct_indices[train_idx] = structure.size ()-1; }}

The code to reconstruct the image throughout the incremental way is roughly the following.

//initialization structure (three-dimensional point cloud)Init_structure (K, Key_points_for_all, Colors_for_all, Matches_for_all, structure, CORRESPOND_STRUCT_IDX , colors, rotations, motions);//Incrementally reconstruct the remaining images for(inti =1; I < matches_for_all.size (); ++i) { vector<Point3f>object_points; vector<Point2f>image_points; Mat R, R, T;//mat Mask;    //Get the three-dimensional points corresponding to the matching points in the image of I, and the corresponding pixels in the i+1 image .Get_objpoints_and_imgpoints (Matches_for_all[i], correspond_struct_idx[i], structure, KEY_PO ints_for_all[i+1], object_points, image_points);//Solve transformation matrixSolvepnpransac (object_points, image_points, K, Noarray (), R, T);//Convert a rotation vector to a rotation matrixRodrigues (R, R);//Save transformation matrixRotations.push_back (R); Motions.push_back (T); vector<Point2f>P1, p2; vector<Vec3b>C1, C2; Get_matched_points (Key_points_for_all[i], key_points_for_all[i +1], Matches_for_all[i], p1, p2); Get_matched_colors (Colors_for_all[i], colors_for_all[i +1], Matches_for_all[i], C1, C2);//three-dimensional reconstruction based on previously obtained r,t     vector<Point3f>Next_structure; Reconstruct (K, rotations[i], motions[i], R, T, p1, p2, next_structure);//Integrate new reconstruction results with previousFusion_structure (Matches_for_all[i], correspond_struct_idx[i], Correspond_struct_idx[i +1], structure, next_structure, colors, C1);}

Test

I tested it with eight images, which are ordered as required in the simplification of the problem.

Most of the time the program spends on feature extraction and matching, and the real reconstruction process takes little time. The final result is as follows.

Each color coordinate system in the diagram represents a camera.

Thinking

• This multi-purpose three-dimensional reconstruction program, requires that the image must be ordered, if the image is unordered, such as just a certain target at different angles of random shooting, how should the program be modified?
• Incremental three-dimensional reconstruction method, there is a big drawback-as the image continues to increase, the error will accumulate, the final error is too big to completely deviate from the goal of reconstruction, how to solve?

Interested readers can think about the above two issues, the second problem is difficult, I will explain in detail in the next article.

Download

The program is developed using VS2015, the OPENCV version is 3.1 and contains the extensions, and if you do not use the SIFT feature, you can modify the source code and then use a library that does not contain the extensions section. After the software runs, the results of the three-dimensional reconstruction are written into the Structure.yml file in the viewer directory, there is a sfmviewer program in the Viewer directory, the Yml file is read directly and the three-dimensional structure is displayed.

Code

OPENCV Realization SFM (III): Multi-mesh three-dimensional reconstruction