Ceres-solver giving wrong results for trilateraion

Question

I'm trying to use google's ceres solver (http://ceres-solver.org/) to calculate non-linear least squares trilateration (goal is indoor positioning with BLE beacons). My problem is that CERES gives results which have a significant error and comparing another solution which also uses the Levenberg-Marquardt algorithm it is evident that something is wrong with my ceres setup.

My starting point is this: https://nrr.mit.edu/sites/default/files/documents/Lab_11_Localization.html B part of the document. At first the library I used was this JAVA project: https://github.com/lemmingapex/Trilateration which uses the above mentioned LM algorithm to solve trilateration problems. The derivatives and jacobi matrices are precalculated/coded (I don't (yet) understand these, in order to move forward quickly unfortunately had to skip deeper understanding). With CERES (my 1st time using it) the AutoDiffCostFunction seemed a really easy way for me to define the problem which is like this:

Coordinate TrilaterationCalculator::ComputePosition2D(
    const std::list<std::pair<Coordinate, double>> &measurementPoints) {
  Problem problem;
  double x = 1.0;
  double y = 1.0;

  for (const auto &measurementPoint : measurementPoints) {
    problem.AddResidualBlock(new AutoDiffCostFunction<BeaconCostFunctor2D, 1, 1, 1>(new BeaconCostFunctor2D(measurementPoint)), nullptr, &x, &y);
  }

  Solver::Options options;
  options.minimizer_progress_to_stdout = false;
  options.minimizer_type = ceres::TRUST_REGION;
  // options.minimizer_type = ceres::LINE_SEARCH; // TODO
  //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.linear_solver_type = ceres::DENSE_QR;
  options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  options.logging_type = ceres::SILENT;
  options.minimizer_progress_to_stdout = false;

  options.function_tolerance = 1e-12;
  options.gradient_tolerance = 1e-12;
  options.parameter_tolerance = 1e-12;
  options.max_num_iterations = 1000;
  //options.max_solver_time_in_seconds = 1;
  //options.num_threads = 4;

  Solver::Summary summary;
  Solve(options, &problem, &summary);

  auto result = Coordinate(x, y, 1.4); //TODO proper handling of z coordinate...
  return result;
}

struct BeaconCostFunctor2D {

public:
  BeaconCostFunctor2D(const std::pair<Coordinate, double> &measurementPoint_)
      : measurementPoint(measurementPoint_) {}

  template <typename T>
  bool operator()(const T *const x, const T *const y, T *residual) const {
    //r^2-(x-x1)^2-(y-y1)^2 
    residual[0] = pow(measurementPoint.second, 2) - pow(x[0]-measurementPoint.first.getX(), 2) - pow(y[0]-measurementPoint.first.getY(), 2);
    return true;
  }

private:
  const std::pair<Coordinate, double> &measurementPoint;
};

Comparing some examples from the 2 solution:

Input (from JAVA code, C++ has the same values but this is shorter; numbers are in millimeter):

double[][] positions = new double[][]{{-24223,26072}, {-13446,16859}, {-20860,15693}, {-21019,25807}, {-17037,21467}, {-11449,15837}, {-3980,24447}, {-16639,15693}, {-21019,17803}, {-4439,21155}, {-12503,20343}, {-16878,24891}, {-24364,22343}, {-20979,21985}, {-17157,17883}, {-7836,16369}, {-12498,24971}, {-160,24931}, {-8860,24514}, {-8825,21002}, {-8769,18404}, };
double[] distances = new double[]{0.001,7874.97,4182.28,0.001,4382.07,3027.21,4380.63,5801.38,3222.07,6158.16,2676.96,2984.05,0.001,2388.27,3359.42,4153.79,2105.41,6676.31,2981.94,2385.64,2417.16,};
 double[] expectedPosition = new double[]{-24706.0, 26754.0};

The java trilateration library's solution: -23085.6 24505.1 (close to expected position)

Ceres solution: -13891.2, 22133.1 (way off)

(Also compared these 2 with other tests, in many both give the same (good) results. But these real-life data seem to "confuse" ceres and gives wrong results.)

I can think of 3 possible things where the problem lies:

1) ceres's automatic differentiation is not working properly (less likely I guess)

2) my problem setup in Ceres is wrong (most likely)

3) (something very stupid coding mistake somewhere?)

Could you please help me with what am I missing? (We moved on to use C++ because of technical requirements so that's why we need to replace this already working version of non-linear trilaterion in JAVA)

By the way isn't this solution calculating the derivatives each time this trilateration calculation is called? So then this introduces a significant delay compared to if I didn't use the autocostfunctor, right?

Thanks for any insight! (I tried joining ceres google groups but haven't been approved yet, hence asking here, as there were also some ceres-solver related questions)

Answer 1

ceres optimize the sum of squared residuals. Instead, you provide squared value itself, so you optimize the 4th power of distances.

residual[0] = measurementPoint.second- sqrt(pow(x[0]-measurementPoint.first.getX(), 2) - pow(y[0]-measurementPoint.first.getY(), 2)); should work