Blender V5.3
bundle.cc
Go to the documentation of this file.
1// Copyright (c) 2011, 2012, 2013 libmv authors.
2//
3// Permission is hereby granted, free of charge, to any person obtaining a copy
4// of this software and associated documentation files (the "Software"), to
5// deal in the Software without restriction, including without limitation the
6// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7// sell copies of the Software, and to permit persons to whom the Software is
8// furnished to do so, subject to the following conditions:
9//
10// The above copyright notice and this permission notice shall be included in
11// all copies or substantial portions of the Software.
12//
13// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19// IN THE SOFTWARE.
20
22
23#include <map>
24#include <thread>
25
26#include "ceres/ceres.h"
27#include "ceres/rotation.h"
28#include "libmv/base/map.h"
29#include "libmv/base/vector.h"
39
40namespace libmv {
41
42namespace {
43
44bool NeedUseInvertIntrinsicsPipeline(const CameraIntrinsics* intrinsics) {
45 const DistortionModelType distortion_model =
46 intrinsics->GetDistortionModelType();
47 return (distortion_model == DISTORTION_MODEL_NUKE);
48}
49
50// Apply distortion model (distort the input) on the input point in the
51// normalized space to get distorted coordinate in the image space.
52//
53// Using intrinsics values from the parameter block, which makes this function
54// suitable for use from a cost functor.
55//
56// Only use for distortion models which are analytically defined for their
57// Apply() function.
58//
59// The invariant_intrinsics are used to access intrinsics which are never
60// packed into parameter block: for example, distortion model type and image
61// dimension.
62template <typename T>
63void ApplyDistortionModelUsingIntrinsicsBlock(
64 const CameraIntrinsics* invariant_intrinsics,
65 const T* const intrinsics_block,
66 const T& normalized_x,
67 const T& normalized_y,
68 T* distorted_x,
69 T* distorted_y) {
70 // Unpack the intrinsics.
71 const T& focal_length =
73 const T& principal_point_x =
75 const T& principal_point_y =
77
78 // TODO(keir): Do early bailouts for zero distortion; these are expensive
79 // jet operations.
80 switch (invariant_intrinsics->GetDistortionModelType()) {
82 const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
83 const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
84 const T& k3 = intrinsics_block[PackedIntrinsics::OFFSET_K3];
85 const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1];
86 const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2];
87
89 focal_length,
90 principal_point_x,
91 principal_point_y,
92 k1,
93 k2,
94 k3,
95 p1,
96 p2,
97 normalized_x,
98 normalized_y,
99 distorted_x,
100 distorted_y);
101 return;
102 }
103
105 const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
106 const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
107
108 ApplyDivisionDistortionModel(focal_length,
109 focal_length,
110 principal_point_x,
111 principal_point_y,
112 k1,
113 k2,
114 normalized_x,
115 normalized_y,
116 distorted_x,
117 distorted_y);
118 return;
119 }
120
122 LOG(FATAL) << "Unsupported distortion model.";
123 return;
124 }
125
127 const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
128 const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
129 const T& k3 = intrinsics_block[PackedIntrinsics::OFFSET_K3];
130 const T& k4 = intrinsics_block[PackedIntrinsics::OFFSET_K4];
131 const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1];
132 const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2];
133
134 ApplyBrownDistortionModel(focal_length,
135 focal_length,
136 principal_point_x,
137 principal_point_y,
138 k1,
139 k2,
140 k3,
141 k4,
142 p1,
143 p2,
144 normalized_x,
145 normalized_y,
146 distorted_x,
147 distorted_y);
148 return;
149 }
150 }
151
152 LOG(FATAL) << "Unknown distortion model.";
153}
154
155// Invert distortion model (undistort the input) on the input point in the
156// image space to get undistorted coordinate in the normalized space.
157//
158// Using intrinsics values from the parameter block, which makes this function
159// suitable for use from a cost functor.
160//
161// Only use for distortion models which are analytically defined for their
162// Invert() function.
163//
164// The invariant_intrinsics are used to access intrinsics which are never
165// packed into parameter block: for example, distortion model type and image
166// dimension.
167template <typename T>
168void InvertDistortionModelUsingIntrinsicsBlock(
169 const CameraIntrinsics* invariant_intrinsics,
170 const T* const intrinsics_block,
171 const T& image_x,
172 const T& image_y,
173 T* normalized_x,
174 T* normalized_y) {
175 // Unpack the intrinsics.
176 const T& focal_length =
178 const T& principal_point_x =
180 const T& principal_point_y =
182
183 // TODO(keir): Do early bailouts for zero distortion; these are expensive
184 // jet operations.
185 switch (invariant_intrinsics->GetDistortionModelType()) {
189 LOG(FATAL) << "Unsupported distortion model.";
190 return;
191
193 const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1];
194 const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2];
195 const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1];
196 const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2];
197
198 InvertNukeDistortionModel(focal_length,
199 focal_length,
200 principal_point_x,
201 principal_point_y,
202 invariant_intrinsics->image_width(),
203 invariant_intrinsics->image_height(),
204 k1,
205 k2,
206 p1,
207 p2,
208 image_x,
209 image_y,
210 normalized_x,
211 normalized_y);
212 return;
213 }
214 }
215
216 LOG(FATAL) << "Unknown distortion model.";
217}
218
219template <typename T>
220void NormalizedToImageSpace(const T* const intrinsics_block,
221 const T& normalized_x,
222 const T& normalized_y,
223 T* image_x,
224 T* image_y) {
225 // Unpack the intrinsics.
226 const T& focal_length =
228 const T& principal_point_x =
230 const T& principal_point_y =
232
233 *image_x = normalized_x * focal_length + principal_point_x;
234 *image_y = normalized_y * focal_length + principal_point_y;
235}
236
237// Cost functor which computes reprojection error of 3D point X on camera
238// defined by angle-axis rotation and its translation (which are in the same
239// block due to optimization reasons).
240//
241// This functor can only be used for distortion models which have analytically
242// defined Apply() function.
243struct ReprojectionErrorApplyIntrinsics {
244 ReprojectionErrorApplyIntrinsics(const CameraIntrinsics* invariant_intrinsics,
245 const double observed_distorted_x,
246 const double observed_distorted_y,
247 const double weight)
248 : invariant_intrinsics_(invariant_intrinsics),
249 observed_distorted_x_(observed_distorted_x),
250 observed_distorted_y_(observed_distorted_y),
251 weight_(weight) {}
252
253 template <typename T>
254 bool operator()(const T* const intrinsics,
255 const T* const R_t, // Rotation denoted by angle axis
256 // followed with translation
257 const T* const X, // Point coordinates 3x1.
258 T* residuals) const {
259 // Compute projective coordinates: x = RX + t.
260 T x[3];
261
262 ceres::AngleAxisRotatePoint(R_t, X, x);
263 x[0] += R_t[3];
264 x[1] += R_t[4];
265 x[2] += R_t[5];
266
267 // Prevent points from going behind the camera.
268 if (x[2] < T(0)) {
269 return false;
270 }
271
272 // Compute normalized coordinates: x /= x[2].
273 T xn = x[0] / x[2];
274 T yn = x[1] / x[2];
275
276 T predicted_distorted_x, predicted_distorted_y;
277 ApplyDistortionModelUsingIntrinsicsBlock(invariant_intrinsics_,
278 intrinsics,
279 xn,
280 yn,
281 &predicted_distorted_x,
282 &predicted_distorted_y);
283
284 // The error is the difference between the predicted and observed position.
285 residuals[0] = (predicted_distorted_x - T(observed_distorted_x_)) * weight_;
286 residuals[1] = (predicted_distorted_y - T(observed_distorted_y_)) * weight_;
287 return true;
288 }
289
290 const CameraIntrinsics* invariant_intrinsics_;
291 const double observed_distorted_x_;
292 const double observed_distorted_y_;
293 const double weight_;
294};
295
296// Cost functor which computes reprojection error of 3D point X on camera
297// defined by angle-axis rotation and its translation (which are in the same
298// block due to optimization reasons).
299//
300// This functor can only be used for distortion models which have analytically
301// defined Invert() function.
302struct ReprojectionErrorInvertIntrinsics {
303 ReprojectionErrorInvertIntrinsics(
304 const CameraIntrinsics* invariant_intrinsics,
305 const double observed_distorted_x,
306 const double observed_distorted_y,
307 const double weight)
308 : invariant_intrinsics_(invariant_intrinsics),
309 observed_distorted_x_(observed_distorted_x),
310 observed_distorted_y_(observed_distorted_y),
311 weight_(weight) {}
312
313 template <typename T>
314 bool operator()(const T* const intrinsics,
315 const T* const R_t, // Rotation denoted by angle axis
316 // followed with translation
317 const T* const X, // Point coordinates 3x1.
318 T* residuals) const {
319 // Unpack the intrinsics.
320 const T& focal_length = intrinsics[PackedIntrinsics::OFFSET_FOCAL_LENGTH];
321 const T& principal_point_x =
323 const T& principal_point_y =
325
326 // Compute projective coordinates: x = RX + t.
327 T x[3];
328
329 ceres::AngleAxisRotatePoint(R_t, X, x);
330 x[0] += R_t[3];
331 x[1] += R_t[4];
332 x[2] += R_t[5];
333
334 // Prevent points from going behind the camera.
335 if (x[2] < T(0)) {
336 return false;
337 }
338
339 // Compute normalized coordinates: x /= x[2].
340 T xn = x[0] / x[2];
341 T yn = x[1] / x[2];
342
343 // Compute image space coordinate from normalized.
344 T predicted_x = focal_length * xn + principal_point_x;
345 T predicted_y = focal_length * yn + principal_point_y;
346
347 T observed_undistorted_normalized_x, observed_undistorted_normalized_y;
348 InvertDistortionModelUsingIntrinsicsBlock(
349 invariant_intrinsics_,
350 intrinsics,
351 T(observed_distorted_x_),
352 T(observed_distorted_y_),
353 &observed_undistorted_normalized_x,
354 &observed_undistorted_normalized_y);
355
356 T observed_undistorted_image_x, observed_undistorted_image_y;
357 NormalizedToImageSpace(intrinsics,
358 observed_undistorted_normalized_x,
359 observed_undistorted_normalized_y,
360 &observed_undistorted_image_x,
361 &observed_undistorted_image_y);
362
363 // The error is the difference between the predicted and observed position.
364 residuals[0] = (predicted_x - observed_undistorted_image_x) * weight_;
365 residuals[1] = (predicted_y - observed_undistorted_image_y) * weight_;
366
367 return true;
368 }
369
370 const CameraIntrinsics* invariant_intrinsics_;
371 const double observed_distorted_x_;
372 const double observed_distorted_y_;
373 const double weight_;
374};
375
376// Print a message to the log which camera intrinsics are gonna to be optimized.
377void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
378 if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
379 LG << "Bundling only camera positions.";
380 } else {
381 std::string bundling_message = "";
382
383#define APPEND_BUNDLING_INTRINSICS(name, flag) \
384 if (bundle_intrinsics & flag) { \
385 if (!bundling_message.empty()) { \
386 bundling_message += ", "; \
387 } \
388 bundling_message += name; \
389 } \
390 (void)0
391
400
401 LG << "Bundling " << bundling_message << ".";
402 }
403}
404
405// Get a vector of camera's rotations denoted by angle axis
406// conjuncted with translations into single block
407//
408// Element with key i matches to a rotation+translation for
409// camera at image i.
410map<int, Vec6> PackCamerasRotationAndTranslation(
411 const EuclideanReconstruction& reconstruction) {
412 map<int, Vec6> all_cameras_R_t;
413
414 vector<EuclideanCamera> all_cameras = reconstruction.AllCameras();
415 for (const EuclideanCamera& camera : all_cameras) {
416 Vec6 camera_R_t;
417 ceres::RotationMatrixToAngleAxis(&camera.R(0, 0), &camera_R_t(0));
418 camera_R_t.tail<3>() = camera.t;
419 all_cameras_R_t.insert(make_pair(camera.image, camera_R_t));
420 }
421
422 return all_cameras_R_t;
423}
424
425// Convert cameras rotations fro mangle axis back to rotation matrix.
426void UnpackCamerasRotationAndTranslation(
427 const map<int, Vec6>& all_cameras_R_t,
428 EuclideanReconstruction* reconstruction) {
429 for (map<int, Vec6>::value_type image_and_camera_R_T : all_cameras_R_t) {
430 const int image = image_and_camera_R_T.first;
431 const Vec6& camera_R_t = image_and_camera_R_T.second;
432
433 EuclideanCamera* camera = reconstruction->CameraForImage(image);
434 if (!camera) {
435 continue;
436 }
437
438 ceres::AngleAxisToRotationMatrix(&camera_R_t(0), &camera->R(0, 0));
439 camera->t = camera_R_t.tail<3>();
440 }
441}
442
443// Converts sparse CRSMatrix to Eigen matrix, so it could be used
444// all over in the pipeline.
445//
446// TODO(sergey): currently uses dense Eigen matrices, best would
447// be to use sparse Eigen matrices
448void CRSMatrixToEigenMatrix(const ceres::CRSMatrix& crs_matrix,
449 Mat* eigen_matrix) {
450 eigen_matrix->resize(crs_matrix.num_rows, crs_matrix.num_cols);
451 eigen_matrix->setZero();
452
453 for (int row = 0; row < crs_matrix.num_rows; ++row) {
454 int start = crs_matrix.rows[row];
455 int end = crs_matrix.rows[row + 1] - 1;
456
457 for (int i = start; i <= end; i++) {
458 int col = crs_matrix.cols[i];
459 double value = crs_matrix.values[i];
460
461 (*eigen_matrix)(row, col) = value;
462 }
463 }
464}
465
466void EuclideanBundlerPerformEvaluation(const Tracks& tracks,
467 EuclideanReconstruction* reconstruction,
468 map<int, Vec6>* all_cameras_R_t,
469 ceres::Problem* problem,
470 BundleEvaluation* evaluation) {
471 int max_track = tracks.MaxTrack();
472 // Number of camera rotations equals to number of translation,
473 int num_cameras = all_cameras_R_t->size();
474 int num_points = 0;
475
476 vector<EuclideanPoint*> minimized_points;
477 for (int i = 0; i <= max_track; i++) {
478 EuclideanPoint* point = reconstruction->PointForTrack(i);
479 if (point) {
480 // We need to know whether the track is a constant zero weight.
481 // If it is so it wouldn't have a parameter block in the problem.
482 //
483 // Usually getting all markers of a track is considered slow, but this
484 // code is only used by the keyframe selection code where there aren't
485 // that many tracks in the storage and there are only 2 frames for each
486 // of the tracks.
487 vector<Marker> markera_of_track = tracks.MarkersForTrack(i);
488 for (int j = 0; j < markera_of_track.size(); j++) {
489 if (markera_of_track.at(j).weight != 0.0) {
490 minimized_points.push_back(point);
491 num_points++;
492 break;
493 }
494 }
495 }
496 }
497
498 LG << "Number of cameras " << num_cameras;
499 LG << "Number of points " << num_points;
500
501 evaluation->num_cameras = num_cameras;
502 evaluation->num_points = num_points;
503
504 if (evaluation->evaluate_jacobian) { // Evaluate jacobian matrix.
505 ceres::CRSMatrix evaluated_jacobian;
506 ceres::Problem::EvaluateOptions eval_options;
507
508 // Cameras goes first in the ordering.
509 int max_image = tracks.MaxImage();
510 for (int i = 0; i <= max_image; i++) {
511 const EuclideanCamera* camera = reconstruction->CameraForImage(i);
512 if (camera) {
513 double* current_camera_R_t = &(*all_cameras_R_t)[i](0);
514
515 // All cameras are variable now.
516 problem->SetParameterBlockVariable(current_camera_R_t);
517
518 eval_options.parameter_blocks.push_back(current_camera_R_t);
519 }
520 }
521
522 // Points goes at the end of ordering,
523 for (int i = 0; i < minimized_points.size(); i++) {
524 EuclideanPoint* point = minimized_points.at(i);
525 eval_options.parameter_blocks.push_back(&point->X(0));
526 }
527
528 problem->Evaluate(eval_options, NULL, NULL, NULL, &evaluated_jacobian);
529
530 CRSMatrixToEigenMatrix(evaluated_jacobian, &evaluation->jacobian);
531 }
532}
533
534template <typename CostFunction>
535void AddResidualBlockToProblemImpl(const CameraIntrinsics* invariant_intrinsics,
536 double observed_x,
537 double observed_y,
538 double weight,
539 double* intrinsics_block,
540 double* camera_R_t,
541 EuclideanPoint* point,
542 ceres::Problem* problem) {
543 problem->AddResidualBlock(
544 new ceres::AutoDiffCostFunction<CostFunction,
545 2,
547 6,
548 3>(new CostFunction(
549 invariant_intrinsics, observed_x, observed_y, weight)),
550 NULL,
551 intrinsics_block,
552 camera_R_t,
553 &point->X(0));
554}
555
556void AddResidualBlockToProblem(const CameraIntrinsics* invariant_intrinsics,
557 const Marker& marker,
558 double marker_weight,
559 double* intrinsics_block,
560 double* camera_R_t,
561 EuclideanPoint* point,
562 ceres::Problem* problem) {
563 if (NeedUseInvertIntrinsicsPipeline(invariant_intrinsics)) {
564 AddResidualBlockToProblemImpl<ReprojectionErrorInvertIntrinsics>(
565 invariant_intrinsics,
566 marker.x,
567 marker.y,
568 marker_weight,
569 intrinsics_block,
570 camera_R_t,
571 point,
572 problem);
573 } else {
574 AddResidualBlockToProblemImpl<ReprojectionErrorApplyIntrinsics>(
575 invariant_intrinsics,
576 marker.x,
577 marker.y,
578 marker_weight,
579 intrinsics_block,
580 camera_R_t,
581 point,
582 problem);
583 }
584}
585
586// This is an utility function to only bundle 3D position of
587// given markers list.
588//
589// Main purpose of this function is to adjust positions of tracks
590// which does have constant zero weight and so far only were using
591// algebraic intersection to obtain their 3D positions.
592//
593// At this point we only need to bundle points positions, cameras
594// are to be totally still here.
595void EuclideanBundlePointsOnly(const CameraIntrinsics* invariant_intrinsics,
596 const vector<Marker>& markers,
597 map<int, Vec6>& all_cameras_R_t,
598 double* intrinsics_block,
599 EuclideanReconstruction* reconstruction) {
600 ceres::Problem::Options problem_options;
601 ceres::Problem problem(problem_options);
602 int num_residuals = 0;
603 for (int i = 0; i < markers.size(); ++i) {
604 const Marker& marker = markers[i];
605 EuclideanCamera* camera = reconstruction->CameraForImage(marker.image);
606 EuclideanPoint* point = reconstruction->PointForTrack(marker.track);
607 if (camera == NULL || point == NULL) {
608 continue;
609 }
610
611 // Rotation of camera denoted in angle axis followed with
612 // camera translation.
613 double* current_camera_R_t = &all_cameras_R_t[camera->image](0);
614
615 AddResidualBlockToProblem(invariant_intrinsics,
616 marker,
617 1.0,
618 intrinsics_block,
619 current_camera_R_t,
620 point,
621 &problem);
622
623 problem.SetParameterBlockConstant(current_camera_R_t);
624 num_residuals++;
625 }
626
627 LG << "Number of residuals: " << num_residuals;
628 if (!num_residuals) {
629 LG << "Skipping running minimizer with zero residuals";
630 return;
631 }
632
633 problem.SetParameterBlockConstant(intrinsics_block);
634
635 // Configure the solver.
636 ceres::Solver::Options options;
637 options.use_nonmonotonic_steps = true;
638 options.preconditioner_type = ceres::SCHUR_JACOBI;
639 options.linear_solver_type = ceres::ITERATIVE_SCHUR;
640 options.use_explicit_schur_complement = true;
641 options.use_inner_iterations = true;
642 options.max_num_iterations = 100;
643 options.num_threads = std::thread::hardware_concurrency();
644
645 // Solve!
646 ceres::Solver::Summary summary;
647 ceres::Solve(options, &problem, &summary);
648
649 LG << "Final report:\n" << summary.FullReport();
650}
651
652} // namespace
653
654void EuclideanBundle(const Tracks& tracks,
655 EuclideanReconstruction* reconstruction) {
656 PolynomialCameraIntrinsics empty_intrinsics;
660 reconstruction,
661 &empty_intrinsics,
662 NULL);
663}
664
666 const int bundle_intrinsics,
667 const int bundle_constraints,
668 EuclideanReconstruction* reconstruction,
669 CameraIntrinsics* intrinsics,
670 BundleEvaluation* evaluation) {
671 LG << "Original intrinsics: " << *intrinsics;
672 vector<Marker> markers = tracks.AllMarkers();
673
674 // N-th element denotes whether track N is a constant zero-weighted track.
675 vector<bool> zero_weight_tracks_flags(tracks.MaxTrack() + 1, true);
676
677 // Pack all intrinsics parameters into a single block and rely on local
678 // parameterizations to control which intrinsics are allowed to vary.
679 PackedIntrinsics packed_intrinsics;
680 intrinsics->Pack(&packed_intrinsics);
681 double* intrinsics_block = packed_intrinsics.GetParametersBlock();
682
683 // Convert cameras rotations to angle axis and merge with translation
684 // into single parameter block for maximal minimization speed.
685 //
686 // Block for minimization has got the following structure:
687 // <3 elements for angle-axis> <3 elements for translation>
688 map<int, Vec6> all_cameras_R_t =
689 PackCamerasRotationAndTranslation(*reconstruction);
690
691 // Parameterization used to restrict camera motion for modal solvers.
692 ceres::SubsetManifold* constant_translation_manifold = NULL;
693 if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
694 std::vector<int> constant_translation;
695
696 // First three elements are rotation, ast three are translation.
697 constant_translation.push_back(3);
698 constant_translation.push_back(4);
699 constant_translation.push_back(5);
700
701 constant_translation_manifold =
702 new ceres::SubsetManifold(6, constant_translation);
703 }
704
705 // Add residual blocks to the problem.
706 ceres::Problem::Options problem_options;
707 ceres::Problem problem(problem_options);
708 int num_residuals = 0;
709 bool have_locked_camera = false;
710 for (int i = 0; i < markers.size(); ++i) {
711 const Marker& marker = markers[i];
712 EuclideanCamera* camera = reconstruction->CameraForImage(marker.image);
713 EuclideanPoint* point = reconstruction->PointForTrack(marker.track);
714 if (camera == NULL || point == NULL) {
715 continue;
716 }
717
718 // Rotation of camera denoted in angle axis followed with
719 // camera translation.
720 double* current_camera_R_t = &all_cameras_R_t[camera->image](0);
721
722 // Skip residual block for markers which does have absolutely
723 // no affect on the final solution.
724 // This way ceres is not gonna to go crazy.
725 if (marker.weight != 0.0) {
726 AddResidualBlockToProblem(intrinsics,
727 marker,
728 marker.weight,
729 intrinsics_block,
730 current_camera_R_t,
731 point,
732 &problem);
733
734 // We lock the first camera to better deal with scene orientation
735 // ambiguity.
736 if (!have_locked_camera) {
737 problem.SetParameterBlockConstant(current_camera_R_t);
738 have_locked_camera = true;
739 }
740
741 if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
742 problem.SetManifold(current_camera_R_t, constant_translation_manifold);
743 }
744
745 zero_weight_tracks_flags[marker.track] = false;
746 num_residuals++;
747 }
748 }
749 LG << "Number of residuals: " << num_residuals;
750
751 if (!num_residuals) {
752 LG << "Skipping running minimizer with zero residuals";
753 return;
754 }
755
757 (bundle_intrinsics & BUNDLE_TANGENTIAL) != 0) {
758 LOG(FATAL) << "Division model doesn't support bundling "
759 "of tangential distortion";
760 }
761
762 BundleIntrinsicsLogMessage(bundle_intrinsics);
763
764 if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
765 // No camera intrinsics are being refined,
766 // set the whole parameter block as constant for best performance.
767 problem.SetParameterBlockConstant(intrinsics_block);
768 } else {
769 // Set the camera intrinsics that are not to be bundled as
770 // constant using some macro trickery.
771
772 std::vector<int> constant_intrinsics;
773#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
774 if (!(bundle_intrinsics & bundle_enum) || \
775 !packed_intrinsics.IsParameterDefined(offset)) { \
776 constant_intrinsics.push_back(offset); \
777 }
790#undef MAYBE_SET_CONSTANT
791
792 if (!constant_intrinsics.empty()) {
793 ceres::SubsetManifold* subset_parameterization =
794 new ceres::SubsetManifold(PackedIntrinsics::NUM_PARAMETERS,
795 constant_intrinsics);
796
797 problem.SetManifold(intrinsics_block, subset_parameterization);
798 }
799 }
800
801 // Configure the solver.
802 ceres::Solver::Options options;
803 options.use_nonmonotonic_steps = true;
804 options.preconditioner_type = ceres::SCHUR_JACOBI;
805 options.linear_solver_type = ceres::ITERATIVE_SCHUR;
806 options.use_explicit_schur_complement = true;
807 options.use_inner_iterations = true;
808 options.max_num_iterations = 100;
809 options.num_threads = std::thread::hardware_concurrency();
810
811 // Solve!
812 ceres::Solver::Summary summary;
813 ceres::Solve(options, &problem, &summary);
814
815 LG << "Final report:\n" << summary.FullReport();
816
817 // Copy rotations and translations back.
818 UnpackCamerasRotationAndTranslation(all_cameras_R_t, reconstruction);
819
820 // Copy intrinsics back.
821 if (bundle_intrinsics != BUNDLE_NO_INTRINSICS) {
822 intrinsics->Unpack(packed_intrinsics);
823 }
824
825 LG << "Final intrinsics: " << *intrinsics;
826
827 if (evaluation) {
828 EuclideanBundlerPerformEvaluation(
829 tracks, reconstruction, &all_cameras_R_t, &problem, evaluation);
830 }
831
832 // Separate step to adjust positions of tracks which are
833 // constant zero-weighted.
834 vector<Marker> zero_weight_markers;
835 for (int track = 0; track < tracks.MaxTrack(); ++track) {
836 if (zero_weight_tracks_flags[track]) {
837 vector<Marker> current_markers = tracks.MarkersForTrack(track);
838 zero_weight_markers.reserve(zero_weight_markers.size() +
839 current_markers.size());
840 for (int i = 0; i < current_markers.size(); ++i) {
841 zero_weight_markers.push_back(current_markers[i]);
842 }
843 }
844 }
845
846 if (zero_weight_markers.size()) {
847 LG << "Refining position of constant zero-weighted tracks";
848 EuclideanBundlePointsOnly(intrinsics,
849 zero_weight_markers,
850 all_cameras_R_t,
851 intrinsics_block,
852 reconstruction);
853 }
854}
855
856void ProjectiveBundle(const Tracks& /*tracks*/,
857 ProjectiveReconstruction* /*reconstruction*/) {
858 // TODO(keir): Implement this! This can't work until we have a better bundler
859 // than SSBA, since SSBA has no support for projective bundling.
860}
861
862} // namespace libmv
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition btTransform.h:90
#define MAYBE_SET_CONSTANT(bundle_enum, offset)
#define APPEND_BUNDLING_INTRINSICS(name, flag)
virtual DistortionModelType GetDistortionModelType() const =0
vector< EuclideanCamera > AllCameras() const
Returns all cameras.
EuclideanPoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
EuclideanCamera * CameraForImage(int image)
Returns a pointer to the camera corresponding to image.
int MaxTrack() const
Returns the maximum track identifier used.
int MaxImage() const
Returns the maximum image identifier used.
vector< Marker > MarkersForTrack(int track) const
Returns all the markers belonging to a track.
virtual void Unpack(const PackedIntrinsics &packed_intrinsics)
virtual void Pack(PackedIntrinsics *packed_intrinsics) const
virtual DistortionModelType GetDistortionModelType() const =0
EuclideanPoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
EuclideanCamera * CameraForImage(int image)
Returns a pointer to the camera corresponding to image.
int MaxTrack() const
Returns the maximum track identifier used.
vector< Marker > AllMarkers() const
Returns all the markers.
vector< Marker > MarkersForTrack(int track) const
Returns all the markers belonging to a track.
CCL_NAMESPACE_BEGIN struct Options options
#define image(slot, qualifiers, format)
void camera(KernelGlobals &kg, const ShadingData &sd, float3 &outview, float &outdepth, float &outdist)
#define LG
#define LOG(level)
Definition log.h:97
#define T
Eigen::Matrix< double, 6, 1 > Vec6
Definition numeric.h:111
@ BUNDLE_NO_TRANSLATION
Definition bundle.h:118
@ BUNDLE_NO_CONSTRAINTS
Definition bundle.h:117
void ProjectiveBundle(const Tracks &, ProjectiveReconstruction *)
Definition bundle.cc:856
void ApplyPolynomialDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const T &k1, const T &k2, const T &k3, const T &p1, const T &p2, const T &normalized_x, const T &normalized_y, T *image_x, T *image_y)
@ BUNDLE_RADIAL_K1
Definition bundle.h:105
@ BUNDLE_RADIAL_K3
Definition bundle.h:107
@ BUNDLE_RADIAL_K4
Definition bundle.h:108
@ BUNDLE_FOCAL_LENGTH
Definition bundle.h:102
@ BUNDLE_TANGENTIAL_P1
Definition bundle.h:112
@ BUNDLE_TANGENTIAL_P2
Definition bundle.h:113
@ BUNDLE_NO_INTRINSICS
Definition bundle.h:100
@ BUNDLE_PRINCIPAL_POINT
Definition bundle.h:103
@ BUNDLE_RADIAL_K2
Definition bundle.h:106
@ BUNDLE_TANGENTIAL
Definition bundle.h:114
void ApplyDivisionDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const T &k1, const T &k2, const T &normalized_x, const T &normalized_y, T *image_x, T *image_y)
Eigen::MatrixXd Mat
Definition numeric.h:62
void EuclideanBundle(const Tracks &tracks, EuclideanReconstruction *reconstruction)
Definition bundle.cc:654
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector
void EuclideanBundleCommonIntrinsics(const Tracks &tracks, const int bundle_intrinsics, const int bundle_constraints, EuclideanReconstruction *reconstruction, CameraIntrinsics *intrinsics, BundleEvaluation *evaluation)
Definition bundle.cc:665
@ DISTORTION_MODEL_POLYNOMIAL
@ DISTORTION_MODEL_DIVISION
@ DISTORTION_MODEL_BROWN
@ DISTORTION_MODEL_NUKE
void InvertNukeDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const int image_width, const int image_height, const T &k1, const T &k2, const T &p1, const T &p2, const T &image_x, const T &image_y, T *normalized_x, T *normalized_y)
void ApplyBrownDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const T &k1, const T &k2, const T &k3, const T &k4, const T &p1, const T &p2, const T &normalized_x, const T &normalized_y, T *image_x, T *image_y)
#define X(TYPE)
int track
Definition marker.h:43