Я делаю итеративный проект ближайшей точки. Есть набор точек, называемый «a» и «b». Я хочу, чтобы матрица преобразования соответствовала «a» и «b». SVD может решить эту проблему отлично и получить матрицу вращения и перевода. теперь я рассматриваю создание некоторых шарниров только для вращения, устанавливая «а» и «b» точечные множества без какого-либо перевода, но только для вращения. Я обыскал интернет, и некоторые обсуждения говорят, что алгоритм Левенберга-Марквардта может его решить.Решение только абсолютной ориентации вращения по алгоритму Левенберга-Маркварда в matlab
скопировать код здесь и изменить код функции стоимости проблемы абсолютной ориентации https://engineering.purdue.edu/kak/computervision/ECE661/HW5_LM_handout.pdf
и функция затрат
E = Σ || Ра-Ь ||^2
R будет матрица вращения и т будет матрица перевод
код MATLAB ниже, он будет возвращать лучшее вращение радиан «R» и перевод «Т» :
a=[0 1 2 3 4 5 6 7 8 9;0 0 0 0 0 0 0 0 0 0];
b=[0 0.7074 1.4148 2.1222 2.8296 3.5369 4.2443 4.9517 5.6591 6.3665;0 -0.7068 -1.4137 -2.1205 -2.8273 -3.5341 -4.2410 -4.9478 -5.6546 -6.3614];
r0=0.0;
tx0=0;
ty0=0;
y_init = [cos(r0) -sin(r0);sin(r0) cos(r0)]*a-[tx0;ty0]*[1 1 1 1 1 1 1 1 1 1];
Ndata=length(b);
Nparams=3;
n_iters=50;
lamda=0.01;
updateJ=1;
r_est=r0;
tx_est=tx0;
ty_est=ty0;
for it=1:n_iters
if updateJ==1
J=zeros(Ndata*2,Nparams);
for i=0:length(a)-1
J(2*i+1,:)= [- a(2,i+1)*cos(r_est) - a(1,i+1)*sin(r_est), -1, 0];
J(2*i+2,:)= [ a(1,i+1)*cos(r_est) - a(2,i+1)*sin(r_est), 0, -1];
end
y_est = [cos(r_est) -sin(r_est);sin(r_est) cos(r_est)]*a-[tx_est;ty_est]*[1 1 1 1 1 1 1 1 1 1];
d=b-y_est;
H=J'*J;
if it==1
e=dot(d,d);
end
end
H_lm=H+(lamda*eye(Nparams,Nparams));
dp=inv(H_lm)*(J'*d(:));
H_lm;
inv(H_lm);
J'*d(:);
g = J'*d(:);
r_lm=r_est+dp(1);
tx_lm=tx_est+dp(2);
ty_lm=ty_est+dp(3);
y_est_lm = [cos(r_lm) -sin(r_lm);sin(r_lm) cos(r_lm)]*a-[tx_lm;ty_lm]*[1 1 1 1 1 1 1 1 1 1];
d_lm=b-y_est_lm;
e_lm=dot(d_lm,d_lm);
if e_lm<e
lamda=lamda/10;
r_est=r_lm;
tx_est=tx_lm;
ty_est=ty_lm;
e=e_lm;
updateJ=1;
else
updateJ=0;
lamda=lamda*10;
end
end
r_est
Он работал так же, как близко форма решения, как SVD.Now я не хочу, перевод, я думаю, что формула будет
E = Σ || Ra-b ||^2
это означает, что я поворачиваю «a» и устанавливаю «b» вокруг точки отсчета.
. Код будет ниже, было бы вернуть лучшее вращение радиан «R»:
a=[0 1 2 3 4 5 6 7 8 9;1 1 1 1 1 1 1 1 1 1];
b=[-1 -2 -3 -4 -5 -6 -7 -8 -9 -10;0 0 0 0 0 0 0 0 0 0];
%initial guess
r0=0;
y_init = [cos(r0) -sin(r0);sin(r0) cos(r0)]*a;
Ndata=length(b);
Nparams=1;
n_iters=50;
lamda=0.01;
updateJ=1;
r_est=r0;
for it=1:n_iters
if updateJ==1
J=zeros(Ndata,Nparams);
for i=0:length(a)-1
J(2*i+1,:)= [-a(2,i+1)*cos(r_est)-a(1,i+1)*sin(r_est)];
J(2*i+2,:)= [ a(1,i+1)*cos(r_est)-a(2,i+1)*sin(r_est)];
end
y_est = [cos(r_est) -sin(r_est);sin(r_est) cos(r_est)]*a;
d=b-y_est;
H=J'*J;
if it==1
e=dot(d,d);
end
end
H_lm=H+(lamda*eye(Nparams,Nparams));
dp=inv(H_lm)*(J'*d(:));
H_lm;
inv(H_lm);
J'*d(:);
g = J'*d(:);
r_lm=r_est+dp(1);
y_est_lm = [cos(r_lm) -sin(r_lm);sin(r_lm) cos(r_lm)]*a;
d_lm=b-y_est_lm;
e_lm=dot(d_lm,d_lm);
if e_lm<e
lamda=lamda/10;
r_est=r_lm;
e=e_lm;
updateJ=1;
else
updateJ=0;
lamda=lamda*10;
end
end
r_est
в этом коде, я удаляю матрицу перевода функции затрат, то сделать алгоритм Левенберга-Marquardt, я надеюсь, что он будет возвращать наилучшая матрица вращения, которая соответствует наборам точек «a» и «b». Однако он всегда возвращает исходное предположение r0. Кажется, я не могу просто удалить матрицу переводов в функции стоимости, чтобы получить лучший поворот.
Что мне делать для решения этой проблемы с абсолютной ориентацией вращения? спасибо за любую идею!