MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
bundle_adjustment.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann, Fabian Langguth
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#ifndef SFM_BUNDLE_ADJUSTMENT_HEADER
11#define SFM_BUNDLE_ADJUSTMENT_HEADER
12
13#include <vector>
14
15#include "util/logging.h"
16#include "sfm/defines.h"
18#include "sfm/ba_dense_vector.h"
20#include "sfm/ba_types.h"
21
22/*
23 * A few notes.
24 *
25 * - PBA normalizes focal length and depth values before LM optimization,
26 * and denormalizes afterwards. Is this necessary with double?
27 * - PBA exits the LM main loop if norm of -JF is small. Useful?
28 * - The slowest part is computing the Schur complement because of matrix
29 * multiplications. How can this be improved?
30 *
31 * Actual TODOs.
32 *
33 * - Better preconditioner for conjugate gradient, i.e., use the 9x9 diagonal
34 * blocks of S instead of B. Requires method in matrix.
35 * - Properly implement and test BA_POINTS mode.
36 * - More accurate implementations for the Jacobian (currently approximated).
37 * - Implement block_size = 9 in linear solver, no need for CG
38 */
39
42
52{
53public:
54 enum BAMode
55 {
56 BA_CAMERAS = 1,
57 BA_POINTS = 2,
58 BA_CAMERAS_AND_POINTS = 1 | 2
59 };
60
75
88
89public:
90 BundleAdjustment (Options const& options);
91
92 void set_cameras (std::vector<Camera>* cameras);
93 void set_points (std::vector<Point3D>* points);
94 void set_observations (std::vector<Observation>* observations);
95
96 Status optimize (void);
97 void print_status (bool detailed = false) const;
98
99private:
102
103private:
104 void sanity_checks (void);
105 void lm_optimize (void);
106
107 /* Helper functions. */
108 void compute_reprojection_errors (DenseVectorType* vector_f,
109 DenseVectorType const* delta_x = nullptr);
110 double compute_mse (DenseVectorType const& vector_f);
111 void radial_distort (double* x, double* y, double const* dist);
112 void rodrigues_to_matrix (double const* r, double* rot);
113
114 /* Analytic Jacobian. */
115 void analytic_jacobian (SparseMatrixType* jac_cam,
116 SparseMatrixType* jac_points);
117 void analytic_jacobian_entries (Camera const& cam, Point3D const& point,
118 double* cam_x_ptr, double* cam_y_ptr,
119 double* point_x_ptr, double* point_y_ptr);
120
121 /* Update of camera/point parameters. */
122 void update_parameters (DenseVectorType const& delta_x);
123 void update_camera (Camera const& cam, double const* update, Camera* out);
124 void update_point (Point3D const& pt, double const* update, Point3D* out);
125
126private:
127 Options opts;
128 Status status;
129 util::Logging log;
130 std::vector<Camera>* cameras;
131 std::vector<Point3D>* points;
132 std::vector<Observation>* observations;
133 int const num_cam_params;
134};
135
136/* ------------------------ Implementation ------------------------ */
137
138inline
139BundleAdjustment::Options::Options (void)
140 : verbose_output(false)
141 , bundle_mode(BA_CAMERAS_AND_POINTS)
142 , lm_max_iterations(50)
143 , lm_min_iterations(0)
144 , lm_delta_threshold(1e-4)
145 , lm_mse_threshold(1e-8)
146{
147}
148
149inline
151 : initial_mse(0.0)
152 , final_mse(0.0)
153 , num_lm_iterations(0)
154 , num_lm_successful_iterations(0)
155 , num_lm_unsuccessful_iterations(0)
156 , num_cg_iterations(0)
157{
158}
159
160inline
162 : opts(options)
163 , log(options.verbose_output
164 ? util::Logging::LOG_DEBUG
165 : util::Logging::LOG_INFO)
166 , cameras(nullptr)
167 , points(nullptr)
168 , observations(nullptr)
169 , num_cam_params(options.fixed_intrinsics ? 6 : 9)
170{
171 this->opts.linear_opts.camera_block_dim = this->num_cam_params;
172}
173
174inline void
175BundleAdjustment::set_cameras (std::vector<Camera>* cameras)
176{
177 this->cameras = cameras;
178}
179
180inline void
181BundleAdjustment::set_points (std::vector<Point3D>* points)
182{
183 this->points = points;
184}
185
186inline void
187BundleAdjustment::set_observations (std::vector<Observation>* points_2d)
188{
189 this->observations = points_2d;
190}
191
194
195#endif /* SFM_BUNDLE_ADJUSTMENT_HEADER */
A simple bundle adjustment optimization implementation.
BundleAdjustment(Options const &options)
void set_cameras(std::vector< Camera > *cameras)
void set_observations(std::vector< Observation > *observations)
void set_points(std::vector< Point3D > *points)
Sparse matrix class in Yale format for column-major matrices.
Parser, tokenizer, timer, smart pointer, threads, etc.
#define SFM_BA_NAMESPACE_BEGIN
Definition defines.h:22
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
#define SFM_BA_NAMESPACE_END
Definition defines.h:23
Camera representation for bundle adjustment.
Definition ba_types.h:13
3D point representation for bundle adjustment.
Definition ba_types.h:25