// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

// Redistribution and use in source and binary forms, with or without

// modification, are permitted provided that the following conditions are met:

//

// * Redistributions of source code must retain the above copyright notice,

//   this list of conditions and the following disclaimer.

// * Redistributions in binary form must reproduce the above copyright notice,

//   this list of conditions and the following disclaimer in the documentation

//   and/or other materials provided with the distribution.

// * Neither the name of Google Inc. nor the names of its contributors may be

//   used to endorse or promote products derived from this software without

//   specific prior written permission.

//

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// POSSIBILITY OF SUCH DAMAGE.

//

// Author: keir@google.com (Keir Mierle)

//

// This fits circles to a collection of points, where the error is related to

// the distance of a point from the circle. This uses auto-differentiation to

// take the derivatives.

//

// The input format is simple text. Feed on standard in:

//

//   x_initial y_initial r_initial

//   x1 y1

//   x2 y2

//   y3 y3

//   ...

//

// And the result after solving will be printed to stdout:

//

//   x y r

//

// There are closed form solutions [1] to this problem which you may want to

// consider instead of using this one. If you already have a decent guess, Ceres

// can squeeze down the last bit of error.

//

//   [1] http://www.mathworks.com/matlabcentral/fileexchange/5557-circle-fit/content/circfit.m

#include <cstdio>

#include <vector>

#include "ceres/ceres.h"

#include "gflags/gflags.h"

#include "glog/logging.h"

using ceres::AutoDiffCostFunction;

using ceres::CauchyLoss;

using ceres::CostFunction;

using ceres::LossFunction;

using ceres::Problem;

using ceres::Solve;

using ceres::Solver;

DEFINE_double(robust_threshold, 0.0, "Robust loss parameter. Set to 0 for "

              "normal squared error (no robustification).");

// The cost for a single sample. The returned residual is related to the

// distance of the point from the circle (passed in as x, y, m parameters).

//

// Note that the radius is parameterized as r = m^2 to constrain the radius to

// positive values.

class DistanceFromCircleCost {

 public:

  DistanceFromCircleCost(double xx, double yy) : xx_(xx), yy_(yy) {}

  template <typename T> bool operator()(const T* const x,

                                        const T* const y,

                                        const T* const m,  // r = m^2

                                        T* residual) const {

    // Since the radius is parameterized as m^2, unpack m to get r.

    T r = *m * *m;

    // Get the position of the sample in the circle's coordinate system.

    T xp = xx_ - *x;

    T yp = yy_ - *y;

    // It is tempting to use the following cost:

    //

    //   residual[0] = r - sqrt(xp*xp + yp*yp);

    //

    // which is the distance of the sample from the circle. This works

    // reasonably well, but the sqrt() adds strong nonlinearities to the cost

    // function. Instead, a different cost is used, which while not strictly a

    // distance in the metric sense (it has units distance^2) it produces more

    // robust fits when there are outliers. This is because the cost surface is

    // more convex.

    residual[0] = r*r - xp*xp - yp*yp;

    return true;

  }

 private:

  // The measured x,y coordinate that should be on the circle.

  double xx_, yy_;

};

int main(int argc, char** argv) {

  CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);

  google::InitGoogleLogging(argv[0]);

  double x, y, r;

  if (scanf("%lg %lg %lg", &x, &y, &r) != 3) {

    fprintf(stderr, "Couldn't read first line.\n");

    return 1;

  }

  fprintf(stderr, "Got x, y, r %lg, %lg, %lg\n", x, y, r);

  // Save initial values for comparison.

  double initial_x = x;

  double initial_y = y;

  double initial_r = r;

  // Parameterize r as m^2 so that it can't be negative.

  double m = sqrt(r);

  Problem problem;

  // Configure the loss function.

  LossFunction* loss = NULL;

  if (FLAGS_robust_threshold) {

    loss = new CauchyLoss(FLAGS_robust_threshold);

  }

  // Add the residuals.

  double xx, yy;

  int num_points = 0;

  while (scanf("%lf %lf\n", &xx, &yy) == 2) {

    CostFunction *cost =

        new AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(

            new DistanceFromCircleCost(xx, yy));

    problem.AddResidualBlock(cost, loss, &x, &y, &m);

    num_points++;

  }

  std::cout << "Got " << num_points << " points.\n";

  // Build and solve the problem.

  Solver::Options options;

  options.max_num_iterations = 500;

  options.linear_solver_type = ceres::DENSE_QR;

  Solver::Summary summary;

  Solve(options, &problem, &summary);

  // Recover r from m.

  r = m * m;

  std::cout << summary.BriefReport() << "\n";

  std::cout << "x : " << initial_x << " -> " << x << "\n";

  std::cout << "y : " << initial_y << " -> " << y << "\n";

  std::cout << "r : " << initial_r << " -> " << r << "\n";

  return 0;

}