Я пытаюсь использовать решатель Google ceres (http://ceres-solver.org/) для вычисления нелинейная трилатерация методом наименьших квадратов (цель — позиционирование в помещении с помощью маяков BLE). Моя проблема в том, что CERES дает результаты со значительной ошибкой, и, сравнивая другое решение, которое также использует алгоритм Левенберга-Марквардта, очевидно, что что-то не так с моей настройкой 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, С++ имеет те же значения, но короче; числа указаны в миллиметрах):
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 (далеко)
(Также сравнил эти 2 теста с другими тестами, во многих случаях оба дают одинаковые (хорошие) результаты. Но эти данные из реальной жизни, похоже, сбивают с толку Цереру и дают неверные результаты.)
Я могу думать о 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)
