MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
bundle_adjustment.cc
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#include <thread>
11#include <iostream>
12#include <iomanip>
13
14#include "math/matrix_tools.h"
15#include "util/timer.h"
17#include "sfm/ba_dense_vector.h"
19
20#define LOG_E this->log.error()
21#define LOG_W this->log.warning()
22#define LOG_I this->log.info()
23#define LOG_V this->log.verbose()
24#define LOG_D this->log.debug()
25
26#define TRUST_REGION_RADIUS_INIT (1000)
27#define TRUST_REGION_RADIUS_DECREMENT (1.0 / 2.0)
28
31
32BundleAdjustment::Status
33BundleAdjustment::optimize (void)
34{
35 util::WallTimer timer;
36 this->sanity_checks();
37 this->status = Status();
38 this->lm_optimize();
39 this->status.runtime_ms = timer.get_elapsed();
40 return this->status;
41}
42
43void
44BundleAdjustment::sanity_checks (void)
45{
46 /* Check for null arguments. */
47 if (this->cameras == nullptr)
48 throw std::invalid_argument("No cameras given");
49 if (this->points == nullptr)
50 throw std::invalid_argument("No tracks given");
51 if (this->observations == nullptr)
52 throw std::invalid_argument("No observations given");
53
54 /* Check for valid focal lengths. */
55 for (std::size_t i = 0; i < this->cameras->size(); ++i)
56 if (this->cameras->at(i).focal_length <= 0.0)
57 throw std::invalid_argument("Camera with invalid focal length");
58
59 /* Check for valid IDs in the observations. */
60 for (std::size_t i = 0; i < this->observations->size(); ++i)
61 {
62 Observation const& obs = this->observations->at(i);
63 if (obs.camera_id < 0
64 || obs.camera_id >= static_cast<int>(this->cameras->size()))
65 throw std::invalid_argument("Observation with invalid camera ID");
66 if (obs.point_id < 0
67 || obs.point_id >= static_cast<int>(this->points->size()))
68 throw std::invalid_argument("Observation with invalid track ID");
69 }
70}
71
72void
73BundleAdjustment::lm_optimize (void)
74{
75 /* Setup linear solver. */
76 LinearSolver::Options pcg_opts;
77 pcg_opts = this->opts.linear_opts;
78 pcg_opts.trust_region_radius = TRUST_REGION_RADIUS_INIT;
79
80 /* Compute reprojection error for the first time. */
81 DenseVectorType F, F_new;
82 this->compute_reprojection_errors(&F);
83 double current_mse = this->compute_mse(F);
84 this->status.initial_mse = current_mse;
85 this->status.final_mse = current_mse;
86
87 /* Levenberg-Marquard main loop. */
88 for (int lm_iter = 0; ; ++lm_iter)
89 {
90 if (lm_iter + 1 > this->opts.lm_min_iterations
91 && (current_mse < this->opts.lm_mse_threshold))
92 {
93 LOG_V << "BA: Satisfied MSE threshold." << std::endl;
94 break;
95 }
96
97 /* Compute Jacobian. */
98 SparseMatrixType Jc, Jp;
99 switch (this->opts.bundle_mode)
100 {
101 case BA_CAMERAS_AND_POINTS:
102 this->analytic_jacobian(&Jc, &Jp);
103 break;
104 case BA_CAMERAS:
105 this->analytic_jacobian(&Jc, nullptr);
106 break;
107 case BA_POINTS:
108 this->analytic_jacobian(nullptr, &Jp);
109 break;
110 default:
111 throw std::runtime_error("Invalid bundle mode");
112 }
113
114 /* Perform linear step. */
115 DenseVectorType delta_x;
116 LinearSolver pcg(pcg_opts);
117 LinearSolver::Status cg_status = pcg.solve(Jc, Jp, F, &delta_x);
118
119 /* Update reprojection errors and MSE after linear step. */
120 double new_mse, delta_mse, delta_mse_ratio = 1.0;
121 if (cg_status.success)
122 {
123 this->compute_reprojection_errors(&F_new, &delta_x);
124 new_mse = this->compute_mse(F_new);
125 delta_mse = current_mse - new_mse;
126 delta_mse_ratio = 1.0 - new_mse / current_mse;
127 this->status.num_cg_iterations += cg_status.num_cg_iterations;
128 }
129 else
130 {
131 new_mse = current_mse;
132 delta_mse = 0.0;
133 }
134 bool successful_iteration = delta_mse > 0.0;
135
136 /*
137 * Apply delta to parameters after successful step.
138 * Adjust the trust region to increase/decrease regulariztion.
139 */
140 if (successful_iteration)
141 {
142 LOG_V << "BA: #" << std::setw(2) << std::left << lm_iter
143 << " success" << std::right
144 << ", MSE " << std::setw(11) << current_mse
145 << " -> " << std::setw(11) << new_mse
146 << ", CG " << std::setw(3) << cg_status.num_cg_iterations
147 << ", TRR " << pcg_opts.trust_region_radius
148 << std::endl;
149
150 this->status.num_lm_iterations += 1;
151 this->status.num_lm_successful_iterations += 1;
152 this->update_parameters(delta_x);
153 std::swap(F, F_new);
154 current_mse = new_mse;
155
156 /* Compute trust region update. FIXME delta_norm or mse? */
157 double const gain_ratio = delta_mse * (F.size() / 2)
158 / cg_status.predicted_error_decrease;
159 double const trust_region_update = 1.0 / std::max(1.0 / 3.0,
160 (1.0 - MATH_POW3(2.0 * gain_ratio - 1.0)));
161 pcg_opts.trust_region_radius *= trust_region_update;
162 }
163 else
164 {
165 LOG_V << "BA: #" << std::setw(2) << std::left << lm_iter
166 << " failure" << std::right
167 << ", MSE " << std::setw(11) << current_mse
168 << ", " << std::setw(11) << " "
169 << " CG " << std::setw(3) << cg_status.num_cg_iterations
170 << ", TRR " << pcg_opts.trust_region_radius
171 << std::endl;
172
173 this->status.num_lm_iterations += 1;
174 this->status.num_lm_unsuccessful_iterations += 1;
175 pcg_opts.trust_region_radius *= TRUST_REGION_RADIUS_DECREMENT;
176 }
177
178 /* Check termination due to LM iterations. */
179 if (lm_iter + 1 < this->opts.lm_min_iterations)
180 continue;
181 if (lm_iter + 1 >= this->opts.lm_max_iterations)
182 {
183 LOG_V << "BA: Reached maximum LM iterations of "
184 << this->opts.lm_max_iterations << std::endl;
185 break;
186 }
187
188 /* Check threshold on the norm of delta_x. */
189 if (successful_iteration)
190 {
191 if (delta_mse_ratio < this->opts.lm_delta_threshold)
192 {
193 LOG_V << "BA: Satisfied delta mse ratio threshold of "
194 << this->opts.lm_delta_threshold << std::endl;
195 break;
196 }
197 }
198 }
199
200 this->status.final_mse = current_mse;
201}
202
203void
204BundleAdjustment::compute_reprojection_errors (DenseVectorType* vector_f,
205 DenseVectorType const* delta_x)
206{
207 if (vector_f->size() != this->observations->size() * 2)
208 vector_f->resize(this->observations->size() * 2);
209
210#pragma omp parallel for
211 for (std::size_t i = 0; i < this->observations->size(); ++i)
212 {
213 Observation const& obs = this->observations->at(i);
214 Point3D const& p3d = this->points->at(obs.point_id);
215 Camera const& cam = this->cameras->at(obs.camera_id);
216
217 double const* flen = &cam.focal_length;
218 double const* dist = cam.distortion;
219 double const* rot = cam.rotation;
220 double const* trans = cam.translation;
221 double const* point = p3d.pos;
222
223 Point3D new_point;
224 Camera new_camera;
225 if (delta_x != nullptr)
226 {
227 std::size_t cam_id = obs.camera_id * this->num_cam_params;
228 std::size_t pt_id = obs.point_id * 3;
229
230 if (this->opts.bundle_mode & BA_CAMERAS)
231 {
232 this->update_camera(cam, delta_x->data() + cam_id, &new_camera);
233 flen = &new_camera.focal_length;
234 dist = new_camera.distortion;
235 rot = new_camera.rotation;
236 trans = new_camera.translation;
237 pt_id += this->cameras->size() * this->num_cam_params;
238 }
239
240 if (this->opts.bundle_mode & BA_POINTS)
241 {
242 this->update_point(p3d, delta_x->data() + pt_id, &new_point);
243 point = new_point.pos;
244 }
245 }
246
247 /* Project point onto image plane. */
248 double rp[] = { 0.0, 0.0, 0.0 };
249 for (int d = 0; d < 3; ++d)
250 {
251 rp[0] += rot[0 + d] * point[d];
252 rp[1] += rot[3 + d] * point[d];
253 rp[2] += rot[6 + d] * point[d];
254 }
255 rp[2] = (rp[2] + trans[2]);
256 rp[0] = (rp[0] + trans[0]) / rp[2];
257 rp[1] = (rp[1] + trans[1]) / rp[2];
258
259 /* Distort reprojections. */
260 this->radial_distort(rp + 0, rp + 1, dist);
261
262 /* Compute reprojection error. */
263 vector_f->at(i * 2 + 0) = rp[0] * (*flen) - obs.pos[0];
264 vector_f->at(i * 2 + 1) = rp[1] * (*flen) - obs.pos[1];
265 }
266}
267
268double
269BundleAdjustment::compute_mse (DenseVectorType const& vector_f)
270{
271 double mse = 0.0;
272 for (std::size_t i = 0; i < vector_f.size(); ++i)
273 mse += vector_f[i] * vector_f[i];
274 return mse / static_cast<double>(vector_f.size() / 2);
275}
276
277void
278BundleAdjustment::radial_distort (double* x, double* y, double const* dist)
279{
280 double const radius2 = *x * *x + *y * *y;
281 double const factor = 1.0 + radius2 * (dist[0] + dist[1] * radius2);
282 *x *= factor;
283 *y *= factor;
284}
285
286void
287BundleAdjustment::rodrigues_to_matrix (double const* r, double* m)
288{
289 /* Obtain angle from vector length. */
290 double a = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
291 /* Precompute sine and cosine terms. */
292 double ct = (a == 0.0) ? 0.5f : (1.0f - std::cos(a)) / (a * a);
293 double st = (a == 0.0) ? 1.0 : std::sin(a) / a;
294 /* R = I + st * K + ct * K^2 (with cross product matrix K). */
295 m[0] = 1.0 - (r[1] * r[1] + r[2] * r[2]) * ct;
296 m[1] = r[0] * r[1] * ct - r[2] * st;
297 m[2] = r[2] * r[0] * ct + r[1] * st;
298 m[3] = r[0] * r[1] * ct + r[2] * st;
299 m[4] = 1.0f - (r[2] * r[2] + r[0] * r[0]) * ct;
300 m[5] = r[1] * r[2] * ct - r[0] * st;
301 m[6] = r[2] * r[0] * ct - r[1] * st;
302 m[7] = r[1] * r[2] * ct + r[0] * st;
303 m[8] = 1.0 - (r[0] * r[0] + r[1] * r[1]) * ct;
304}
305
306void
307BundleAdjustment::analytic_jacobian (SparseMatrixType* jac_cam,
308 SparseMatrixType* jac_points)
309{
310 std::size_t const camera_cols = this->cameras->size()
311 * this->num_cam_params;
312 std::size_t const point_cols = this->points->size() * 3;
313 std::size_t const jacobi_rows = this->observations->size() * 2;
314
315 SparseMatrixType::Triplets cam_triplets, point_triplets;
316 if (jac_cam != nullptr)
317 cam_triplets.resize(this->observations->size() * 2
318 * this->num_cam_params);
319 if (jac_points != nullptr)
320 point_triplets.resize(this->observations->size() * 3 * 2);
321
322#pragma omp parallel
323 {
324 double cam_x_ptr[9], cam_y_ptr[9], point_x_ptr[3], point_y_ptr[3];
325#pragma omp for
326 for (std::size_t i = 0; i < this->observations->size(); ++i)
327 {
328 Observation const& obs = this->observations->at(i);
329 Point3D const& p3d = this->points->at(obs.point_id);
330 Camera const& cam = this->cameras->at(obs.camera_id);
331 this->analytic_jacobian_entries(cam, p3d,
332 cam_x_ptr, cam_y_ptr, point_x_ptr, point_y_ptr);
333
334 if (p3d.is_constant)
335 {
336 std::fill(point_x_ptr, point_x_ptr + 3, 0.0);
337 std::fill(point_y_ptr, point_y_ptr + 3, 0.0);
338 }
339
340 std::size_t row_x = i * 2, row_y = row_x + 1;
341 std::size_t cam_col = obs.camera_id * this->num_cam_params;
342 std::size_t point_col = obs.point_id * 3;
343 std::size_t cam_offset = i * this->num_cam_params * 2;
344 for (int j = 0; jac_cam != nullptr && j < this->num_cam_params; ++j)
345 {
346 cam_triplets[cam_offset + j * 2 + 0] =
347 SparseMatrixType::Triplet(row_x, cam_col + j, cam_x_ptr[j]);
348 cam_triplets[cam_offset + j * 2 + 1] =
349 SparseMatrixType::Triplet(row_y, cam_col + j, cam_y_ptr[j]);
350 }
351 std::size_t point_offset = i * 3 * 2;
352 for (int j = 0; jac_points != nullptr && j < 3; ++j)
353 {
354 point_triplets[point_offset + j * 2 + 0] =
355 SparseMatrixType::Triplet(row_x, point_col + j, point_x_ptr[j]);
356 point_triplets[point_offset + j * 2 + 1] =
357 SparseMatrixType::Triplet(row_y, point_col + j, point_y_ptr[j]);
358 }
359 }
360
361#pragma omp sections
362 {
363#pragma omp section
364 if (jac_cam != nullptr)
365 {
366 jac_cam->allocate(jacobi_rows, camera_cols);
367 jac_cam->set_from_triplets(cam_triplets);
368 }
369
370#pragma omp section
371 if (jac_points != nullptr)
372 {
373 jac_points->allocate(jacobi_rows, point_cols);
374 jac_points->set_from_triplets(point_triplets);
375 }
376 }
377 }
378}
379
380void
381BundleAdjustment::analytic_jacobian_entries (Camera const& cam,
382 Point3D const& point,
383 double* cam_x_ptr, double* cam_y_ptr,
384 double* point_x_ptr, double* point_y_ptr)
385{
386 /*
387 * This function computes the Jacobian entries for the given camera and
388 * 3D point pair that leads to one observation.
389 *
390 * The camera block 'cam_x_ptr' and 'cam_y_ptr' is:
391 * - ID 0: Derivative of focal length f
392 * - ID 1-2: Derivative of distortion parameters k0, k1
393 * - ID 3-5: Derivative of translation t0, t1, t2
394 * - ID 6-8: Derivative of rotation r0, r1, r2
395 *
396 * The 3D point block 'point_x_ptr' and 'point_y_ptr' is:
397 * - ID 0-2: Derivative in x, y, and z direction.
398 *
399 * The function that leads to the observation is given as follows:
400 *
401 * Px = f * D(ix,iy) * ix (image observation x coordinate)
402 * Py = f * D(ix,iy) * iy (image observation y coordinate)
403 *
404 * with the following definitions:
405 *
406 * x = R0 * X + t0 (homogeneous projection)
407 * y = R1 * X + t1 (homogeneous projection)
408 * z = R2 * X + t2 (homogeneous projection)
409 * ix = x / z (central projection)
410 * iy = y / z (central projection)
411 * D(ix, iy) = 1 + k0 (ix^2 + iy^2) + k1 (ix^2 + iy^2)^2 (distortion)
412 *
413 * The derivatives for intrinsics (f, k0, k1) are easy to compute exactly.
414 * The derivatives for extrinsics (r, t) and point coordinates Xx, Xy, Xz,
415 * are a bit of a pain to compute.
416 */
417
418 /* Aliases. */
419 double const* r = cam.rotation;
420 double const* t = cam.translation;
421 double const* k = cam.distortion;
422 double const* p3d = point.pos;
423
424 /* Temporary values. */
425 double const rx = r[0] * p3d[0] + r[1] * p3d[1] + r[2] * p3d[2];
426 double const ry = r[3] * p3d[0] + r[4] * p3d[1] + r[5] * p3d[2];
427 double const rz = r[6] * p3d[0] + r[7] * p3d[1] + r[8] * p3d[2];
428 double const px = rx + t[0];
429 double const py = ry + t[1];
430 double const pz = rz + t[2];
431 double const ix = px / pz;
432 double const iy = py / pz;
433 double const fz = cam.focal_length / pz;
434 double const radius2 = ix * ix + iy * iy;
435 double const rd_factor = 1.0 + (k[0] + k[1] * radius2) * radius2;
436
437 /* Compute exact camera and point entries if intrinsics are fixed */
438 if (this->opts.fixed_intrinsics)
439 {
440 cam_x_ptr[0] = fz * rd_factor;
441 cam_x_ptr[1] = 0.0;
442 cam_x_ptr[2] = -fz * rd_factor * ix;
443 cam_x_ptr[3] = -fz * rd_factor * ry * ix;
444 cam_x_ptr[4] = fz * rd_factor * (rz + rx * ix);
445 cam_x_ptr[5] = -fz * rd_factor * ry;
446
447 cam_y_ptr[0] = 0.0;
448 cam_y_ptr[1] = fz * rd_factor;
449 cam_y_ptr[2] = -fz * rd_factor * iy;
450 cam_y_ptr[3] = -fz * rd_factor * (rz + ry * iy);
451 cam_y_ptr[4] = fz * rd_factor * rx * iy;
452 cam_y_ptr[5] = fz * rd_factor * rx;
453
454 /*
455 * Compute point derivatives in x, y, and z.
456 */
457 point_x_ptr[0] = fz * rd_factor * (r[0] - r[6] * ix);
458 point_x_ptr[1] = fz * rd_factor * (r[1] - r[7] * ix);
459 point_x_ptr[2] = fz * rd_factor * (r[2] - r[8] * ix);
460
461 point_y_ptr[0] = fz * rd_factor * (r[3] - r[6] * iy);
462 point_y_ptr[1] = fz * rd_factor * (r[4] - r[7] * iy);
463 point_y_ptr[2] = fz * rd_factor * (r[5] - r[8] * iy);
464 return;
465 }
466
467 /* The intrinsics are easy to compute exactly. */
468 cam_x_ptr[0] = ix * rd_factor;
469 cam_x_ptr[1] = cam.focal_length * ix * radius2;
470 cam_x_ptr[2] = cam.focal_length * ix * radius2 * radius2;
471
472 cam_y_ptr[0] = iy * rd_factor;
473 cam_y_ptr[1] = cam.focal_length * iy * radius2;
474 cam_y_ptr[2] = cam.focal_length * iy * radius2 * radius2;
475
476#define JACOBIAN_APPROX_CONST_RD 0
477#define JACOBIAN_APPROX_PBA 0
478#if JACOBIAN_APPROX_CONST_RD
479 /*
480 * Compute approximations of the Jacobian entries for the extrinsics
481 * by assuming the distortion coefficent D(ix, iy) is constant.
482 */
483 cam_x_ptr[3] = fz * rd_factor;
484 cam_x_ptr[4] = 0.0;
485 cam_x_ptr[5] = -fz * rd_factor * ix;
486 cam_x_ptr[6] = -fz * rd_factor * ry * ix;
487 cam_x_ptr[7] = fz * rd_factor * (rz + rx * ix);
488 cam_x_ptr[8] = -fz * rd_factor * ry;
489
490 cam_y_ptr[3] = 0.0;
491 cam_y_ptr[4] = fz * rd_factor;
492 cam_y_ptr[5] = -fz * rd_factor * iy;
493 cam_y_ptr[6] = -fz * rd_factor * (rz + ry * iy);
494 cam_y_ptr[7] = fz * rd_factor * rx * iy;
495 cam_y_ptr[8] = fz * rd_factor * rx;
496
497 /*
498 * Compute point derivatives in x, y, and z.
499 */
500 point_x_ptr[0] = fz * rd_factor * (r[0] - r[6] * ix);
501 point_x_ptr[1] = fz * rd_factor * (r[1] - r[7] * ix);
502 point_x_ptr[2] = fz * rd_factor * (r[2] - r[8] * ix);
503
504 point_y_ptr[0] = fz * rd_factor * (r[3] - r[6] * iy);
505 point_y_ptr[1] = fz * rd_factor * (r[4] - r[7] * iy);
506 point_y_ptr[2] = fz * rd_factor * (r[5] - r[8] * iy);
507#elif JACOBIAN_APPROX_PBA
508 /* Computation of Jacobian approximation with one distortion argument. */
509
510 double rd_derivative_x;
511 double rd_derivative_y;
512
513 rd_derivative_x = 2.0 * ix * ix * (k[0] + 2.0 * k[1] * radius2);
514 rd_derivative_y = 2.0 * iy * iy * (k[0] + 2.0 * k[1] * radius2);
515 rd_derivative_x += rd_factor;
516 rd_derivative_y += rd_factor;
517
518 cam_x_ptr[3] = fz * rd_derivative_x;
519 cam_x_ptr[4] = 0.0;
520 cam_x_ptr[5] = -fz * rd_derivative_x * ix;
521 cam_x_ptr[6] = -fz * rd_derivative_x * ry * ix;
522 cam_x_ptr[7] = fz * rd_derivative_x * (rz + rx * ix);
523 cam_x_ptr[8] = -fz * rd_derivative_x * ry;
524
525 cam_y_ptr[3] = 0.0;
526 cam_y_ptr[4] = fz * rd_derivative_y;
527 cam_y_ptr[5] = -fz * rd_derivative_y * iy;
528 cam_y_ptr[6] = -fz * rd_derivative_y * (rz + ry * iy);
529 cam_y_ptr[7] = fz * rd_derivative_y * rx * iy;
530 cam_y_ptr[8] = fz * rd_derivative_y * rx;
531
532 /*
533 * Compute point derivatives in x, y, and z.
534 */
535 point_x_ptr[0] = fz * rd_derivative_x * (r[0] - r[6] * ix);
536 point_x_ptr[1] = fz * rd_derivative_x * (r[1] - r[7] * ix);
537 point_x_ptr[2] = fz * rd_derivative_x * (r[2] - r[8] * ix);
538
539 point_y_ptr[0] = fz * rd_derivative_y * (r[3] - r[6] * iy);
540 point_y_ptr[1] = fz * rd_derivative_y * (r[4] - r[7] * iy);
541 point_y_ptr[2] = fz * rd_derivative_y * (r[5] - r[8] * iy);
542
543#else
544 /* Computation of the full Jacobian. */
545
546 /*
547 * To keep everything comprehensible the chain rule
548 * is applied excessively
549 */
550 double const f = cam.focal_length;
551
552 double const rd_deriv_rad = k[0] + 2.0 * k[1] * radius2;
553
554 double const rad_deriv_px = 2.0 * ix / pz;
555 double const rad_deriv_py = 2.0 * iy / pz;
556 double const rad_deriv_pz = -2.0 * radius2 / pz;
557
558 double const rd_deriv_px = rd_deriv_rad * rad_deriv_px;
559 double const rd_deriv_py = rd_deriv_rad * rad_deriv_py;
560 double const rd_deriv_pz = rd_deriv_rad * rad_deriv_pz;
561
562 double const ix_deriv_px = 1 / pz;
563 double const ix_deriv_pz = -ix / pz;
564
565 double const iy_deriv_py = 1 / pz;
566 double const iy_deriv_pz = -iy / pz;
567
568 double const ix_deriv_r0 = -ix * ry / pz;
569 double const ix_deriv_r1 = (rz + rx * ix) / pz;
570 double const ix_deriv_r2 = -ry / pz;
571
572 double const iy_deriv_r0 = -(rz + ry * iy) / pz;
573 double const iy_deriv_r1 = rx * iy / pz;
574 double const iy_deriv_r2 = rx / pz;
575
576 double const rad_deriv_r0 = 2.0 * ix * ix_deriv_r0 + 2.0 * iy * iy_deriv_r0;
577 double const rad_deriv_r1 = 2.0 * ix * ix_deriv_r1 + 2.0 * iy * iy_deriv_r1;
578 double const rad_deriv_r2 = 2.0 * ix * ix_deriv_r2 + 2.0 * iy * iy_deriv_r2;
579
580 double const rd_deriv_r0 = rd_deriv_rad * rad_deriv_r0;
581 double const rd_deriv_r1 = rd_deriv_rad * rad_deriv_r1;
582 double const rd_deriv_r2 = rd_deriv_rad * rad_deriv_r2;
583
584 double const ix_deriv_X0 = (r[0] - r[6] * ix) / pz;
585 double const ix_deriv_X1 = (r[1] - r[7] * ix) / pz;
586 double const ix_deriv_X2 = (r[2] - r[8] * ix) / pz;
587
588 double const iy_deriv_X0 = (r[3] - r[6] * iy) / pz;
589 double const iy_deriv_X1 = (r[4] - r[7] * iy) / pz;
590 double const iy_deriv_X2 = (r[5] - r[8] * iy) / pz;
591
592 double const rad_deriv_X0 = 2.0 * ix * ix_deriv_X0 + 2.0 * iy * iy_deriv_X0;
593 double const rad_deriv_X1 = 2.0 * ix * ix_deriv_X1 + 2.0 * iy * iy_deriv_X1;
594 double const rad_deriv_X2 = 2.0 * ix * ix_deriv_X2 + 2.0 * iy * iy_deriv_X2;
595
596 double const rd_deriv_X0 = rd_deriv_rad * rad_deriv_X0;
597 double const rd_deriv_X1 = rd_deriv_rad * rad_deriv_X1;
598 double const rd_deriv_X2 = rd_deriv_rad * rad_deriv_X2;
599
600 /*
601 * Compute translation derivatives
602 * NOTE: px_deriv_t0 = 1
603 */
604 cam_x_ptr[3] = f * (rd_deriv_px * ix + rd_factor * ix_deriv_px);
605 cam_x_ptr[4] = f * (rd_deriv_py * ix); // + rd_factor * ix_deriv_py = 0
606 cam_x_ptr[5] = f * (rd_deriv_pz * ix + rd_factor * ix_deriv_pz);
607
608 cam_y_ptr[3] = f * (rd_deriv_px * iy); // + rd_factor * iy_deriv_px = 0
609 cam_y_ptr[4] = f * (rd_deriv_py * iy + rd_factor * iy_deriv_py);
610 cam_y_ptr[5] = f * (rd_deriv_pz * iy + rd_factor * iy_deriv_pz);
611
612 /*
613 * Compute rotation derivatives
614 */
615 cam_x_ptr[6] = f * (rd_deriv_r0 * ix + rd_factor * ix_deriv_r0);
616 cam_x_ptr[7] = f * (rd_deriv_r1 * ix + rd_factor * ix_deriv_r1);
617 cam_x_ptr[8] = f * (rd_deriv_r2 * ix + rd_factor * ix_deriv_r2);
618
619 cam_y_ptr[6] = f * (rd_deriv_r0 * iy + rd_factor * iy_deriv_r0);
620 cam_y_ptr[7] = f * (rd_deriv_r1 * iy + rd_factor * iy_deriv_r1);
621 cam_y_ptr[8] = f * (rd_deriv_r2 * iy + rd_factor * iy_deriv_r2);
622
623 /*
624 * Compute point derivatives in x, y, and z.
625 */
626 point_x_ptr[0] = f * (rd_deriv_X0 * ix + rd_factor * ix_deriv_X0);
627 point_x_ptr[1] = f * (rd_deriv_X1 * ix + rd_factor * ix_deriv_X1);
628 point_x_ptr[2] = f * (rd_deriv_X2 * ix + rd_factor * ix_deriv_X2);
629
630 point_y_ptr[0] = f * (rd_deriv_X0 * iy + rd_factor * iy_deriv_X0);
631 point_y_ptr[1] = f * (rd_deriv_X1 * iy + rd_factor * iy_deriv_X1);
632 point_y_ptr[2] = f * (rd_deriv_X2 * iy + rd_factor * iy_deriv_X2);
633
634#endif
635}
636
637void
638BundleAdjustment::update_parameters (DenseVectorType const& delta_x)
639{
640 /* Update cameras. */
641 std::size_t total_camera_params = 0;
642 if (this->opts.bundle_mode & BA_CAMERAS)
643 {
644 for (std::size_t i = 0; i < this->cameras->size(); ++i)
645 this->update_camera(this->cameras->at(i),
646 delta_x.data() + this->num_cam_params * i,
647 &this->cameras->at(i));
648 total_camera_params = this->cameras->size() * this->num_cam_params;
649 }
650
651 /* Update points. */
652 if (this->opts.bundle_mode & BA_POINTS)
653 {
654 for (std::size_t i = 0; i < this->points->size(); ++i)
655 this->update_point(this->points->at(i),
656 delta_x.data() + total_camera_params + i * 3,
657 &this->points->at(i));
658 }
659}
660
661void
662BundleAdjustment::update_camera (Camera const& cam,
663 double const* update, Camera* out)
664{
665 if (opts.fixed_intrinsics)
666 {
667 out->focal_length = cam.focal_length;
668 out->distortion[0] = cam.distortion[0];
669 out->distortion[1] = cam.distortion[1];
670 }
671 else
672 {
673 out->focal_length = cam.focal_length + update[0];
674 out->distortion[0] = cam.distortion[0] + update[1];
675 out->distortion[1] = cam.distortion[1] + update[2];
676 }
677
678 int const offset = this->opts.fixed_intrinsics ? 0 : 3;
679 out->translation[0] = cam.translation[0] + update[0 + offset];
680 out->translation[1] = cam.translation[1] + update[1 + offset];
681 out->translation[2] = cam.translation[2] + update[2 + offset];
682
683 double rot_orig[9];
684 std::copy(cam.rotation, cam.rotation + 9, rot_orig);
685 double rot_update[9];
686 this->rodrigues_to_matrix(update + 3 + offset, rot_update);
687 math::matrix_multiply(rot_update, 3, 3, rot_orig, 3, out->rotation);
688}
689
690void
691BundleAdjustment::update_point (Point3D const& pt,
692 double const* update, Point3D* out)
693{
694 out->pos[0] = pt.pos[0] + update[0];
695 out->pos[1] = pt.pos[1] + update[1];
696 out->pos[2] = pt.pos[2] + update[2];
697}
698
699void
700BundleAdjustment::print_status (bool detailed) const
701{
702 if (!detailed)
703 {
704 std::cout << "BA: MSE " << this->status.initial_mse
705 << " -> " << this->status.final_mse << ", "
706 << this->status.num_lm_iterations << " LM iters, "
707 << this->status.num_cg_iterations << " CG iters, "
708 << this->status.runtime_ms << "ms."
709 << std::endl;
710 return;
711 }
712
713 std::cout << "Bundle Adjustment Status:" << std::endl;
714 std::cout << " Initial MSE: "
715 << this->status.initial_mse << std::endl;
716 std::cout << " Final MSE: "
717 << this->status.final_mse << std::endl;
718 std::cout << " LM iterations: "
719 << this->status.num_lm_iterations << " ("
720 << this->status.num_lm_successful_iterations << " successful, "
721 << this->status.num_lm_unsuccessful_iterations << " unsuccessful)"
722 << std::endl;
723 std::cout << " CG iterations: "
724 << this->status.num_cg_iterations << std::endl;
725}
726
#define TRUST_REGION_RADIUS_DECREMENT
#define LOG_V
#define TRUST_REGION_RADIUS_INIT
Cross-platform high-resolution real-time timer.
Definition timer.h:30
std::size_t get_elapsed(void) const
Returns the milli seconds since last reset.
Definition timer.h:94
#define MATH_POW3(x)
Definition defines.h:69
void matrix_multiply(T const *mat_a, int rows_a, int cols_a, T const *mat_b, int cols_b, T *mat_res)
Matrix multiplication of dynamically sized dense matrices.
STL namespace.
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
Definition image.h:478
#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
Observation of a 3D point for a camera.
Definition ba_types.h:32