应人邀请帮忙推导求解最小二乘的PNP估计位姿问题， matlab编程实现根据PNP求解时解貌似有问题？
clc;clear all;
n=4;
f=1000;
pw3=zeros(n,3);%物体坐标3维
pc2=zeros(n,2);%成像坐标2维
pw3=[-1000,-100,0;1000,-100,0;-1000,100,0;-1000,100,0];
pc2=[-67.8995,-56.3607;83.138,43.324;73.6759,61.1555;-75.8558,-39.5293];
% syms r11 r12 r13 r22 r23 r33 t1 t2 t3;
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 t1 t2 t3 zc;
R=zeros(3,3);
T=zeros(3,1);
M=zeros(3,3);
R=[r11,r12,r13;r21,r22,r23;r31,r32,r33];%旋转矩阵
T=[t1;t2;t3];%平移向量
M(1,1)=f;
M(2,2)=f;
M(3,3)=1;
S1=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S2=M*[R,T]*[pw3(2,:)';1]-[pc2(2,:)';1]*zc;
S3=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
S4=M*[R,T]*[pw3(1,:)';1]-[pc2(1,:)';1]*zc;
% solve(S1=0,S2=0,S3=0,S4=0);
% solve('S1=0','S2=0','S3=0','S4=0');
% [r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve('S1=0','S2=0','S3=0','S4=0','','','');
% [r11,r12,r13,r22,r23,t1,t2,t3]=solve('1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '0*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000000*r11 - 100000*r12 + 1000*t1 - 41569/500','1000000*r12 - 100000*r22 + 1000*t2 - 10831/250',...
% '1000*r13 - 100*r23 + t3 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% 't3 - 100*r23 - 1000*r13 - 1',...
% '1000*t1 - 100000*r12 - 1000000*r11 + 4778002545291297/70368744177664',...
% '1000*t2 - 100000*r22 - 1000000*r12 + 7932063359948135/140737488355328',...
% ' t3 - 100*r23 - 1000*r13 - 1');
%
% % 二
[r11,r12,r21,r22,r31,r32,t1,t2,t3,zc]=solve(...
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',...
' 1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',...
' t3 - 100*r32 - 1000*r31 - zc',...
' 1000000*r11 - 100000*r12 + 1000*t1 - (41569*zc)/500',...
' 1000000*r21 - 100000*r22 + 1000*t2 - (10831*zc)/250',...
'1000*r31 - 100*r32 + t3 - zc',...
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',...
'1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',...
' t3 - 100*r32 - 1000*r31 - zc',...
' 1000*t1 - 100000*r12 - 1000000*r11 + (4778002545291297*zc)/70368744177664',...
' 1000*t2 - 100000*r22 - 1000000*r21 + (7932063359948135*zc)/140737488355328',...
' t3 - 100*r32 - 1000*r31 - zc');
clc;clear all;
n=4;
f=1000;
pw3=zeros(n,3);%物体坐标3维
pc2=zeros(n,2);%成像坐标2维
pw3=[-1000,-100,0;1000,-100,0;-1000,100,0;-1000,100,0];
pc2=[-67.8995,-56.3607;83.138,43.324;73.6759,61.1555;-75.8558,-39.5293];
% syms r11 r12 r13 r22 r23 r33 t1 t2 t3;
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 t1 t2 t3 zc;
R=zeros(3,3);
T=zeros(3,1);
M=zeros(3,3);
R=[r11,r12,r13;r21,r22,r23;r31,r32,r33];%旋转矩阵
T=[t1;t2;t3];%平移向量
M(1,1)=f;
M(2,2)=f;
M(3,3)=1;
S1=M*[R,T]*[pw3(1,:)’;1]-[pc2(1,:)’;1]*zc;
S2=M*[R,T]*[pw3(2,:)’;1]-[pc2(2,:)’;1]*zc;
S3=M*[R,T]*[pw3(1,:)’;1]-[pc2(1,:)’;1]*zc;
S4=M*[R,T]*[pw3(1,:)’;1]-[pc2(1,:)’;1]*zc;
% solve(S1=0,S2=0,S3=0,S4=0);
% solve(‘S1=0′,’S2=0′,’S3=0′,’S4=0’);
% [r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve(‘S1=0′,’S2=0′,’S3=0′,’S4=0’,”,”,”);
% [r11,r12,r13,r22,r23,t1,t2,t3]=solve(‘1000*t1 – 100000*r12 – 1000000*r11 + 4778002545291297/70368744177664’,…
% ‘0*t2 – 100000*r22 – 1000000*r12 + 7932063359948135/140737488355328’,…
% ‘t3 – 100*r23 – 1000*r13 – 1’,…
% ‘1000000*r11 – 100000*r12 + 1000*t1 – 41569/500′,’1000000*r12 – 100000*r22 + 1000*t2 – 10831/250’,…
% ‘1000*r13 – 100*r23 + t3 – 1’,…
% ‘1000*t1 – 100000*r12 – 1000000*r11 + 4778002545291297/70368744177664’,…
% ‘1000*t2 – 100000*r22 – 1000000*r12 + 7932063359948135/140737488355328’,…
% ‘t3 – 100*r23 – 1000*r13 – 1’,…
% ‘1000*t1 – 100000*r12 – 1000000*r11 + 4778002545291297/70368744177664’,…
% ‘1000*t2 – 100000*r22 – 1000000*r12 + 7932063359948135/140737488355328’,…
% ‘ t3 – 100*r23 – 1000*r13 – 1’);
%
% % 二
[r11,r12,r21,r22,r31,r32,t1,t2,t3,zc]=solve(…
‘ 1000*t1 – 100000*r12 – 1000000*r11 + (4778002545291297*zc)/70368744177664’,…
‘ 1000*t2 – 100000*r22 – 1000000*r21 + (7932063359948135*zc)/140737488355328’,…
‘ t3 – 100*r32 – 1000*r31 – zc’,…
‘ 1000000*r11 – 100000*r12 + 1000*t1 – (41569*zc)/500’,…
‘ 1000000*r21 – 100000*r22 + 1000*t2 – (10831*zc)/250’,…
‘1000*r31 – 100*r32 + t3 – zc’,…
‘ 1000*t1 – 100000*r12 – 1000000*r11 + (4778002545291297*zc)/70368744177664’,…
‘1000*t2 – 100000*r22 – 1000000*r21 + (7932063359948135*zc)/140737488355328’,…
‘ t3 – 100*r32 – 1000*r31 – zc’,…
‘ 1000*t1 – 100000*r12 – 1000000*r11 + (4778002545291297*zc)/70368744177664’,…
‘ 1000*t2 – 100000*r22 – 1000000*r21 + (7932063359948135*zc)/140737488355328’,…
‘ t3 – 100*r32 – 1000*r31 – zc’);
clc;clear all;
%% 该代码为验证迭代法求解位姿的程序 2017 12 15
n=4;
f=1000;
pw3=zeros(n,3);%物体坐标3维
pc2=zeros(n,2);%成像坐标2维
pw3=[-1000,-100,0;1000,-100,0;-1000,100,0;-1000,100,0];
pc2=[-67.8995,-56.3607;83.138,43.324;73.6759,61.1555;-75.8558,-39.5293];
% syms r11 r12 r13 r22 r23 r33 t1 t2 t3;
syms r11 r12 r13 r22 r23 r33 t1 t2 t3 zc;
R=zeros(3,3);
T=zeros(3,1);
M=zeros(3,3);
R=[r11,r12,r13;r12,r22,r23;r13,r23,r33];%旋转矩阵
T=[t1;t2;t3];%平移向量
M(1,1)=f;
M(2,2)=f;
M(3,3)=1;
S1=M*[R,T]*[pw3(1,:)’;1]-[pc2(1,:)’;1]*zc;
S2=M*[R,T]*[pw3(2,:)’;1]-[pc2(2,:)’;1]*zc;
S3=M*[R,T]*[pw3(1,:)’;1]-[pc2(1,:)’;1]*zc;
S4=M*[R,T]*[pw3(1,:)’;1]-[pc2(1,:)’;1]*zc;
% solve(S1=0,S2=0,S3=0,S4=0);
% solve(‘S1=0′,’S2=0′,’S3=0′,’S4=0’);
% [r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve(‘S1=0′,’S2=0′,’S3=0′,’S4=0’,”,”,”);
% [r11,r12,r13,r22,r23,t1,t2,t3]=solve(‘1000*t1 – 100000*r12 – 1000000*r11 + 4778002545291297/70368744177664’,…
% ‘0*t2 – 100000*r22 – 1000000*r12 + 7932063359948135/140737488355328’,…
% ‘t3 – 100*r23 – 1000*r13 – 1’,…
% ‘1000000*r11 – 100000*r12 + 1000*t1 – 41569/500′,’1000000*r12 – 100000*r22 + 1000*t2 – 10831/250’,…
% ‘1000*r13 – 100*r23 + t3 – 1’,…
% ‘1000*t1 – 100000*r12 – 1000000*r11 + 4778002545291297/70368744177664’,…
% ‘1000*t2 – 100000*r22 – 1000000*r12 + 7932063359948135/140737488355328’,…
% ‘t3 – 100*r23 – 1000*r13 – 1’,…
% ‘1000*t1 – 100000*r12 – 1000000*r11 + 4778002545291297/70368744177664’,…
% ‘1000*t2 – 100000*r22 – 1000000*r12 + 7932063359948135/140737488355328’,…
% ‘ t3 – 100*r23 – 1000*r13 – 1’);
% % 二
[r11,r12,r13,r22,r23,r33,t1,t2,t3]=solve(…
‘1000*t1 – 100000*r12 – 1000000*r11 + (4778002545291297*zc)/70368744177664’,…
‘1000*t2 – 100000*r22 – 1000000*r12 + (7932063359948135*zc)/140737488355328’,…
‘ t3 – 100*r23 – 1000*r13 – zc’,…
‘ 1000000*r11 – 100000*r12 + 1000*t1 – (41569*zc)/500’,…
‘ 1000000*r12 – 100000*r22 + 1000*t2 – (10831*zc)/250’,…
‘1000*r13 – 100*r23 + t3 – zc’,…
‘1000*t1 – 100000*r12 – 1000000*r11 + (4778002545291297*zc)/70368744177664’,…
‘1000*t2 – 100000*r22 – 1000000*r12 + (7932063359948135*zc)/140737488355328’,…
‘ t3 – 100*r23 – 1000*r13 – zc’,…
‘1000*t1 – 100000*r12 – 1000000*r11 + (4778002545291297*zc)/70368744177664’,…
‘ 1000*t2 – 100000*r22 – 1000000*r12 + (7932063359948135*zc)/140737488355328’,…
‘ t3 – 100*r23 – 1000*r13 – zc’);