1+ #include " opencv_all.hpp"
2+ #include " cvsba.h"
3+
4+ int main (void )
5+ {
6+ double camera_focal = 1000 ;
7+ cv::Point2d camera_center (320 , 240 );
8+ int n_views = 5 ;
9+
10+ // Load multiple views of 'box.xyz'
11+ // c.f. You need to run 'image_generation.cpp' to generate point observation.
12+ // You can apply Gaussian noise by change value of 'camera_noise' if necessay.
13+ std::vector<std::vector<cv::Point2d> > xs;
14+ for (int i = 0 ; i < n_views; i++)
15+ {
16+ FILE* fin = fopen (cv::format (" image_generation%d.xyz" c_str (), " rt"
17+ if (fin == NULL ) return -1 ;
18+ std::vector<cv::Point2d> pts;
19+ while (!feof (fin))
20+ {
21+ double x, y, w;
22+ if (fscanf (fin, " %lf %lf %lf" 3 )
23+ pts.push_back (cv::Point2d (x, y));
24+ }
25+ fclose (fin);
26+ xs.push_back (pts);
27+ if (xs.front ().size () != xs.back ().size ()) return -1 ;
28+ }
29+ std::vector<int > visible_all (xs.front ().size (), 1 );
30+ std::vector<std::vector<int > > visibility (n_views, visible_all);
31+
32+ // Prepare each camera projection matrix
33+ std::vector<cv::Mat> Ks, dist_coeffs, Rs, ts;
34+ cv::Mat K = (cv::Mat_<double >(3 , 3 ) << camera_focal, 0 , camera_center.x , 0 , camera_focal, camera_center.y , 0 , 0 , 1 );
35+ Ks.resize (n_views, K); // K for all cameras
36+ dist_coeffs.resize (n_views, cv::Mat::zeros (5 , 1 , CV_64F)); // dist_coeff for all cameras
37+ Rs.push_back (cv::Mat::eye (3 , 3 , CV_64F)); // R for the first camera (index: 0)
38+ ts.push_back (cv::Mat::zeros (3 , 1 , CV_64F)); // t for the first camera (index: 0)
39+
40+ // Esitmate relative pose of the inital two views
41+ cv::Mat F = cv::findFundamentalMat (xs[0 ], xs[1 ], cv::FM_8POINT);
42+ cv::Mat E = K.t () * F * K;
43+ cv::Mat R, t;
44+ cv::recoverPose (E, xs[0 ], xs[1 ], K, R, t);
45+ Rs.push_back (R); // R for the second camera
46+ ts.push_back (t); // t for the second camera
47+
48+ // Reconstruct the initial 3D points of 'box.xyz' (triangulation)
49+ cv::Mat P0 = K * cv::Mat::eye (3 , 4 , CV_64F);
50+ cv::Mat Rt, X;
51+ cv::hconcat (R, t, Rt);
52+ cv::Mat P1 = K * Rt;
53+ cv::triangulatePoints (P0, P1, xs[0 ], xs[1 ], X);
54+ std::vector<cv::Point3d> Xs;
55+ X.row (0 ) = X.row (0 ) / X.row (3 );
56+ X.row (1 ) = X.row (1 ) / X.row (3 );
57+ X.row (2 ) = X.row (2 ) / X.row (3 );
58+ X.row (3 ) = 1 ;
59+ for (int c = 0 ; c < X.cols ; c++)
60+ Xs.push_back (cv::Point3d (X.at <double >(0 , c), X.at <double >(1 , c), X.at <double >(2 , c)));
61+
62+ // Estimate the initial relative pose of other views (PnP)
63+ for (int i = 2 ; i < n_views; i++)
64+ {
65+ cv::Mat rvec;
66+ cv::solvePnP (Xs, xs[i], Ks[i], dist_coeffs[i], rvec, t);
67+ cv::Rodrigues (rvec, R);
68+ Rs.push_back (R); // R for the third and other cameras
69+ ts.push_back (t); // t for the third and other cameras
70+ }
71+
72+ // Optimize camera pose and 3D points (bundle adjustment)
73+ try
74+ {
75+ cvsba::Sba sba;
76+ cvsba::Sba::Params param;
77+ param.type = cvsba::Sba::MOTIONSTRUCTURE;
78+ param.fixedIntrinsics = 5 ;
79+ param.fixedDistortion = 5 ;
80+ param.verbose = true ;
81+ sba.setParams (param);
82+ double error = sba.run (Xs, xs, visibility, Ks, Rs, ts, dist_coeffs);
83+ }
84+ catch (cv::Exception) { }
85+
86+ // Store the 3D points
87+ FILE* fout = fopen (" bundle_adjustment.xyz" " wt"
88+ if (fout == NULL ) return -1 ;
89+ for (size_t i = 0 ; i < Xs.size (); i++)
90+ fprintf (fout, " %f %f %f\n " x , Xs[i].y , Xs[i].z );
91+ fclose (fout);
92+ return 0 ;
93+ }
0 commit comments