MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
bundler_common.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <iostream>
11#include <algorithm>
12#include <fstream>
13#include <limits>
14#include <sstream>
15#include <vector>
16#include <cstring>
17#include <cerrno>
18
19#include "util/exception.h"
20#include "sfm/bundler_common.h"
21
22#define PREBUNDLE_SIGNATURE "MVE_PREBUNDLE\n"
23#define PREBUNDLE_SIGNATURE_LEN 14
24
25#define SURVEY_SIGNATURE "MVE_SURVEY\n"
26#define SURVEY_SIGNATURE_LEN 11
27
30
31
32/* --------------- Data Structure for Feature Tracks -------------- */
33
34void
35Track::invalidate (void)
36{
37 std::fill(this->pos.begin(), this->pos.end(),
38 std::numeric_limits<float>::quiet_NaN());
39}
40
41void
42Track::remove_view (int view_id)
43{
44 for (FeatureReferenceList::iterator iter = this->features.begin();
45 iter != this->features.end();)
46 {
47 if (iter->view_id == view_id)
48 iter = this->features.erase(iter);
49 else
50 iter++;
51 }
52}
53
54/* ------------------ Input/Output for Prebundle ------------------ */
55
56void
58 PairwiseMatching const& matching, std::ostream& out)
59{
60 /* Write signature. */
62
63 /* Write number of viewports. */
64 int32_t num_viewports = static_cast<int32_t>(viewports.size());
65 out.write(reinterpret_cast<char const*>(&num_viewports), sizeof(int32_t));
66
67 /* Write per-viewport data. */
68 for (std::size_t i = 0; i < viewports.size(); ++i)
69 {
70 FeatureSet const& vpf = viewports[i].features;
71
72 /* Write positions. */
73 int32_t num_positions = static_cast<int32_t>(vpf.positions.size());
74 out.write(reinterpret_cast<char const*>(&num_positions), sizeof(int32_t));
75 for (std::size_t j = 0; j < vpf.positions.size(); ++j)
76 out.write(reinterpret_cast<char const*>(&vpf.positions[j]), sizeof(math::Vec2f));
77
78 /* Write colors. */
79 int32_t num_colors = static_cast<int32_t>(vpf.colors.size());
80 out.write(reinterpret_cast<char const*>(&num_colors), sizeof(int32_t));
81 for (std::size_t j = 0; j < vpf.colors.size(); ++j)
82 out.write(reinterpret_cast<char const*>(&vpf.colors[j]), sizeof(math::Vec3uc));
83 }
84
85 /* Write number of matching pairs. */
86 int32_t num_pairs = static_cast<int32_t>(matching.size());
87 out.write(reinterpret_cast<char const*>(&num_pairs), sizeof(int32_t));
88
89 /* Write per-matching pair data. */
90 for (std::size_t i = 0; i < matching.size(); ++i)
91 {
92 TwoViewMatching const& tvr = matching[i];
93 int32_t id1 = static_cast<int32_t>(tvr.view_1_id);
94 int32_t id2 = static_cast<int32_t>(tvr.view_2_id);
95 int32_t num_matches = static_cast<int32_t>(tvr.matches.size());
96 out.write(reinterpret_cast<char const*>(&id1), sizeof(int32_t));
97 out.write(reinterpret_cast<char const*>(&id2), sizeof(int32_t));
98 out.write(reinterpret_cast<char const*>(&num_matches), sizeof(int32_t));
99 for (std::size_t j = 0; j < tvr.matches.size(); ++j)
100 {
101 CorrespondenceIndex const& c = tvr.matches[j];
102 int32_t i1 = static_cast<int32_t>(c.first);
103 int32_t i2 = static_cast<int32_t>(c.second);
104 out.write(reinterpret_cast<char const*>(&i1), sizeof(int32_t));
105 out.write(reinterpret_cast<char const*>(&i2), sizeof(int32_t));
106 }
107 }
108}
109
110void
111load_prebundle_data (std::istream& in, ViewportList* viewports,
112 PairwiseMatching* matching)
113{
114 /* Read and check file signature. */
115 char signature[PREBUNDLE_SIGNATURE_LEN + 1];
116 in.read(signature, PREBUNDLE_SIGNATURE_LEN);
117 signature[PREBUNDLE_SIGNATURE_LEN] = '\0';
118 if (std::string(PREBUNDLE_SIGNATURE) != signature)
119 throw std::invalid_argument("Invalid prebundle file signature");
120
121 viewports->clear();
122 matching->clear();
123
124 /* Read number of viewports. */
125 int32_t num_viewports;
126 in.read(reinterpret_cast<char*>(&num_viewports), sizeof(int32_t));
127 viewports->resize(num_viewports);
128
129 /* Read per-viewport data. */
130 for (int i = 0; i < num_viewports; ++i)
131 {
132 FeatureSet& vpf = viewports->at(i).features;
133
134 /* Read positions. */
135 int32_t num_positions;
136 in.read(reinterpret_cast<char*>(&num_positions), sizeof(int32_t));
137 vpf.positions.resize(num_positions);
138 for (int j = 0; j < num_positions; ++j)
139 in.read(reinterpret_cast<char*>(&vpf.positions[j]), sizeof(math::Vec2f));
140
141 /* Read colors. */
142 int32_t num_colors;
143 in.read(reinterpret_cast<char*>(&num_colors), sizeof(int32_t));
144 vpf.colors.resize(num_colors);
145 for (int j = 0; j < num_colors; ++j)
146 in.read(reinterpret_cast<char*>(&vpf.colors[j]), sizeof(math::Vec3uc));
147 }
148
149 /* Read number of matching pairs. */
150 int32_t num_pairs;
151 in.read(reinterpret_cast<char*>(&num_pairs), sizeof(int32_t));
152
153 /* Read per-matching pair data. */
154 for (int32_t i = 0; i < num_pairs; ++i)
155 {
156 int32_t id1, id2, num_matches;
157 in.read(reinterpret_cast<char*>(&id1), sizeof(int32_t));
158 in.read(reinterpret_cast<char*>(&id2), sizeof(int32_t));
159 in.read(reinterpret_cast<char*>(&num_matches), sizeof(int32_t));
160
161 TwoViewMatching tvr;
162 tvr.view_1_id = static_cast<int>(id1);
163 tvr.view_2_id = static_cast<int>(id2);
164 tvr.matches.reserve(num_matches);
165 for (int32_t j = 0; j < num_matches; ++j)
166 {
167 int32_t i1, i2;
168 in.read(reinterpret_cast<char*>(&i1), sizeof(int32_t));
169 in.read(reinterpret_cast<char*>(&i2), sizeof(int32_t));
171 c.first = static_cast<int>(i1);
172 c.second = static_cast<int>(i2);
173 tvr.matches.push_back(c);
174 }
175 matching->push_back(tvr);
176 }
177}
178
179void
181 PairwiseMatching const& matching, std::string const& filename)
182{
183 std::ofstream out(filename.c_str(), std::ios::binary);
184 if (!out.good())
185 throw util::FileException(filename, std::strerror(errno));
186 save_prebundle_data(viewports, matching, out);
187 out.close();
188}
189
190void
191load_prebundle_from_file (std::string const& filename,
192 ViewportList* viewports, PairwiseMatching* matching)
193{
194 std::ifstream in(filename.c_str(), std::ios::binary);
195 if (!in.good())
196 throw util::FileException(filename, std::strerror(errno));
197
198 try
199 {
200 load_prebundle_data(in, viewports, matching);
201 }
202 catch (...)
203 {
204 in.close();
205 throw;
206 }
207
208 if (in.eof())
209 {
210 in.close();
211 throw util::Exception("Premature EOF");
212 }
213 in.close();
214}
215
216void
217load_survey_from_file (std::string const& filename,
218 SurveyPointList* survey_points)
219{
220 std::ifstream in(filename.c_str());
221 if (!in.good())
222 throw util::FileException(filename, std::strerror(errno));
223
224 char signature[SURVEY_SIGNATURE_LEN + 1];
225 in.read(signature, SURVEY_SIGNATURE_LEN);
226 signature[SURVEY_SIGNATURE_LEN] = '\0';
227 if (std::string(SURVEY_SIGNATURE) != signature)
228 throw std::invalid_argument("Invalid survey file signature");
229
230 std::size_t num_points = 0;
231 std::size_t num_observations = 0;
232 in >> num_points >> num_observations;
233 if (in.fail())
234 {
235 in.close();
236 throw util::Exception("Invalid survey file header");
237 }
238
239 survey_points->resize(num_points);
240 for (std::size_t i = 0; i < num_points; ++i)
241 {
242 SurveyPoint& survey_point = survey_points->at(i);
243 for (int j = 0; j < 3; ++j)
244 in >> survey_point.pos[j];
245 }
246
247 for (std::size_t i = 0; i < num_observations; ++i)
248 {
249 int point_id, view_id;
250 float x, y;
251 in >> point_id >> view_id >> x >> y;
252
253 if (static_cast<std::size_t>(point_id) > num_points)
254 throw util::Exception("Invalid survey point id");
255
256 SurveyPoint& survey_point = survey_points->at(point_id);
257 survey_point.observations.emplace_back(view_id, x, y);
258 }
259
260 if (in.fail())
261 {
262 in.close();
263 throw util::Exception("Parsing error");
264 }
265
266 in.close();
267}
268
269/* ---------------------- Feature undistortion -------------------- */
270
271namespace {
272 double distort_squared_radius (double const r2, double const k1,
273 double const k2)
274 {
275 // Compute the distorted squared radius:
276 // (radius * distortion_coeff)^2 = r^2 * coeff^2
277 double coeff = 1.0 + r2 * k1 + r2 * r2 * k2;
278 return r2 * coeff * coeff;
279 }
280
281 double solve_undistorted_squared_radius (double const r2,
282 double const k1, double const k2)
283 {
284 // Guess initial interval upper and lower bound
285 double lbound = r2, ubound = r2;
286 while (distort_squared_radius(lbound, k1, k2) > r2)
287 {
288 ubound = lbound;
289 lbound /= 1.05;
290 }
291 while (distort_squared_radius(ubound, k1, k2) < r2)
292 {
293 lbound = ubound;
294 ubound *= 1.05;
295 }
296
297 // We use bisection as we can easily find a pretty good initial guess,
298 // and we don't want to compute all roots for a 5th degree polynomial.
299
300 // Perform a bisection until epsilon accuracy is reached
301 double mid = 0.5 * (lbound + ubound);
302 while (mid != lbound && mid != ubound)
303 {
304 if (distort_squared_radius(mid, k1, k2) > r2)
305 ubound = mid;
306 else
307 lbound = mid;
308 mid = 0.5 * (lbound + ubound);
309 }
310 // Return center of interval
311 return mid;
312 }
313}
314
316undistort_feature (math::Vec2f const& f, double const k1, double const k2,
317 float const focal_length)
318{
319 // Convert to camera coords
320 double const r2 = f.square_norm() / MATH_POW2(focal_length);
321 double scale = 1.0;
322 if (r2 > 0.0)
323 scale = std::sqrt(solve_undistorted_squared_radius(r2, k1, k2) / r2);
324 return f * scale;
325}
326
#define PREBUNDLE_SIGNATURE_LEN
#define SURVEY_SIGNATURE
#define PREBUNDLE_SIGNATURE
#define SURVEY_SIGNATURE_LEN
Vector class for arbitrary dimensions and types.
Definition vector.h:87
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
Definition vector.h:441
The FeatureSet holds per-feature information for a single view, and allows to transparently compute a...
Definition feature_set.h:28
std::vector< math::Vec3uc > colors
Per-feature image color.
Definition feature_set.h:68
std::vector< math::Vec2f > positions
Per-feature image position.
Definition feature_set.h:66
Universal, simple exception class.
Definition exception.h:24
Exception class for file exceptions with additional filename.
Definition exception.h:53
#define MATH_POW2(x)
Definition defines.h:68
Vector< float, 2 > Vec2f
Definition vector.h:30
Vector< unsigned char, 3 > Vec3uc
Definition vector.h:63
void load_prebundle_from_file(std::string const &filename, ViewportList *viewports, PairwiseMatching *matching)
Loads the pre-bundle data from file, initializing viewports and matching.
std::vector< Viewport > ViewportList
The list of all viewports considered for bundling.
math::Vec2f undistort_feature(math::Vec2f const &f, double const k1, double const k2, float const focal_length)
void save_prebundle_to_file(ViewportList const &viewports, PairwiseMatching const &matching, std::string const &filename)
Saves the pre-bundle data to file, which records all viewport and matching data necessary for increme...
void load_prebundle_data(std::istream &in, ViewportList *viewports, PairwiseMatching *matching)
std::vector< SurveyPoint > SurveyPointList
The list of all survey poins.
std::vector< TwoViewMatching > PairwiseMatching
The matching result between several pairs of views.
void save_prebundle_data(ViewportList const &viewports, PairwiseMatching const &matching, std::ostream &out)
void load_survey_from_file(std::string const &filename, SurveyPointList *survey_points)
Loads survey points and their observations from file.
std::pair< int, int > CorrespondenceIndex
The IDs of a matching feature pair in two images.
#define SFM_BUNDLER_NAMESPACE_END
Definition defines.h:17
#define SFM_BUNDLER_NAMESPACE_BEGIN
Definition defines.h:16
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
Representation of a survey point.
SurveyObservationList observations
The matching result between two views.
CorrespondenceIndices matches