Я сейчас занимаюсь именно этой задачей. В данных условиях (небольшая база опорных точек и довольно точно известное положение нескольких из них в системе координат наблюдателя) ее можно решать так.
1 (предобработка). Для каждой точки из базы определяем расстояния до остальных 19 точек, полученный вектор сортируем. Получаем 20 векторов LB[20][19].
2. Для видимых точек делаем то же самое — получаем 5 векторов отсортированных расстояний LS[5][4].
3. Для каждой пары «вектор из LB / вектор из LS» определяем меру того, насколько второй вектор является подмножеством первого — для каждого элемента второго вектора ищем ближайший элемент первого и находим максимум или сумму отклонений.
4. Для каждого вектора из LS берем ближайший (в смысле заданной в (3) меры) вектор из LB. Получили первую кандидатуру на соответствие точек. Проверяем, соответствуют ли друг другу расстояния, и если да — пытаемся найти преобразование системы координат. Если видимые точки с хорошей точностью совпали с базой, то нам повезло.
Если не повезло, то дальше есть два пути.
5a. Начинаем просматривать не наилучшие соответствия, а следующие за ними (какой-нибудь динамикой). В конце концов повезет — у нас всего 5 индексов для перебора.
5b. Выбираем вектор из LS, соответствие которого с вектором из LB было наиболее надежным в том смысле, что второе по качеству соответствие (того же вектора из LS но с другим вектором из LB) гораздо хуже. Говорим, что самое надежное соответствие — правильное, т.е. один кубик мы узнали. Корректируем меру соответствий, добавляя новый критерий — расстояния до найденного кубика должны совпадать. После чего снова выбираем наиболее надежное соответствие, и т.д. Скорее всего, повезет.
Если не повезло, придется повышать сложность — например, сравнивать не расстояния между парами точек, а форму треугольников. Но я надеюсь, что до этого не дойдет.
