GoogleGit

blob: b58ce4ee03ffb2f2be993741ca2f2fc66c4fe94d [file] [log] [blame]
  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. #include <cstdio>
  53. #include <vector>
  54. #include "ceres/ceres.h"
  55. #include "gflags/gflags.h"
  56. #include "glog/logging.h"
  57. using ceres::AutoDiffCostFunction;
  58. using ceres::CauchyLoss;
  59. using ceres::CostFunction;
  60. using ceres::LossFunction;
  61. using ceres::Problem;
  62. using ceres::Solve;
  63. using ceres::Solver;
  64. DEFINE_double(robust_threshold, 0.0, "Robust loss parameter. Set to 0 for "
  65. "normal squared error (no robustification).");
  66. // The cost for a single sample. The returned residual is related to the
  67. // distance of the point from the circle (passed in as x, y, m parameters).
  68. //
  69. // Note that the radius is parameterized as r = m^2 to constrain the radius to
  70. // positive values.
  71. class DistanceFromCircleCost {
  72. public:
  73. DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {}
  74. template <typename T> bool operator()(const T* const x,
  75. const T* const y,
  76. const T* const m, // r = m^2
  77. T* residual) const {
  78. // Since the radius is parameterized as m^2, unpack m to get r.
  79. T r = *m * *m;
  80. // Get the position of the sample in the circle's coordinate system.
  81. T xp = xx_ - *x;
  82. T yp = yy_ - *y;
  83. // It is tempting to use the following cost:
  84. //
  85. // residual[0] = r - sqrt(xp*xp + yp*yp);
  86. //
  87. // which is the distance of the sample from the circle. This works
  88. // reasonably well, but the sqrt() adds strong nonlinearities to the cost
  89. // function. Instead, a different cost is used, which while not strictly a
  90. // distance in the metric sense (it has units distance^2) it produces more
  91. // robust fits when there are outliers. This is because the cost surface is
  92. // more convex.
  93. residual[0] = r*r - xp*xp - yp*yp;
  94. return true;
  95. }
  96. private:
  97. // The measured x,y coordinate that should be on the circle.
  98. double xx_, yy_;
  99. };
  100. int main(int argc, char** argv) {
  101. CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
  102. google::InitGoogleLogging(argv[0]);
  103. double x, y, r;
  104. if (scanf("%lg %lg %lg", &x, &y, &r) != 3) {
  105. fprintf(stderr, "Couldn't read first line.\n");
  106. return 1;
  107. }
  108. fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r);
  109. // Save initial values for comparison.
  110. double initial_x = x;
  111. double initial_y = y;
  112. double initial_r = r;
  113. // Parameterize r as m^2 so that it can't be negative.
  114. double m = sqrt(r);
  115. Problem problem;
  116. // Configure the loss function.
  117. LossFunction* loss = NULL;
  118. if (FLAGS_robust_threshold) {
  119. loss = new CauchyLoss(FLAGS_robust_threshold);
  120. }
  121. // Add the residuals.
  122. double xx, yy;
  123. int num_points = 0;
  124. while (scanf("%lf %lf\n", &xx, &yy) == 2) {
  125. CostFunction *cost =
  126. new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(
  127. new DistanceFromCircleCost(xx, yy));
  128. problem.AddResidualBlock(cost, loss, &x, &y, &m);
  129. num_points++;
  130. }
  131. std::cout << "Got " << num_points << " points.\n";
  132. // Build and solve the problem.
  133. Solver::Options options;
  134. options.max_num_iterations = 500;
  135. options.linear_solver_type = ceres::DENSE_QR;
  136. Solver::Summary summary;
  137. Solve(options, &problem, &summary);
  138. // Recover r from m.
  139. r = m * m;
  140. std::cout << summary.BriefReport() << "\n";
  141. std::cout << "x : " << initial_x << " -> " << x << "\n";
  142. std::cout << "y : " << initial_y << " -> " << y << "\n";
  143. std::cout << "r : " << initial_r << " -> " << r << "\n";
  144. return 0;
  145. }