Ceres解算器对trilateraion给出错误结果

Question

我试图使用谷歌的ceres求解器（http://ceres-solver.org/）来计算非线性最小二乘三边测量（目标是使用BLE信标进行室内定位）。我的问题是，CERES给出的结果有一个显著的错误，比较另一个解决方案，也使用Levenberg-Marquott算法，很明显，我的ceres设置有问题。

我的出发点是：https://nrr.mit.edu/sites/default/files/documents/Lab_11_Localization.htmlB文件的一部分。起初，我使用的库是这个JAVA项目：https://github.com/lemmingapex/Trilateration它使用上述LM算法来解决三边测量问题。导数和雅可比矩阵是预先计算/编码的（我（还）不了解这些，为了快速前进，不幸的是，我不得不跳过更深入的理解）。对于CERES（我第一次使用它），AutoDiffCostFunction似乎是我定义问题的一种非常简单的方法，如下所示：

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;
};

比较2解决方案中的一些示例：

输入（来自JAVA代码，C具有相同的值，但这更短；数字以毫米为单位）：

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};

java三边测量库的解决方案：-23085.6 24505.1（接近预期位置）

Ceres解决方案：-13891.22133.1（远）

（还将这两项测试与其他测试进行了比较，在许多情况下，它们都给出了相同（良好）的结果。但这些真实数据似乎“混淆”了ceres，并给出了错误的结果。）

我能想到问题所在的三个可能的方面：

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?)

你能帮我弥补我错过的吗？（由于技术要求，我们继续使用C，所以这就是为什么我们需要替换JAVA中已经工作的非线性三元组版本）

顺便问一下，这个解不是每次调用三边测量计算时都会计算导数吗？因此，与我不使用autocostfunctor相比，这引入了一个显著的延迟，对吗？

谢谢你的洞察力！（我曾尝试加入ceres谷歌集团，但尚未获得批准，因此在此提问，因为还有一些与ceres solver相关的问题）

Answer 1

ceres优化残差平方和。相反，您提供了平方值本身，因此您优化了距离的四次方。

残差[0]=测量点。second-sqrt（pow（x[0]-measurementPoint.first.getX（），2）-pow（y[0]-measurementPoint.first.getY（），2）） 应该可以工作