extern/libmv/libmv/simple_pipeline/modal_solver.cc

   1 // Copyright (c) 2012 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/modal_solver.h"
  22
  23 #include <cstdio>
  24
  25 #include "ceres/ceres.h"
  26 #include "ceres/rotation.h"
  27 #include "libmv/logging/logging.h"
  28 #include "libmv/multiview/panography.h"
  29
  30 #ifdef _MSC_VER
  31 #  define snprintf _snprintf
  32 #endif
  33
  34 namespace libmv {
  35
  36 namespace {
  37 void ProjectMarkerOnSphere(const Marker &marker, Vec3 &X) {
  38   X(0) = marker.x;
  39   X(1) = marker.y;
  40   X(2) = 1.0;
  41
  42   X *= 5.0 / X.norm();
  43 }
  44
  45 void ModalSolverLogProress(ProgressUpdateCallback *update_callback,
  46                            double progress) {
  47   if (update_callback) {
  48     char message[256];
  49
  50     snprintf(message, sizeof(message), "Solving progress %d%%",
  51              (int)(progress * 100));
  52
  53     update_callback->invoke(progress, message);
  54   }
  55 }
  56
  57 struct ModalReprojectionError {
  58   ModalReprojectionError(double observed_x,
  59                          double observed_y,
  60                          const Vec3 &bundle)
  61     : observed_x(observed_x), observed_y(observed_y), bundle(bundle) { }
  62
  63   template <typename T>
  64   bool operator()(const T* quaternion,   // Rotation quaternion
  65                   T* residuals) const {
  66     T R[9];
  67     ceres::QuaternionToRotation(quaternion, R);
  68
  69     // Convert bundle position from double to T.
  70     T X[3];
  71     X[0] = T(bundle(0));
  72     X[1] = T(bundle(1));
  73     X[2] = T(bundle(2));
  74
  75     // Compute projective coordinates: x = RX.
  76     T x[3];
  77     x[0] = R[0]*X[0] + R[3]*X[1] + R[6]*X[2];
  78     x[1] = R[1]*X[0] + R[4]*X[1] + R[7]*X[2];
  79     x[2] = R[2]*X[0] + R[5]*X[1] + R[8]*X[2];
  80
  81     // Compute normalized coordinates: x /= x[2].
  82     T xn = x[0] / x[2];
  83     T yn = x[1] / x[2];
  84
  85     // The error is the difference between reprojected
  86     // and observed marker position.
  87     residuals[0] = xn - T(observed_x);
  88     residuals[1] = yn - T(observed_y);
  89
  90     return true;
  91   }
  92
  93   double observed_x;
  94   double observed_y;
  95   Vec3 bundle;
  96 };
  97 }  // namespace
  98
  99 void ModalSolver(const Tracks &tracks,
 100                  EuclideanReconstruction *reconstruction,
 101                  ProgressUpdateCallback *update_callback) {
 102   int max_image = tracks.MaxImage();
 103   int max_track = tracks.MaxTrack();
 104
 105   LG << "Max image: " << max_image;
 106   LG << "Max track: " << max_track;
 107
 108   // For minimization we're using quaternions.
 109   Vec3 zero_rotation = Vec3::Zero();
 110   Vec4 quaternion;
 111   ceres::AngleAxisToQuaternion(&zero_rotation(0), &quaternion(0));
 112
 113   for (int image = 0; image <= max_image; ++image) {
 114     vector<Marker> all_markers = tracks.MarkersInImage(image);
 115
 116     ModalSolverLogProress(update_callback, (float) image / max_image);
 117
 118     // Skip empty images without doing anything.
 119     if (all_markers.size() == 0) {
 120       LG << "Skipping image: " << image;
 121       continue;
 122     }
 123
 124     // STEP 1: Estimate rotation analytically.
 125     Mat3 current_R;
 126     ceres::QuaternionToRotation(&quaternion(0), &current_R(0, 0));
 127
 128     // Construct point cloud for current and previous images,
 129     // using markers appear at current image for which we know
 130     // 3D positions.
 131     Mat x1, x2;
 132     for (int i = 0; i < all_markers.size(); ++i) {
 133       Marker &marker = all_markers[i];
 134       EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
 135       if (point) {
 136         Vec3 X;
 137         ProjectMarkerOnSphere(marker, X);
 138
 139         int last_column = x1.cols();
 140         x1.conservativeResize(3, last_column + 1);
 141         x2.conservativeResize(3, last_column + 1);
 142
 143         x1.col(last_column) = current_R * point->X;
 144         x2.col(last_column) = X;
 145       }
 146     }
 147
 148     if (x1.cols() >= 2) {
 149       Mat3 delta_R;
 150
 151       // Compute delta rotation matrix for two point clouds.
 152       // Could be a bit confusing at first glance, but order
 153       // of clouds is indeed so.
 154       GetR_FixedCameraCenter(x2, x1, 1.0, &delta_R);
 155
 156       // Convert delta rotation form matrix to final image
 157       // rotation stored in a quaternion
 158       Vec3 delta_angle_axis;
 159       ceres::RotationMatrixToAngleAxis(&delta_R(0, 0), &delta_angle_axis(0));
 160
 161       Vec3 current_angle_axis;
 162       ceres::QuaternionToAngleAxis(&quaternion(0), &current_angle_axis(0));
 163
 164       Vec3 angle_axis = current_angle_axis + delta_angle_axis;
 165
 166       ceres::AngleAxisToQuaternion(&angle_axis(0), &quaternion(0));
 167
 168       LG << "Analytically computed quaternion "
 169          << quaternion.transpose();
 170     }
 171
 172     // STEP 2: Refine rotation with Ceres.
 173     ceres::Problem problem;
 174
 175     ceres::LocalParameterization* quaternion_parameterization =
 176         new ceres::QuaternionParameterization;
 177
 178     int num_residuals = 0;
 179     for (int i = 0; i < all_markers.size(); ++i) {
 180       Marker &marker = all_markers[i];
 181       EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
 182
 183       if (point) {
 184         problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
 185             ModalReprojectionError,
 186             2, /* num_residuals */
 187             4>(new ModalReprojectionError(marker.x, marker.y,
 188                                           point->X)),
 189             NULL,
 190             &quaternion(0));
 191         num_residuals++;
 192
 193         problem.SetParameterization(&quaternion(0),
 194                                     quaternion_parameterization);
 195       }
 196     }
 197
 198     LG << "Number of residuals: " << num_residuals;
 199
 200     if (num_residuals) {
 201       // Configure the solve.
 202       ceres::Solver::Options solver_options;
 203       solver_options.linear_solver_type = ceres::DENSE_QR;
 204       solver_options.max_num_iterations = 50;
 205       solver_options.update_state_every_iteration = true;
 206       solver_options.gradient_tolerance = 1e-36;
 207       solver_options.parameter_tolerance = 1e-36;
 208       solver_options.function_tolerance = 1e-36;
 209
 210       // Run the solve.
 211       ceres::Solver::Summary summary;
 212       ceres::Solve(solver_options, &problem, &summary);
 213
 214       LG << "Summary:\n" << summary.FullReport();
 215       LG << "Refined quaternion " << quaternion.transpose();
 216     }
 217
 218     // Convert quaternion to rotation matrix.
 219     Mat3 R;
 220     ceres::QuaternionToRotation(&quaternion(0), &R(0, 0));
 221     reconstruction->InsertCamera(image, R, Vec3::Zero());
 222
 223     // STEP 3: reproject all new markers appeared at image
 224
 225     // Check if there're new markers appeared on current image
 226     // and reproject them on sphere to obtain 3D position/
 227     for (int track = 0; track <= max_track; ++track) {
 228       if (!reconstruction->PointForTrack(track)) {
 229         Marker marker = tracks.MarkerInImageForTrack(image, track);
 230
 231         if (marker.image == image) {
 232           // New track appeared on this image,
 233           // project it's position onto sphere.
 234
 235           LG << "Projecting track " << track << " at image " << image;
 236
 237           Vec3 X;
 238           ProjectMarkerOnSphere(marker, X);
 239           reconstruction->InsertPoint(track, R.inverse() * X);
 240         }
 241       }
 242     }
 243   }
 244 }
 245
 246 }  // namespace libmv