1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
|
#include <opencv2/sfm.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <iostream>
#include <fstream>
#include <string>
using namespace std;
using namespace cv;
using namespace cv::sfm;
static void help() {
cout
<< "\n------------------------------------------------------------------------------------\n"
<< " This program shows the multiview reconstruction capabilities in the \n"
<< " OpenCV Structure From Motion (SFM) module.\n"
<< " It reconstruct a scene from a set of 2D images \n"
<< " Usage:\n"
<< " example_sfm_scene_reconstruction <path_to_file> <f> <cx> <cy>\n"
<< " where: path_to_file is the file absolute path into your system which contains\n"
<< " the list of images to use for reconstruction. \n"
<< " f is the focal lenght in pixels. \n"
<< " cx is the image principal point x coordinates in pixels. \n"
<< " cy is the image principal point y coordinates in pixels. \n"
<< "------------------------------------------------------------------------------------\n\n"
<< endl;
}
int getdir(const string _filename, vector<string> &files)
{
ifstream myfile(_filename.c_str());
if (!myfile.is_open()) {
cout << "Unable to read file: " << _filename << endl;
exit(0);
} else {;
size_t found = _filename.find_last_of("/\\");
string line_str, path_to_file = _filename.substr(0, found);
while ( getline(myfile, line_str) )
files.push_back(path_to_file+string("/")+line_str);
}
return 1;
}
int main(int argc, char* argv[])
{
// Read input parameters
if ( argc != 5 )
{
help();
exit(0);
}
// Parse the image paths
vector<string> images_paths;
getdir( argv[1], images_paths );
// Build instrinsics
float f = atof(argv[2]),
cx = atof(argv[3]), cy = atof(argv[4]);
Matx33d K = Matx33d( f, 0, cx,
0, f, cy,
0, 0, 1);
/// Reconstruct the scene using the 2d images
bool is_projective = true;
vector<Mat> Rs_est, ts_est, points3d_estimated;
reconstruct(images_paths, Rs_est, ts_est, K, points3d_estimated, is_projective);
// Print output
cout << "\n----------------------------\n" << endl;
cout << "Reconstruction: " << endl;
cout << "============================" << endl;
cout << "Estimated 3D points: " << points3d_estimated.size() << endl;
cout << "Estimated cameras: " << Rs_est.size() << endl;
cout << "Refined intrinsics: " << endl << K << endl << endl;
cout << "3D Visualization: " << endl;
cout << "============================" << endl;
/// Create 3D windows
viz::Viz3d window("Coordinate Frame");
window.setWindowSize(Size(500,500));
window.setWindowPosition(Point(150,150));
window.setBackgroundColor(); // black by default
// Create the pointcloud
cout << "Recovering points ... ";
// recover estimated points3d
vector<Vec3f> point_cloud_est;
for (int i = 0; i < points3d_estimated.size(); ++i)
point_cloud_est.push_back(Vec3f(points3d_estimated[i]));
cout << "[DONE]" << endl;
/// Recovering cameras
cout << "Recovering cameras ... ";
vector<Affine3d> path;
for (size_t i = 0; i < Rs_est.size(); ++i)
path.push_back(Affine3d(Rs_est[i],ts_est[i]));
cout << "[DONE]" << endl;
/// Add the pointcloud
if ( point_cloud_est.size() > 0 )
{
cout << "Rendering points ... ";
viz::WCloud cloud_widget(point_cloud_est, viz::Color::green());
window.showWidget("point_cloud", cloud_widget);
cout << "[DONE]" << endl;
}
else
{
cout << "Cannot render points: Empty pointcloud" << endl;
}
/// Add cameras
if ( path.size() > 0 )
{
cout << "Rendering Cameras ... ";
window.showWidget("cameras_frames_and_lines", viz::WTrajectory(path, viz::WTrajectory::BOTH, 0.1, viz::Color::green()));
window.showWidget("cameras_frustums", viz::WTrajectoryFrustums(path, K, 0.1, viz::Color::yellow()));
window.setViewerPose(path[0]);
cout << "[DONE]" << endl;
}
else
{
cout << "Cannot render the cameras: Empty path" << endl;
}
/// Wait for key 'q' to close the window
cout << endl << "Press 'q' to close each windows ... " << endl;
window.spin();
return 0;
}
|
