1

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

我的出发点是：https ://nrr.mit.edu/sites/default/files/documents/Lab_11_Localization.html文档的B部分。起初我使用的库是这个 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（接近预期位置）

谷神星解决方案：-13891.2, 22133.1（远离）

（还将这两个测试与其他测试进行比较，在许多测试中都给出了相同（良好）的结果。但这些现实生活中的数据似乎“混淆”了谷神星并给出了错误的结果。）

我可以想到 3 个可能的问题所在：

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 google 群组但尚未获得批准，因此在这里询问，因为还有一些与 ceres-solver 相关的问题）

4

1 回答 1

2

ceres 优化残差平方和。相反，您自己提供平方值，因此您可以优化距离的 4 次方。

residual[0] = measurementPoint.second- sqrt(pow(x[0]-measurementPoint.first.getX(), 2) - pow(y[0]-measurementPoint.first.getY(), 2));应该管用

于 2020-12-22T11:24:42.837 回答