Найти оптимальное жесткое преобразование по заданным парам соответствия точек

Question

У меня есть Источник и цель в одной системе координат. Есть «n» точек в источнике и «n» точек в цели (n> = 3). Соответствия также известны. Я хотел бы найти оптимальную матрицу жесткого преобразования (в некоторых случаях 6 степеней свободы или меньше).

Я понимаю, что это решается путем минимизации квадратов расстояний между исходной и целевой точками.

У меня есть два следующих вопроса.

1) Какой лучший решатель в этих случаях? 2) В случае алгоритма Левенберга – Марквардта с кватернионами, представляющими вращения, каков наилучший способ вычисления матрицы Якоби?

Answer 1

Учитывая точки P [] и соответствующие точки Q [], мы хотим найти перевод T и вращение R, чтобы минимизировать

E = Sum{ <Q[i] - (R*P[i]+T)|Q[i] - (R*P[i]+T)>}

но это

E = Sum{ <Q[i] - R*P[i] - T | Q[i] - R*P[i] - T>}

и учитывая R, значение Т, которое минимизирует это, составляет

T = mean { Q[i] - R*P[i]} = Qbar - R*Pbar

где Qbar - среднее значение Qs и Pbar Ps.

Подключив это значение T, мы получим

E = Sum{ <q[i] - R*p[i] | q[i] - R*p[i]>}
where q[i] = Q[i]-Qbar, p[i] = P[i]-Pbar

Нахождение R для минимизации E - это проблема ортогональных прокруток . Когда это решено для R, мы можем вычислить T, как указано выше.

Модификации, когда требуется решение для взвешенного случая, просты. Прежде всего Pbar и Qbar должны быть средневзвешенными, а затем мы должны использовать

q[i] = sqrt( weight[i]) * ( Q[i]-Qbar)
p[i] = sqrt( weight[i]) * ( P[i]-Pbar)