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