1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 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 (at) 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 google::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 } 165
