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)
