Я пытаюсь понять / изучить алгоритм ICP. Я застрял с алгоритмом сопоставления точек до преобразования и перемещения точек.
У меня есть входное облако точек M размера M и облако фиксированных точек N размера N. M меньше N. Они довольно хорошо предварительно выровнены.
При попытке сопоставить пары я ожидал найти совпадения N.
Я могу пройтись по каждой точке в N и вычислить расстояние до каждой точки в M. Я могу найтиM точек в N такое, что расстояние является наименьшим до каждой точки в M. Теперь у меня есть облако точек P размера M, которое является подмножеством N.
Далее я выполняю подгонку N кP.
Затем повторяем. Я наблюдаю свои совпадения на каждой итерации, и после 2-й итерации совпадения совершенно неверны. Я не делаю сопоставление.
Вопрос: Как мне минимизировать сумму наименьших квадратов моих вычисленных расстояний, чтобы определить правильные совпадения точек? Как выглядит настоящий алгоритм в виде кода? Или пример псевдокода? (Больше, чем формула белой книги, пожалуйста)
Кстати, я не использую никаких оптимизаций (дерево kd / ближайшая точка и т. Д.). Пока просто грубая сила.