examples/snavely_reprojection_error.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   4 //
   5 // Redistribution and use in source and binary forms, with or without
   6 // modification, are permitted provided that the following conditions are met:
   7 //
   8 // * Redistributions of source code must retain the above copyright notice,
   9 //   this list of conditions and the following disclaimer.
  10 // * Redistributions in binary form must reproduce the above copyright notice,
  11 //   this list of conditions and the following disclaimer in the documentation
  12 //   and/or other materials provided with the distribution.
  13 // * Neither the name of Google Inc. nor the names of its contributors may be
  14 //   used to endorse or promote products derived from this software without
  15 //   specific prior written permission.
  16 //
  17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27 // POSSIBILITY OF SUCH DAMAGE.
  28 //
  29 // Author: sameeragarwal@google.com (Sameer Agarwal)
  30 //
  31 // Templated struct implementing the camera model and residual
  32 // computation for bundle adjustment used by Noah Snavely's Bundler
  33 // SfM system. This is also the camera model/residual for the bundle
  34 // adjustment problems in the BAL dataset. It is templated so that we
  35 // can use Ceres's automatic differentiation to compute analytic
  36 // jacobians.
  37 //
  38 // For details see: http://phototour.cs.washington.edu/bundler/
  39 // and http://grail.cs.washington.edu/projects/bal/
  40
  41 #ifndef CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
  42 #define CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
  43
  44 #include "ceres/rotation.h"
  45
  46 namespace ceres {
  47 namespace examples {
  48
  49 // Templated pinhole camera model for used with Ceres.  The camera is
  50 // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
  51 // focal length and 2 for radial distortion. The principal point is not modeled
  52 // (i.e. it is assumed be located at the image center).
  53 struct SnavelyReprojectionError {
  54   SnavelyReprojectionError(double observed_x, double observed_y)
  55       : observed_x(observed_x), observed_y(observed_y) {}
  56
  57   template <typename T>
  58   bool operator()(const T* const camera,
  59                   const T* const point,
  60                   T* residuals) const {
  61     // camera[0,1,2] are the angle-axis rotation.
  62     T p[3];
  63     AngleAxisRotatePoint(camera, point, p);
  64
  65     // camera[3,4,5] are the translation.
  66     p[0] += camera[3];
  67     p[1] += camera[4];
  68     p[2] += camera[5];
  69
  70     // Compute the center of distortion. The sign change comes from
  71     // the camera model that Noah Snavely's Bundler assumes, whereby
  72     // the camera coordinate system has a negative z axis.
  73     const T xp = - p[0] / p[2];
  74     const T yp = - p[1] / p[2];
  75
  76     // Apply second and fourth order radial distortion.
  77     const T& l1 = camera[7];
  78     const T& l2 = camera[8];
  79     const T r2 = xp*xp + yp*yp;
  80     const T distortion = 1.0 + r2  * (l1 + l2  * r2);
  81
  82
  83     // Compute final projected point position.
  84     const T& focal = camera[6];
  85     const T predicted_x = focal * distortion * xp;
  86     const T predicted_y = focal * distortion * yp;
  87
  88     // The error is the difference between the predicted and observed position.
  89     residuals[0] = predicted_x - observed_x;
  90     residuals[1] = predicted_y - observed_y;
  91
  92     return true;
  93   }
  94
  95   // Factory to hide the construction of the CostFunction object from
  96   // the client code.
  97   static ceres::CostFunction* Create(const double observed_x,
  98                                      const double observed_y) {
  99     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
 100                 new SnavelyReprojectionError(observed_x, observed_y)));
 101   }
 102
 103   double observed_x;
 104   double observed_y;
 105 };
 106
 107 // Templated pinhole camera model for used with Ceres.  The camera is
 108 // parameterized using 10 parameters. 4 for rotation, 3 for
 109 // translation, 1 for focal length and 2 for radial distortion. The
 110 // principal point is not modeled (i.e. it is assumed be located at
 111 // the image center).
 112 struct SnavelyReprojectionErrorWithQuaternions {
 113   // (u, v): the position of the observation with respect to the image
 114   // center point.
 115   SnavelyReprojectionErrorWithQuaternions(double observed_x, double observed_y)
 116       : observed_x(observed_x), observed_y(observed_y) {}
 117
 118   template <typename T>
 119   bool operator()(const T* const camera,
 120                   const T* const point,
 121                   T* residuals) const {
 122     // camera[0,1,2,3] is are the rotation of the camera as a quaternion.
 123     //
 124     // We use QuaternionRotatePoint as it does not assume that the
 125     // quaternion is normalized, since one of the ways to run the
 126     // bundle adjuster is to let Ceres optimize all 4 quaternion
 127     // parameters without a local parameterization.
 128     T p[3];
 129     QuaternionRotatePoint(camera, point, p);
 130
 131     p[0] += camera[4];
 132     p[1] += camera[5];
 133     p[2] += camera[6];
 134
 135     // Compute the center of distortion. The sign change comes from
 136     // the camera model that Noah Snavely's Bundler assumes, whereby
 137     // the camera coordinate system has a negative z axis.
 138     const T xp = - p[0] / p[2];
 139     const T yp = - p[1] / p[2];
 140
 141     // Apply second and fourth order radial distortion.
 142     const T& l1 = camera[8];
 143     const T& l2 = camera[9];
 144
 145     const T r2 = xp*xp + yp*yp;
 146     const T distortion = 1.0 + r2  * (l1 + l2  * r2);
 147
 148     // Compute final projected point position.
 149     const T& focal = camera[7];
 150     const T predicted_x = focal * distortion * xp;
 151     const T predicted_y = focal * distortion * yp;
 152
 153     // The error is the difference between the predicted and observed position.
 154     residuals[0] = predicted_x - observed_x;
 155     residuals[1] = predicted_y - observed_y;
 156
 157     return true;
 158   }
 159
 160   // Factory to hide the construction of the CostFunction object from
 161   // the client code.
 162   static ceres::CostFunction* Create(const double observed_x,
 163                                      const double observed_y) {
 164     return (new ceres::AutoDiffCostFunction<
 165             SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(
 166                 new SnavelyReprojectionErrorWithQuaternions(observed_x,
 167                                                             observed_y)));
 168   }
 169
 170   double observed_x;
 171   double observed_y;
 172 };
 173
 174 }  // namespace examples
 175 }  // namespace ceres
 176
 177 #endif  // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_