RPnP算法原文及代码解析

大大大管的笔记本

已于 2023-05-08 20:26:40 修改

阅读量3.5k

点赞数 9

文章标签：计算机视觉人工智能目标跟踪

于 2022-03-24 12:11:33 首次发布

本文链接：https://blog.csdn.net/cg1135217680/article/details/123705673

版权

（有不对的地方希望得到您的指正~~）

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 <= 0)= [];

if isempty(t2s)
    fprintf('no solution!\n');
    return
end

% 从每个局部最小值计算相机姿势。
m= length(t2s);
for i= 1:m
    t2= t2s(i);
    % 计算旋转矩阵
    d2= cg1+t2;
    x= v2*d2- v1; 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);
    else
        y= xcross([0; 0; 1], x); y= y/norm(y);
        z= xcross(x,y); z= z/norm(z);
    end
    Rx= [x y z];  % 相机坐标系下的三角形坐标系的旋转矩阵

    % 计算 c, s, tx, ty, tz
    D= zeros(2*n,6);
    r= Rx.';
    for j= 1:n
        ui= xx(1,j); vi= xx(2,j);
        xi= XX(1,j); yi= XX(2,j); zi= XX(3,j);
        % 下面这个算式应该是生成2nx6的矩阵
        D(2*j-1,:)= [-r(2)*yi+ui*(r(8)*yi+r(9)*zi)-r(3)*zi, ...
            -r(3)*yi+ui*(r(9)*yi-r(8)*zi)+r(2)*zi, ...
            -1, 0, ui, ui*r(7)*xi-r(1)*xi];
        D(2*j, :)= [-r(5)*yi+vi*(r(8)*yi+r(9)*zi)-r(6)*zi, ...
            -r(6)*yi+vi*(r(9)*yi-r(8)*zi)+r(5)*zi, ...
            0, -1, vi, vi*r(7)*xi-r(4)*xi];
    end
    DTD= D.'*D;
    [V D]= eig(DTD);
    
    V1= V(:,1); V1= V1/V1(end);
    c= V1(1); s= V1(2); t= V1(3:5);  % 当前三角形坐标系的c,s,t

    % 通过3d对齐计算相机姿态
    xi= XX(1,:); yi= XX(2,:); zi= XX(3,:);
    % 旋转至相机坐标系后的三角形物体坐标系，即测量值
    XXcs= [r(1)*xi+(r(2)*c+r(3)*s)*yi+(-r(2)*s+r(3)*c)*zi+t(1);
        r(4)*xi+(r(5)*c+r(6)*s)*yi+(-r(5)*s+r(6)*c)*zi+t(2);
        r(7)*xi+(r(8)*c+r(9)*s)*yi+(-r(8)*s+r(9)*c)*zi+t(3)];
    
    XXc= zeros(size(XXcs));
    for j= 1:n
        XXc(:,j)= xxv(:,j)*norm(XXcs(:,j));  % xxv之前归一化，现在还原
    end
    
    [R t]= calcampose(XXc,XXw);  % XXc为测量值，XXw为原物体坐标系的值
    
    % 计算重投影误差
    XXc= R*XXw+t*ones(1,n);
    xxc= [XXc(1,:)./XXc(3,:); XXc(2,:)./XXc(3,:)];
    %r= mean(sqrt(sum((xxc-xx).^2)));
    r = norm(xxc-xx,'fro')/n;
    
    res{i}.R= R;
    res{i}.t= t;
    res{i}.r= r;
end

% 以最小的重投影误差确定相机姿态。
minr= inf;
for i= 1:m
    if res{i}.r < minr
        minr= res{i}.r;
        R= res{i}.R;
        t= res{i}.t;
    end
end

return

function B = getp3p(l1,l2,A5,C1,C2,D1,D2,D3)  % 获取PST方法中的系数矩阵

A1= (D2/D1)^2;
A2= A1*C1^2-C2^2;
A3= l2*A5-l1;
A4= l1*A5-l2;
A6= (D3^2-D1^2-D2^2)/(2*D1^2);
A7= 1-l1^2-l2^2+l1*l2*A5+A6*C1^2;

B= [A6^2-A1*A5^2, 2*(A3*A6-A1*A4*A5), A3^2+2*A6*A7-A1*A4^2-A2*A5^2,...
    2*(A3*A7-A2*A4*A5), A7^2-A2*A4^2];

return

function F7= getpoly7(F)  % F(x)*F'(x)的7阶系数矩阵

F7= [4*F(1)^2;
7*F(2)*F(1);
6*F(3)*F(1)+3*F(2)^2;
5*F(4)*F(1)+5*F(3)*F(2);
4*F(5)*F(1)+4*F(4)*F(2)+2*F(3)^2;
3*F(5)*F(2)+3*F(4)*F(3);
2*F(5)*F(3)+F(4)^2;
F(5)*F(4)].';

return

function [R2,t2] = calcampose(XXc,XXw)  % 计算位姿

n= length(XXc);

X= XXw;
Y= XXc;

K= eye(n)-ones(n,n)/n;

ux= mean(X,2);
uy= mean(Y,2);
sigmx2= mean(sum((X*K).^2));

SXY= Y*K*(X')/n;
[U, D, V]= svd(SXY);
S= eye(3);
if det(SXY) < 0
    S(3,3)= -1;
end

R2= U*S*(V');
c2= trace(D*S)/sigmx2;
t2= uy-c2*R2*ux;

X= R2(:,1);
Y= R2(:,2);
Z= R2(:,3);
if norm(xcross(X,Y)-Z) > 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)];
 
 return

emmm。。。好像之前的上传的文件失效了。我又上传到百度网盘了，需要的可以自取。

链接：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.