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
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
/*
Based on TheiaSfM library.
https://github.com/sweeneychris/TheiaSfM/blob/master/src/theia/io/read_bundler_files.cc
Adapted by Edgar Riba <edgar.riba@gmail.com>
*/
#include <iostream>
#include <fstream>
#include <opencv2/core.hpp>
// The bundle files contain the estimated scene and camera geometry have the
// following format:
// # Bundle file v0.3
// <num_cameras> <num_points> [two integers]
// <camera1>
// <camera2>
// ...
// <cameraN>
// <point1>
// <point2>
// ...
// <pointM>
// Each camera entry <cameraI> contains the estimated camera intrinsics and
// extrinsics, and has the form:
// <f> <k1> <k2> [the focal length, followed by two radial distortion
// coeffs]
// <R> [a 3x3 matrix representing the camera rotation]
// <t> [a 3-vector describing the camera translation]
// The cameras are specified in the order they appear in the list of images.
//
// Each point entry has the form:
// <position> [a 3-vector describing the 3D position of the point]
// <color> [a 3-vector describing the RGB color of the point]
// <view list> [a list of views the point is visible in]
//
// The view list begins with the length of the list (i.e., the number of cameras
// the point is visible in). The list is then given as a list of quadruplets
// <camera> <key> <x> <y>, where <camera> is a camera index, <key> the index of
// the SIFT keypoint where the point was detected in that camera, and <x> and
// <y> are the detected positions of that keypoint. Both indices are 0-based
// (e.g., if camera 0 appears in the list, this corresponds to the first camera
// in the scene file and the first image in "list.txt"). The pixel positions are
// floating point numbers in a coordinate system where the origin is the center
// of the image, the x-axis increases to the right, and the y-axis increases
// towards the top of the image. Thus, (-w/2, -h/2) is the lower-left corner of
// the image, and (w/2, h/2) is the top-right corner (where w and h are the
// width and height of the image).
static bool readBundlerFile(const std::string &file,
std::vector<cv::Matx33d> &Rs,
std::vector<cv::Vec3d> &Ts,
std::vector<cv::Matx33d> &Ks,
std::vector<cv::Vec3d> &points3d) {
// Read in num cameras, num points.
std::ifstream ifs(file.c_str(), std::ios::in);
if (!ifs.is_open()) {
std::cout << "Cannot read the file from " << file << std::endl;
return false;
}
const cv::Matx33d bundler_to_opencv(1, 0, 0, 0, -1, 0, 0, 0, -1);
std::string header_string;
std::getline(ifs, header_string);
// If the first line starts with '#' then it is a comment, so skip it!
if (header_string[0] == '#') {
std::getline(ifs, header_string);
}
const char* p = header_string.c_str();
char* p2;
const int num_cameras = strtol(p, &p2, 10);
p = p2;
const int num_points = strtol(p, &p2, 10);
// Read in the camera params.
for (int i = 0; i < num_cameras; i++) {
// Read in focal length, radial distortion.
std::string internal_params;
std::getline(ifs, internal_params);
p = internal_params.c_str();
const double focal_length = strtod(p, &p2);
p = p2;
//const double k1 = strtod(p, &p2);
p = p2;
//const double k2 = strtod(p, &p2);
p = p2;
cv::Matx33d intrinsics;
intrinsics(0,0) = intrinsics(1,1) = focal_length;
Ks.push_back(intrinsics);
// Read in rotation (row-major).
cv::Matx33d rotation;
for (int r = 0; r < 3; r++) {
std::string rotation_row;
std::getline(ifs, rotation_row);
p = rotation_row.c_str();
for (int c = 0; c < 3; c++) {
rotation(r, c) = strtod(p, &p2);
p = p2;
}
}
std::string translation_string;
std::getline(ifs, translation_string);
p = translation_string.c_str();
cv::Vec3d translation;
for (int j = 0; j < 3; j++) {
translation(j) = strtod(p, &p2);
p = p2;
}
rotation = bundler_to_opencv * rotation;
translation = bundler_to_opencv * translation;
cv::Matx33d rotation_t = rotation.t();
translation = -1.0 * rotation_t * translation;
Rs.push_back(rotation);
Ts.push_back(translation);
if ((i + 1) % 100 == 0 || i == num_cameras - 1) {
std::cout << "\r Loading parameters for camera " << i + 1 << " / "
<< num_cameras << std::flush;
}
}
std::cout << std::endl;
// Read in each 3D point and correspondences.
for (int i = 0; i < num_points; i++) {
// Read position.
std::string position_str;
std::getline(ifs, position_str);
p = position_str.c_str();
cv::Vec3d position;
for (int j = 0; j < 3; j++) {
position(j) = strtod(p, &p2);
p = p2;
}
points3d.push_back(position);
// Read color.
std::string color_str;
std::getline(ifs, color_str);
p = color_str.c_str();
cv::Vec3d color;
for (int j = 0; j < 3; j++) {
color(j) = static_cast<double>(strtol(p, &p2, 10)) / 255.0;
p = p2;
}
// Read viewlist.
std::string view_list_string;
std::getline(ifs, view_list_string);
p = view_list_string.c_str();
const int num_views = strtol(p, &p2, 10);
p = p2;
// Reserve the view list for this 3D point.
for (int j = 0; j < num_views; j++) {
// Camera key x y
//const int camera_index = strtol(p, &p2, 10);
p = p2;
// Returns the index of the sift descriptor in the camera for this track.
strtol(p, &p2, 10);
p = p2;
//const float x_pos = strtof(p, &p2);
p = p2;
//const float y_pos = strtof(p, &p2);
p = p2;
}
if ((i + 1) % 100 == 0 || i == num_points - 1) {
std::cout << "\r Loading 3D points " << i + 1 << " / " << num_points
<< std::flush;
}
}
std::cout << std::endl;
ifs.close();
return true;
}