examples/circle_fit.cc

   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: keir@google.com (Keir Mierle)
  30 //
  31 // This fits circles to a collection of points, where the error is related to
  32 // the distance of a point from the circle. This uses auto-differentiation to
  33 // take the derivatives.
  34 //
  35 // The input format is simple text. Feed on standard in:
  36 //
  37 //   x_initial y_initial r_initial
  38 //   x1 y1
  39 //   x2 y2
  40 //   y3 y3
  41 //   ...
  42 //
  43 // And the result after solving will be printed to stdout:
  44 //
  45 //   x y r
  46 //
  47 // There are closed form solutions [1] to this problem which you may want to
  48 // consider instead of using this one. If you already have a decent guess, Ceres
  49 // can squeeze down the last bit of error.
  50 //
  51 //   [1] http://www.mathworks.com/matlabcentral/fileexchange/5557-circle-fit/content/circfit.m
  52
  53 #include <cstdio>
  54 #include <vector>
  55
  56 #include "ceres/ceres.h"
  57 #include "gflags/gflags.h"
  58 #include "glog/logging.h"
  59
  60 using ceres::AutoDiffCostFunction;
  61 using ceres::CauchyLoss;
  62 using ceres::CostFunction;
  63 using ceres::LossFunction;
  64 using ceres::Problem;
  65 using ceres::Solve;
  66 using ceres::Solver;
  67
  68 DEFINE_double(robust_threshold, 0.0, "Robust loss parameter. Set to 0 for "
  69               "normal squared error (no robustification).");
  70
  71 // The cost for a single sample. The returned residual is related to the
  72 // distance of the point from the circle (passed in as x, y, m parameters).
  73 //
  74 // Note that the radius is parameterized as r = m^2 to constrain the radius to
  75 // positive values.
  76 class DistanceFromCircleCost {
  77  public:
  78   DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {}
  79   template <typename T> bool operator()(const T* const x,
  80                                         const T* const y,
  81                                         const T* const m,  // r = m^2
  82                                         T* residual) const {
  83     // Since the radius is parameterized as m^2, unpack m to get r.
  84     T r = *m * *m;
  85
  86     // Get the position of the sample in the circle's coordinate system.
  87     T xp = xx_ - *x;
  88     T yp = yy_ - *y;
  89
  90     // It is tempting to use the following cost:
  91     //
  92     //   residual[0] = r - sqrt(xp*xp + yp*yp);
  93     //
  94     // which is the distance of the sample from the circle. This works
  95     // reasonably well, but the sqrt() adds strong nonlinearities to the cost
  96     // function. Instead, a different cost is used, which while not strictly a
  97     // distance in the metric sense (it has units distance^2) it produces more
  98     // robust fits when there are outliers. This is because the cost surface is
  99     // more convex.
 100     residual[0] = r*r - xp*xp - yp*yp;
 101     return true;
 102   }
 103
 104  private:
 105   // The measured x,y coordinate that should be on the circle.
 106   double xx_, yy_;
 107 };
 108
 109 int main(int argc, char** argv) {
 110   CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
 111   google::InitGoogleLogging(argv[0]);
 112
 113   double x, y, r;
 114   if (scanf("%lg %lg %lg", &x, &y, &r) != 3) {
 115     fprintf(stderr, "Couldn't read first line.\n");
 116     return 1;
 117   }
 118   fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r);
 119
 120   // Save initial values for comparison.
 121   double initial_x = x;
 122   double initial_y = y;
 123   double initial_r = r;
 124
 125   // Parameterize r as m^2 so that it can't be negative.
 126   double m = sqrt(r);
 127
 128   Problem problem;
 129
 130   // Configure the loss function.
 131   LossFunction* loss = NULL;
 132   if (FLAGS_robust_threshold) {
 133     loss = new CauchyLoss(FLAGS_robust_threshold);
 134   }
 135
 136   // Add the residuals.
 137   double xx, yy;
 138   int num_points = 0;
 139   while (scanf("%lf %lf\n", &xx, &yy) == 2) {
 140     CostFunction *cost =
 141         new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(
 142             new DistanceFromCircleCost(xx, yy));
 143     problem.AddResidualBlock(cost, loss, &x, &y, &m);
 144     num_points++;
 145   }
 146
 147   std::cout << "Got " << num_points << " points.\n";
 148
 149   // Build and solve the problem.
 150   Solver::Options options;
 151   options.max_num_iterations = 500;
 152   options.linear_solver_type = ceres::DENSE_QR;
 153   Solver::Summary summary;
 154   Solve(options, &problem, &summary);
 155
 156   // Recover r from m.
 157   r = m * m;
 158
 159   std::cout << summary.BriefReport() << "\n";
 160   std::cout << "x : " << initial_x << " -> " << x << "\n";
 161   std::cout << "y : " << initial_y << " -> " << y << "\n";
 162   std::cout << "r : " << initial_r << " -> " << r << "\n";
 163   return 0;
 164 }