RPnP算法原文及代码解析 |
您所在的位置：网站首页 › 迭代是谁提出来的 › RPnP算法原文及代码解析 |
RPnP算法原文及代码解析
|
（有不对的地方希望得到您的指正~~） RPnP（Robust Perspective-n-Point）是一种快速且具有鲁棒性的PnP求解方法。在2D-3D点较少的情况下，能取得较理想的计算效果。并且其结果远优于EPnP+GN，是目前解决PnP问题的最有效的方法之一。由于RPnP算法是在PST（Perspective Similar Triangle）算法之上，提出来的，故在解析RPnP前，需简单介绍一下PST算法。
PST算法是为了解决P3P问题，提出的一种透视相似三角形的方法。如上图所示，最上面的三角形为物体坐标系，中间为最上面三角形的相似三角形。
上图为PST的原理图。简单来看，PST算法是通过设一个3D点（l0）到光心（O）的距离已知，通过几何限制条件求出t2与t1的结果，再由3D点中的距离关系，通过比例获得出三角形在空间中的位置（详细过程请看原文）。在PST算法的原文中将各个限制条件，融合在一起，得到了一个4次方程：
其中：
但解的结果最多有4个，故依旧需要第4个点作为限制点，来确定物体坐标系与相机坐标系的转换关系。 RPnP将图像坐标系中距离最远的两个点，看作初始点（文中解释是因为最长边受到噪声的影响最小）。并将3D点中的n个点，看作n-2个三角形（即每个点都与两个初始点形成三角形）。再运用PST算法就可以得到下面的方程组：
文中设置出一个损失函数 对损失函数求导 F'为一个7阶方程，通过特征值的方式，可以求解出极值点。即求解出当前三角形在空间中的位姿。在原文中将Pi0与Pj0看作Z轴，通过叉乘的方式获得当前三角形在相机坐标系下的物体坐标系（详细过程请看原文）。如下公式所述：
通过上式，使用SVD的方法，可将n-2个三角形的物体坐标系与相机坐标系的转换关系求解出来。计算各个转换关系对应的重投影误差，取最小的重投影误差作为结果。下图为，对比结果。
以下为matlab的RPnP的代码解析（代码与实际中的原文有些小地方不太一样）。
上图为个人对代码中参数的理解 function [R t]= RPnP(XX,xx) R= []; t= []; n= length(xx); XXw= XX; % 原物体坐标系中的点 xxv= [xx; ones(1,n)]; % 图像坐标系中的点 % 归一化 for i=1:n xxv(:,i)= xxv(:,i)/norm(xxv(:,i)); end % 选择在图像平面中投影长度最长的边$P_{i1}P_{i2}$。 lmin= inf; i1= 0; i2= 0; for i= 1:n-1 for j= i+1:n l= xxv(1,i)*xxv(1,j)+xxv(2,i)*xxv(2,j)+xxv(3,i)*xxv(3,j); if l < lmin i1= i; i2= j; lmin= l; end end end % 计算旋转矩阵 $O_aX_aY_aZ_a$. % 距离最远的两个点P1，P2（此处为图像坐标系中距离最远的点与对应的物体坐标系中的点） p1= XX(:,i1); p2= XX(:,i2); p0= (p1+p2)/2; % 新物体坐标系原点 x= p2-p0; x= x/norm(x); % 归一化之后的新物体坐标系的x轴单位向量 if abs([0 1 0]*x) < abs([0 0 1]*x) z= xcross(x,[0; 1; 0]); z= z/norm(z); y= xcross(z, x); y= y/norm(y); %叉乘后的y轴单位向量 else y= xcross([0; 0; 1], x); y= y/norm(y); z= xcross(x,y); z= z/norm(z); %叉乘后的z轴单位向量 end Ro= [x y z]; % 原物体坐标系到新物体坐标系的旋转矩阵 % 将参考点从原始物体坐标系转换为新的坐标系$O_aX_aY_aZ_a$。（以下为称为物体坐标系） XX= Ro.'*(XX-repmat(p0,1,n)); % 将n点集划分为（n-2）个3点子集，并建立P3P方程 v1= xxv(:,i1); % 像平面下pi坐标 v2= xxv(:,i2); % 像平面下pj坐标 cg1= v1.'*v2; % pi*pj，即两点夹角余弦值 sg1= sqrt(1-cg1^2); % 两点夹角正弦值 D1= norm(XX(:,i1)-XX(:,i2)); % PiPj的长度 D4= zeros(n-2,5); if 0 % 确定F'，成本函数的偏差。 j = 0; for i= 1:n if i == i1 || i == i2 continue; end j= j+1; vi= xxv(:,i); cg2= v1.'*vi; % Pi坐标和其他点的余弦值 cg3= v2.'*vi; % Pj坐标和其他点的余弦值 sg2= sqrt(1-cg2^2); % Pi坐标和其他点的正弦值 D2= norm(XX(:,i1)-XX(:,i)); %物体坐标系下Pi点与其他点的长度 D3= norm(XX(:,i)-XX(:,i2)); %物体坐标系下Pj点与其他点的长度 % 从每个子集中获得P3P方程的系数。 D4(j,:)= getp3p(cg1,cg2,cg3,sg1,sg2,D1,D2,D3); % （n-2）x5的系数矩阵 end % 得到七阶多项式，即成本函数的偏差。 D7= zeros(1,8); for i= 1:n-2 D7= D7+ getpoly7(D4(i,:)); % F(x)*F'(x)的7阶系数矩阵 end else % ======================================================================= %下面的代码与上面的代码相同（介于“if 0”和“else”之间） %但是当点数较大时，下面的代码比matlab中的前者效率略高，因为使用了点乘运算。 idx= true(1,n); idx([i1 i2])= false; vi= xxv(:,idx); cg2= vi.'*v1; cg3= vi.'*v2; sg2= sqrt(1-cg2.^2); D2= cg2; D3= cg2; didx= find(idx); for i= 1:n-2 D2(i)= norm(XX(:,i1)-XX(:,didx(i))); D3(i)= norm(XX(:,didx(i))-XX(:,i2)); end A1= (D2./D1).^2; A2= A1*sg1^2-sg2.^2; A3= cg2.*cg3-cg1; A4= cg1*cg3-cg2; A6= (D3.^2-D1^2-D2.^2)./(2*D1^2); A7= 1-cg1^2-cg2.^2+cg1*cg2.*cg3+A6.*sg1^2; D4= [A6.^2-A1.*cg3.^2, 2*(A3.*A6-A1.*A4.*cg3),... A3.^2+2*A6.*A7-A1.*A4.^2-A2.*cg3.^2,... 2*(A3.*A7-A2.*A4.*cg3), A7.^2-A2.*A4.^2]; F7= [4*D4(:,1).^2,... 7*D4(:,2).*D4(:,1),... 6*D4(:,3).*D4(:,1)+3*D4(:,2).^2,... 5*D4(:,4).*D4(:,1)+5*D4(:,3).*D4(:,2),... 4*D4(:,5).*D4(:,1)+4*D4(:,4).*D4(:,2)+2*D4(:,3).^2,... 3*D4(:,5).*D4(:,2)+3*D4(:,4).*D4(:,3),... 2*D4(:,5).*D4(:,3)+D4(:,4).^2,... D4(:,5).*D4(:,4)]; D7= sum(F7); end % 求代价函数的极小值。 t2s= roots(D7); maxreal= max(abs(real(t2s))); t2s(abs(imag(t2s))/maxreal > 0.001)= []; % 虚部过大或过小值置空？ t2s= real(t2s); D6= (7:-1:1).*D7(1:7); F6= polyval(D6,t2s); % 求解多项式的值？ t2s(F6 2e-2 R2(:,3)= -Z; end return function c = xcross(a,b) % 向量叉乘 c = [a(2)*b(3)-a(3)*b(2); a(3)*b(1)-a(1)*b(3); a(1)*b(2)-a(2)*b(1)]; returnemmm。。。好像之前的上传的文件失效了。我又上传到百度网盘了，需要的可以自取。 链接：https://pan.baidu.com/s/1YgAWTulRPL9LlpCE4XVodw 提取码：qwer 文章参考： 1.Li S, Xu C. A stable direct solution of perspective-three-point problem[J]. International Journal of Pattern Recognition and Artificial Intelligence, 2011, 25(05): 627-642. 2.Li S, Xu C, Xie M. A robust O (n) solution to the perspective-n-point problem[J]. IEEE transactions on pattern analysis and machine intelligence, 2012, 34(7): 1444-1450. 相关资源：RPnP论文+Matlab代码其中包含epnp，lhm等算法，以及论文原文图片生成代码，各个文件的详解_EPnpmatlab-机器学习文档类资源-CSDN文库 |
【本文地址】