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()
26#define TRUST_REGION_RADIUS_INIT (1000)
27#define TRUST_REGION_RADIUS_DECREMENT (1.0 / 2.0)
32BundleAdjustment::Status
33BundleAdjustment::optimize (
void)
36 this->sanity_checks();
44BundleAdjustment::sanity_checks (
void)
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");
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");
60 for (std::size_t i = 0; i < this->observations->size(); ++i)
64 || obs.
camera_id >=
static_cast<int>(this->cameras->size()))
65 throw std::invalid_argument(
"Observation with invalid camera ID");
67 || obs.
point_id >=
static_cast<int>(this->points->size()))
68 throw std::invalid_argument(
"Observation with invalid track ID");
73BundleAdjustment::lm_optimize (
void)
76 LinearSolver::Options pcg_opts;
77 pcg_opts = this->opts.linear_opts;
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;
88 for (
int lm_iter = 0; ; ++lm_iter)
90 if (lm_iter + 1 > this->opts.lm_min_iterations
91 && (current_mse < this->opts.lm_mse_threshold))
93 LOG_V <<
"BA: Satisfied MSE threshold." << std::endl;
98 SparseMatrixType Jc, Jp;
99 switch (this->opts.bundle_mode)
101 case BA_CAMERAS_AND_POINTS:
102 this->analytic_jacobian(&Jc, &Jp);
105 this->analytic_jacobian(&Jc,
nullptr);
108 this->analytic_jacobian(
nullptr, &Jp);
111 throw std::runtime_error(
"Invalid bundle mode");
115 DenseVectorType delta_x;
116 LinearSolver pcg(pcg_opts);
117 LinearSolver::Status cg_status = pcg.solve(Jc, Jp, F, &delta_x);
120 double new_mse, delta_mse, delta_mse_ratio = 1.0;
121 if (cg_status.success)
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;
131 new_mse = current_mse;
134 bool successful_iteration = delta_mse > 0.0;
140 if (successful_iteration)
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
150 this->status.num_lm_iterations += 1;
151 this->status.num_lm_successful_iterations += 1;
152 this->update_parameters(delta_x);
154 current_mse = new_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;
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
173 this->status.num_lm_iterations += 1;
174 this->status.num_lm_unsuccessful_iterations += 1;
179 if (lm_iter + 1 < this->opts.lm_min_iterations)
181 if (lm_iter + 1 >= this->opts.lm_max_iterations)
183 LOG_V <<
"BA: Reached maximum LM iterations of "
184 << this->opts.lm_max_iterations << std::endl;
189 if (successful_iteration)
191 if (delta_mse_ratio < this->opts.lm_delta_threshold)
193 LOG_V <<
"BA: Satisfied delta mse ratio threshold of "
194 << this->opts.lm_delta_threshold << std::endl;
200 this->status.final_mse = current_mse;
204BundleAdjustment::compute_reprojection_errors (DenseVectorType* vector_f,
205 DenseVectorType
const* delta_x)
207 if (vector_f->size() != this->observations->size() * 2)
208 vector_f->resize(this->observations->size() * 2);
210#pragma omp parallel for
211 for (std::size_t i = 0; i < this->observations->size(); ++i)
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);
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;
225 if (delta_x !=
nullptr)
227 std::size_t cam_id = obs.camera_id * this->num_cam_params;
228 std::size_t pt_id = obs.point_id * 3;
230 if (this->opts.bundle_mode & BA_CAMERAS)
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;
240 if (this->opts.bundle_mode & BA_POINTS)
242 this->update_point(p3d, delta_x->data() + pt_id, &new_point);
243 point = new_point.pos;
248 double rp[] = { 0.0, 0.0, 0.0 };
249 for (
int d = 0; d < 3; ++d)
251 rp[0] += rot[0 + d] * point[d];
252 rp[1] += rot[3 + d] * point[d];
253 rp[2] += rot[6 + d] * point[d];
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];
260 this->radial_distort(rp + 0, rp + 1, dist);
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];
269BundleAdjustment::compute_mse (DenseVectorType
const& vector_f)
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);
278BundleAdjustment::radial_distort (
double* x,
double* y,
double const* dist)
280 double const radius2 = *x * *x + *y * *y;
281 double const factor = 1.0 + radius2 * (dist[0] + dist[1] * radius2);
287BundleAdjustment::rodrigues_to_matrix (
double const* r,
double* m)
290 double a = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
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;
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;
307BundleAdjustment::analytic_jacobian (SparseMatrixType* jac_cam,
308 SparseMatrixType* jac_points)
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;
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);
324 double cam_x_ptr[9], cam_y_ptr[9], point_x_ptr[3], point_y_ptr[3];
326 for (std::size_t i = 0; i < this->observations->size(); ++i)
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);
336 std::fill(point_x_ptr, point_x_ptr + 3, 0.0);
337 std::fill(point_y_ptr, point_y_ptr + 3, 0.0);
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)
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]);
351 std::size_t point_offset = i * 3 * 2;
352 for (
int j = 0; jac_points !=
nullptr && j < 3; ++j)
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]);
364 if (jac_cam !=
nullptr)
366 jac_cam->allocate(jacobi_rows, camera_cols);
367 jac_cam->set_from_triplets(cam_triplets);
371 if (jac_points !=
nullptr)
373 jac_points->allocate(jacobi_rows, point_cols);
374 jac_points->set_from_triplets(point_triplets);
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)
419 double const* r = cam.rotation;
420 double const* t = cam.translation;
421 double const* k = cam.distortion;
422 double const* p3d = point.pos;
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;
438 if (this->opts.fixed_intrinsics)
440 cam_x_ptr[0] = fz * rd_factor;
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;
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;
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);
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);
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;
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;
476#define JACOBIAN_APPROX_CONST_RD 0
477#define JACOBIAN_APPROX_PBA 0
478#if JACOBIAN_APPROX_CONST_RD
483 cam_x_ptr[3] = fz * rd_factor;
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;
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;
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);
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
510 double rd_derivative_x;
511 double rd_derivative_y;
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;
518 cam_x_ptr[3] = fz * rd_derivative_x;
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;
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;
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);
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);
550 double const f = cam.focal_length;
552 double const rd_deriv_rad = k[0] + 2.0 * k[1] * radius2;
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;
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;
562 double const ix_deriv_px = 1 / pz;
563 double const ix_deriv_pz = -ix / pz;
565 double const iy_deriv_py = 1 / pz;
566 double const iy_deriv_pz = -iy / pz;
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;
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;
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;
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;
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;
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;
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;
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;
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);
606 cam_x_ptr[5] = f * (rd_deriv_pz * ix + rd_factor * ix_deriv_pz);
608 cam_y_ptr[3] = f * (rd_deriv_px * iy);
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);
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);
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);
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);
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);
638BundleAdjustment::update_parameters (DenseVectorType
const& delta_x)
641 std::size_t total_camera_params = 0;
642 if (this->opts.bundle_mode & BA_CAMERAS)
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;
652 if (this->opts.bundle_mode & BA_POINTS)
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));
662BundleAdjustment::update_camera (Camera
const& cam,
663 double const* update, Camera* out)
665 if (opts.fixed_intrinsics)
667 out->focal_length = cam.focal_length;
668 out->distortion[0] = cam.distortion[0];
669 out->distortion[1] = cam.distortion[1];
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];
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];
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);
691BundleAdjustment::update_point (Point3D
const& pt,
692 double const* update, Point3D* out)
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];
700BundleAdjustment::print_status (
bool detailed)
const
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."
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)"
723 std::cout <<
" CG iterations: "
724 << this->status.num_cg_iterations << std::endl;
#define TRUST_REGION_RADIUS_DECREMENT
#define TRUST_REGION_RADIUS_INIT
Cross-platform high-resolution real-time timer.
std::size_t get_elapsed(void) const
Returns the milli seconds since last reset.
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.
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
#define SFM_BA_NAMESPACE_BEGIN
#define SFM_NAMESPACE_END
#define SFM_NAMESPACE_BEGIN
#define SFM_BA_NAMESPACE_END
Observation of a 3D point for a camera.