Gauss-Newton Least Squares Circle Fit (2D) using Eigen (C++)

GaussNewton.cpp

GNCircleSolver::ResultInfo GNCircleSolver::GN_Fit(Eigen::VectorXd& initEstimate)
{
  int i = 0;
  this->status = GNCircleSolver::ResultInfo::FAILURE;
  Eigen::VectorXd _old = initEstimate; // x,y,r
  Eigen::VectorXd	_new = initEstimate; // x,y,r
  while (i < this->maxItr)
  {
    _old = _new;
    Eigen::MatrixXd jacobian = CalculateJacobian(_old, this->points);
    Eigen::MatrixXd dy = CalculateResiduals(_old, this->points);
    Eigen::MatrixXd pInv = jacobian.completeOrthogonalDecomposition().pseudoInverse();
    Eigen::MatrixXd delta = pInv * dy;
    _new = _old - delta;
    i++;
    if ((_new - _old).norm() < this->maxTol)
    {
      this->status = GNCircleSolver::ResultInfo::SUCCESS;
      break;
    }
  }
  this->lastEstimate = _new;
  if (this->status == GNCircleSolver::ResultInfo::FAILURE)
  {
    std::cout << "The Solver stopped without convergence. Try changing maximum iteration limit." << std::endl;
  }
  return this->status;
}

Eigen::MatrixXd GNCircleSolver::CalculateJacobian(const Eigen::VectorXd& x, const Eigen::MatrixXd& p)
{
  Eigen::MatrixXd fjac(p.rows(), x.rows());
  for (int i = 0; i < x.size(); i++) {
    Eigen::VectorXd xPlus(x);
    xPlus(i) += this->epsilon;
    Eigen::VectorXd xMinus(x);
    xMinus(i) -= this->epsilon;

    Eigen::VectorXd fvecPlus(p.rows());
    fvecPlus = CalculateResiduals(xPlus, p);

    Eigen::VectorXd fvecMinus(p.rows());
    fvecMinus = CalculateResiduals(xMinus, p);

    Eigen::VectorXd fvecDiff(p.rows());
    fvecDiff = (fvecPlus - fvecMinus) / (2.0 * this->epsilon);

    fjac.block(0, i, p.rows(), 1) = fvecDiff;
  }
  return fjac;
}

Eigen::MatrixXd GNCircleSolver::CalculateResiduals(const Eigen::VectorXd& x, const Eigen::MatrixXd& p)
{
  Eigen::MatrixXd y(p.rows(), 1);
  for (int i = 0; i < y.rows(); ++i)
  {
    y(i) = x(2) - sqrt(pow(x(0) - p(i, 0), 2) + pow(x(1) - p(i, 1), 2));
  }
  return y;
};

GaussNewton.h

#include <Eigen/Dense>

/**
* Gauss-Newton solver for circle
*/
class GNCircleSolver
{
public:
  enum ResultInfo
  {
    SUCCESS,
    FAILURE
  };
  GNCircleSolver() = delete;
  GNCircleSolver(const Eigen::MatrixXd& points, double maxTol = 1e-6, double eps = 1e-3, int maxItr = 100) : 
  points(points), maxTol(maxTol), epsilon(eps), maxItr(maxItr) {}

  ResultInfo GN_Fit(Eigen::VectorXd&);
  ResultInfo GetLastRunStatus();
  Eigen::VectorXd GetEstimates();

  void SetMaxIterations(int);
  void SetMaxTolerance(double);
  void SetStepValue(double);


private:
  double maxTol;//  1e-6f
  double epsilon;// 1e-3f
  int maxItr;//  100
  ResultInfo status = ResultInfo::FAILURE;
  const Eigen::MatrixXd points;
  Eigen::VectorXd lastEstimate;

  /**
  * Calculate residual from circle equation
  */
  Eigen::MatrixXd CalculateResiduals(const Eigen::VectorXd&, const Eigen::MatrixXd&);

  /**
  * Calculates Jacobian numercially.
  */
  Eigen::MatrixXd CalculateJacobian(const Eigen::VectorXd&, const Eigen::MatrixXd&);
};

Gauss-Newton iterative Least Squares Circle Fit using Eigen library.
This implementation uses double type numbers, hence the MatrixXd & VectorXd