Weighted tracks
[blender.git] / extern / libmv / libmv / simple_pipeline / bundle.cc
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
21 #include "libmv/simple_pipeline/bundle.h"
22
23 #include <map>
24
25 #include "ceres/ceres.h"
26 #include "ceres/rotation.h"
27 #include "libmv/base/vector.h"
28 #include "libmv/logging/logging.h"
29 #include "libmv/multiview/fundamental.h"
30 #include "libmv/multiview/projection.h"
31 #include "libmv/numeric/numeric.h"
32 #include "libmv/simple_pipeline/camera_intrinsics.h"
33 #include "libmv/simple_pipeline/reconstruction.h"
34 #include "libmv/simple_pipeline/tracks.h"
35
36 #ifdef _OPENMP
37 #  include <omp.h>
38 #endif
39
40 namespace libmv {
41
42 // The intrinsics need to get combined into a single parameter block; use these
43 // enums to index instead of numeric constants.
44 enum {
45   OFFSET_FOCAL_LENGTH,
46   OFFSET_PRINCIPAL_POINT_X,
47   OFFSET_PRINCIPAL_POINT_Y,
48   OFFSET_K1,
49   OFFSET_K2,
50   OFFSET_K3,
51   OFFSET_P1,
52   OFFSET_P2,
53 };
54
55 namespace {
56
57 // Cost functor which computes reprojection error of 3D point X
58 // on camera defined by angle-axis rotation and it's translation
59 // (which are in the same block due to optimization reasons).
60 //
61 // This functor uses a radial distortion model.
62 struct OpenCVReprojectionError {
63   OpenCVReprojectionError(const double observed_x,
64                           const double observed_y,
65                           const double weight)
66       : observed_x_(observed_x), observed_y_(observed_y),
67         weight_(weight) {}
68
69   template <typename T>
70   bool operator()(const T* const intrinsics,
71                   const T* const R_t,  // Rotation denoted by angle axis
72                                        // followed with translation
73                   const T* const X,    // Point coordinates 3x1.
74                   T* residuals) const {
75     // Unpack the intrinsics.
76     const T& focal_length      = intrinsics[OFFSET_FOCAL_LENGTH];
77     const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X];
78     const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y];
79     const T& k1                = intrinsics[OFFSET_K1];
80     const T& k2                = intrinsics[OFFSET_K2];
81     const T& k3                = intrinsics[OFFSET_K3];
82     const T& p1                = intrinsics[OFFSET_P1];
83     const T& p2                = intrinsics[OFFSET_P2];
84
85     // Compute projective coordinates: x = RX + t.
86     T x[3];
87
88     ceres::AngleAxisRotatePoint(R_t, X, x);
89     x[0] += R_t[3];
90     x[1] += R_t[4];
91     x[2] += R_t[5];
92
93     // Prevent points from going behind the camera.
94     if (x[2] < T(0)) {
95       return false;
96     }
97
98     // Compute normalized coordinates: x /= x[2].
99     T xn = x[0] / x[2];
100     T yn = x[1] / x[2];
101
102     T predicted_x, predicted_y;
103
104     // Apply distortion to the normalized points to get (xd, yd).
105     // TODO(keir): Do early bailouts for zero distortion; these are expensive
106     // jet operations.
107     ApplyRadialDistortionCameraIntrinsics(focal_length,
108                                           focal_length,
109                                           principal_point_x,
110                                           principal_point_y,
111                                           k1, k2, k3,
112                                           p1, p2,
113                                           xn, yn,
114                                           &predicted_x,
115                                           &predicted_y);
116
117     // The error is the difference between the predicted and observed position.
118     residuals[0] = (predicted_x - T(observed_x_)) * weight_;
119     residuals[1] = (predicted_y - T(observed_y_)) * weight_;
120     return true;
121   }
122
123   const double observed_x_;
124   const double observed_y_;
125   const double weight_;
126 };
127
128 // Print a message to the log which camera intrinsics are gonna to be optimixed.
129 void BundleIntrinsicsLogMessage(const int bundle_intrinsics) {
130   if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
131     LOG(INFO) << "Bundling only camera positions.";
132   } else {
133     std::string bundling_message = "";
134
135 #define APPEND_BUNDLING_INTRINSICS(name, flag) \
136     if (bundle_intrinsics & flag) { \
137       if (!bundling_message.empty()) { \
138         bundling_message += ", "; \
139       } \
140       bundling_message += name; \
141     } (void)0
142
143     APPEND_BUNDLING_INTRINSICS("f",      BUNDLE_FOCAL_LENGTH);
144     APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT);
145     APPEND_BUNDLING_INTRINSICS("k1",     BUNDLE_RADIAL_K1);
146     APPEND_BUNDLING_INTRINSICS("k2",     BUNDLE_RADIAL_K2);
147     APPEND_BUNDLING_INTRINSICS("p1",     BUNDLE_TANGENTIAL_P1);
148     APPEND_BUNDLING_INTRINSICS("p2",     BUNDLE_TANGENTIAL_P2);
149
150     LOG(INFO) << "Bundling " << bundling_message << ".";
151   }
152 }
153
154 // Pack intrinsics from object to an array for easier
155 // and faster minimization.
156 void PackIntrinisicsIntoArray(const CameraIntrinsics &intrinsics,
157                               double ceres_intrinsics[8]) {
158   ceres_intrinsics[OFFSET_FOCAL_LENGTH]       = intrinsics.focal_length();
159   ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X]  = intrinsics.principal_point_x();
160   ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y]  = intrinsics.principal_point_y();
161   ceres_intrinsics[OFFSET_K1]                 = intrinsics.k1();
162   ceres_intrinsics[OFFSET_K2]                 = intrinsics.k2();
163   ceres_intrinsics[OFFSET_K3]                 = intrinsics.k3();
164   ceres_intrinsics[OFFSET_P1]                 = intrinsics.p1();
165   ceres_intrinsics[OFFSET_P2]                 = intrinsics.p2();
166 }
167
168 // Unpack intrinsics back from an array to an object.
169 void UnpackIntrinsicsFromArray(const double ceres_intrinsics[8],
170                                  CameraIntrinsics *intrinsics) {
171     intrinsics->SetFocalLength(ceres_intrinsics[OFFSET_FOCAL_LENGTH],
172                                ceres_intrinsics[OFFSET_FOCAL_LENGTH]);
173
174     intrinsics->SetPrincipalPoint(ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X],
175                                   ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y]);
176
177     intrinsics->SetRadialDistortion(ceres_intrinsics[OFFSET_K1],
178                                     ceres_intrinsics[OFFSET_K2],
179                                     ceres_intrinsics[OFFSET_K3]);
180
181     intrinsics->SetTangentialDistortion(ceres_intrinsics[OFFSET_P1],
182                                         ceres_intrinsics[OFFSET_P2]);
183 }
184
185 // Get a vector of camera's rotations denoted by angle axis
186 // conjuncted with translations into single block
187 //
188 // Element with index i matches to a rotation+translation for
189 // camera at image i.
190 vector<Vec6> PackCamerasRotationAndTranslation(
191     const Tracks &tracks,
192     const EuclideanReconstruction &reconstruction) {
193   vector<Vec6> all_cameras_R_t;
194   int max_image = tracks.MaxImage();
195
196   all_cameras_R_t.resize(max_image + 1);
197
198   for (int i = 0; i <= max_image; i++) {
199     const EuclideanCamera *camera = reconstruction.CameraForImage(i);
200
201     if (!camera) {
202       continue;
203     }
204
205     ceres::RotationMatrixToAngleAxis(&camera->R(0, 0),
206                                      &all_cameras_R_t[i](0));
207     all_cameras_R_t[i].tail<3>() = camera->t;
208   }
209   return all_cameras_R_t;
210 }
211
212 // Convert cameras rotations fro mangle axis back to rotation matrix.
213 void UnpackCamerasRotationAndTranslation(
214     const Tracks &tracks,
215     const vector<Vec6> &all_cameras_R_t,
216     EuclideanReconstruction *reconstruction) {
217   int max_image = tracks.MaxImage();
218
219   for (int i = 0; i <= max_image; i++) {
220     EuclideanCamera *camera = reconstruction->CameraForImage(i);
221
222     if (!camera) {
223       continue;
224     }
225
226     ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0),
227                                      &camera->R(0, 0));
228     camera->t = all_cameras_R_t[i].tail<3>();
229   }
230 }
231
232 // Converts sparse CRSMatrix to Eigen matrix, so it could be used
233 // all over in the pipeline.
234 //
235 // TODO(sergey): currently uses dense Eigen matrices, best would
236 //               be to use sparse Eigen matrices
237 void CRSMatrixToEigenMatrix(const ceres::CRSMatrix &crs_matrix,
238                             Mat *eigen_matrix) {
239   eigen_matrix->resize(crs_matrix.num_rows, crs_matrix.num_cols);
240   eigen_matrix->setZero();
241
242   for (int row = 0; row < crs_matrix.num_rows; ++row) {
243     int start = crs_matrix.rows[row];
244     int end = crs_matrix.rows[row + 1] - 1;
245
246     for (int i = start; i <= end; i++) {
247       int col = crs_matrix.cols[i];
248       double value = crs_matrix.values[i];
249
250       (*eigen_matrix)(row, col) = value;
251     }
252   }
253 }
254
255 void EuclideanBundlerPerformEvaluation(const Tracks &tracks,
256                                        EuclideanReconstruction *reconstruction,
257                                        vector<Vec6> *all_cameras_R_t,
258                                        ceres::Problem *problem,
259                                        BundleEvaluation *evaluation) {
260     int max_track = tracks.MaxTrack();
261     // Number of camera rotations equals to number of translation,
262     int num_cameras = all_cameras_R_t->size();
263     int num_points = 0;
264
265     for (int i = 0; i <= max_track; i++) {
266       const EuclideanPoint *point = reconstruction->PointForTrack(i);
267       if (point) {
268         num_points++;
269       }
270     }
271
272     LG << "Number of cameras " << num_cameras;
273     LG << "Number of points " << num_points;
274
275     evaluation->num_cameras = num_cameras;
276     evaluation->num_points = num_points;
277
278     if (evaluation->evaluate_jacobian) {
279       // Evaluate jacobian matrix.
280       ceres::CRSMatrix evaluated_jacobian;
281       ceres::Problem::EvaluateOptions eval_options;
282
283       // Cameras goes first in the ordering.
284       int max_image = tracks.MaxImage();
285       bool is_first_camera = true;
286       for (int i = 0; i <= max_image; i++) {
287         const EuclideanCamera *camera = reconstruction->CameraForImage(i);
288         if (camera) {
289           double *current_camera_R_t = &(*all_cameras_R_t)[i](0);
290
291           // All cameras are variable now.
292           if (is_first_camera) {
293             problem->SetParameterBlockVariable(current_camera_R_t);
294             is_first_camera = false;
295           }
296
297           eval_options.parameter_blocks.push_back(current_camera_R_t);
298         }
299       }
300
301       // Points goes at the end of ordering,
302       for (int i = 0; i <= max_track; i++) {
303         EuclideanPoint *point = reconstruction->PointForTrack(i);
304         if (point) {
305           eval_options.parameter_blocks.push_back(&point->X(0));
306         }
307       }
308
309       problem->Evaluate(eval_options,
310                         NULL, NULL, NULL,
311                         &evaluated_jacobian);
312
313       CRSMatrixToEigenMatrix(evaluated_jacobian, &evaluation->jacobian);
314     }
315 }
316
317 }  // namespace
318
319 void EuclideanBundle(const Tracks &tracks,
320                      EuclideanReconstruction *reconstruction) {
321   CameraIntrinsics intrinsics;
322   EuclideanBundleCommonIntrinsics(tracks,
323                                   BUNDLE_NO_INTRINSICS,
324                                   BUNDLE_NO_CONSTRAINTS,
325                                   reconstruction,
326                                   &intrinsics,
327                                   NULL);
328 }
329
330 void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
331                                      const int bundle_intrinsics,
332                                      const int bundle_constraints,
333                                      EuclideanReconstruction *reconstruction,
334                                      CameraIntrinsics *intrinsics,
335                                      BundleEvaluation *evaluation) {
336   LG << "Original intrinsics: " << *intrinsics;
337   vector<Marker> markers = tracks.AllMarkers();
338
339   ceres::Problem::Options problem_options;
340   ceres::Problem problem(problem_options);
341
342   // Residual blocks with 10 parameters are unwieldly with Ceres, so pack the
343   // intrinsics into a single block and rely on local parameterizations to
344   // control which intrinsics are allowed to vary.
345   double ceres_intrinsics[8];
346   PackIntrinisicsIntoArray(*intrinsics, ceres_intrinsics);
347
348   // Convert cameras rotations to angle axis and merge with translation
349   // into single parameter block for maximal minimization speed.
350   //
351   // Block for minimization has got the following structure:
352   //   <3 elements for angle-axis> <3 elements for translation>
353   vector<Vec6> all_cameras_R_t =
354     PackCamerasRotationAndTranslation(tracks, *reconstruction);
355
356   // Parameterization used to restrict camera motion for modal solvers.
357   ceres::SubsetParameterization *constant_translation_parameterization = NULL;
358   if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
359       std::vector<int> constant_translation;
360
361       // First three elements are rotation, ast three are translation.
362       constant_translation.push_back(3);
363       constant_translation.push_back(4);
364       constant_translation.push_back(5);
365
366       constant_translation_parameterization =
367         new ceres::SubsetParameterization(6, constant_translation);
368   }
369
370   // Add residual blocks to the problem.
371   int num_residuals = 0;
372   bool have_locked_camera = false;
373   for (int i = 0; i < markers.size(); ++i) {
374     const Marker &marker = markers[i];
375     EuclideanCamera *camera = reconstruction->CameraForImage(marker.image);
376     EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
377     if (camera == NULL || point == NULL) {
378       continue;
379     }
380
381     // Rotation of camera denoted in angle axis followed with
382     // camera translaiton.
383     double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
384
385     // Skip residual block for markers which does have absolutely
386     // no affect on the final solution.
387     // This way ceres is not gonna to go crazy.
388     if (marker.weight != 0.0) {
389       problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
390           OpenCVReprojectionError, 2, 8, 6, 3>(
391               new OpenCVReprojectionError(
392                   marker.x,
393                   marker.y,
394                   marker.weight)),
395           NULL,
396           ceres_intrinsics,
397           current_camera_R_t,
398           &point->X(0));
399
400       // We lock the first camera to better deal with scene orientation ambiguity.
401       if (!have_locked_camera) {
402         problem.SetParameterBlockConstant(current_camera_R_t);
403         have_locked_camera = true;
404       }
405
406       if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
407         problem.SetParameterization(current_camera_R_t,
408                                   constant_translation_parameterization);
409       }
410     }
411
412     num_residuals++;
413   }
414   LG << "Number of residuals: " << num_residuals;
415
416   if (!num_residuals) {
417     LG << "Skipping running minimizer with zero residuals";
418     return;
419   }
420
421   BundleIntrinsicsLogMessage(bundle_intrinsics);
422
423   if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
424     // No camera intrinsics are being refined,
425     // set the whole parameter block as constant for best performance.
426     problem.SetParameterBlockConstant(ceres_intrinsics);
427   } else {
428     // Set the camera intrinsics that are not to be bundled as
429     // constant using some macro trickery.
430
431     std::vector<int> constant_intrinsics;
432 #define MAYBE_SET_CONSTANT(bundle_enum, offset) \
433     if (!(bundle_intrinsics & bundle_enum)) { \
434       constant_intrinsics.push_back(offset); \
435     }
436     MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH,    OFFSET_FOCAL_LENGTH);
437     MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X);
438     MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y);
439     MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1,       OFFSET_K1);
440     MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2,       OFFSET_K2);
441     MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1,   OFFSET_P1);
442     MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2,   OFFSET_P2);
443 #undef MAYBE_SET_CONSTANT
444
445     // Always set K3 constant, it's not used at the moment.
446     constant_intrinsics.push_back(OFFSET_K3);
447
448     ceres::SubsetParameterization *subset_parameterization =
449       new ceres::SubsetParameterization(8, constant_intrinsics);
450
451     problem.SetParameterization(ceres_intrinsics, subset_parameterization);
452   }
453
454   // Configure the solver.
455   ceres::Solver::Options options;
456   options.use_nonmonotonic_steps = true;
457   options.preconditioner_type = ceres::SCHUR_JACOBI;
458   options.linear_solver_type = ceres::ITERATIVE_SCHUR;
459   options.use_inner_iterations = true;
460   options.max_num_iterations = 100;
461
462 #ifdef _OPENMP
463   options.num_threads = omp_get_max_threads();
464   options.num_linear_solver_threads = omp_get_max_threads();
465 #endif
466
467   // Solve!
468   ceres::Solver::Summary summary;
469   ceres::Solve(options, &problem, &summary);
470
471   LG << "Final report:\n" << summary.FullReport();
472
473   // Copy rotations and translations back.
474   UnpackCamerasRotationAndTranslation(tracks,
475                                       all_cameras_R_t,
476                                       reconstruction);
477
478   // Copy intrinsics back.
479   if (bundle_intrinsics != BUNDLE_NO_INTRINSICS)
480     UnpackIntrinsicsFromArray(ceres_intrinsics, intrinsics);
481
482   LG << "Final intrinsics: " << *intrinsics;
483
484   if (evaluation) {
485     EuclideanBundlerPerformEvaluation(tracks, reconstruction, &all_cameras_R_t,
486                                       &problem, evaluation);
487   }
488 }
489
490 void ProjectiveBundle(const Tracks & /*tracks*/,
491                       ProjectiveReconstruction * /*reconstruction*/) {
492   // TODO(keir): Implement this! This can't work until we have a better bundler
493   // than SSBA, since SSBA has no support for projective bundling.
494 }
495
496 }  // namespace libmv