相机校参及C-arm校参 07 - 根据2D图像计算相机的相对位姿

已知条件为：

1.calibrated camera 校准后相机的intrisic parameters （K）

2. Model geometry 模型中特征点（有些文献中成为标记物Tag）的3D Coordinate， noted as "X"

3. 图像中的位置坐标, noted as "x"

4.计算求解的速度和initial guess有关

求：Model（或者说是Model上的特征点）相对于相机的pose （位姿）

方法：根据最小二乘法预估图像中的位置直至收敛（supposing E<0.00001 ）

(supposing x is a very small value)

 根据公式我们可以列出以下等式：

小p为图像中的坐标（x,y）

大P为Model坐标系下的坐标（X,Y,Z）

K为instric parameters（focal point （xc,yc）; image center (ximg,yimg)）

需要优化的参数就是旋转和平移矩阵

%matlab

function p = fProject (x, P_M; K)

ax = x(1);
ay = x(2);
az = x(3);
tx = x(4);
ty = x(5);
tz = x(6);

Rx = [ 1 0 0; 0 cos(ax) -sin(ay); 0, sin(ax) cos(ax)];
Ry = [cos(ay) 0 sin(ay); 0 1 0; -sin(ay) 0 cos(ay)];
Rz = [cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 1];
R = Rx*Ry*Rz

Mext = [R [tx; ty; tz]];

ph = K*Mext*P_M;

%divide through 3rd element of each column to get rid of 3rd row%
ph(1,:) = ph(1,:)./ph(3,:);
ph(2,:) = ph(2,:)./ph(3,:);
ph = ph(1:2,:);

p = reshape(ph, [], 1);

return


I = imread("XXX.png");
imshow(I,[]);

%the position of feature in Model coordinate%
P_M = [ 0 0 2  0 0 2;
       10 2 0 10 2 0;
        6 6 6  2 2 2;
        1 1 1  1 1 1 ];
f = 715; cx =354; cy = 245;
K = [ f 0 cx;
      0 f cy;
      0 0  1 ];
%y0 is a 6X1 matrix by [x0;y0;x1;y1;...]%
y0 = [ 183, 147; 
       350; 133;
       454; 144;
       176; 258;
       399, 275;
       444; 286];
%initial guess%
x = [1.5; -10.0; 0.0; 0.0; 0.0; 30.0];

y = fProject (x, P_M; K);

for i =1:2:length(y)
    rectangle('Position', [y(i)-8 y(i+1)-8, 16, 16]); 'FaceColor', 'r');
end 


for i = 1:10
    fprinf('nIteration %dnCurrent pose:n', i);
    disp(x);

    y = fproject(x, P_M, K);
    
    imshow(I, []);

    for i =1:2:length(y)
    rectangle('Position', [y(i)-8 y(i+1)-8, 16, 16]); 'FaceColor', 'r');
    end

    pause(1);
    
    e = 0.00001;
    dy = y0 -y;
    fprintf('Residual error: %f n', norm(dy));

    J(:,1) = (fProject(x+[e;0,0,0,0,0],P_M,K)-y)/e;
    J(:,2) = (fProject(x+[0;e,0,0,0,0],P_M,K)-y)/e;
    J(:,3) = (fProject(x+[0;0,e,0,0,0],P_M,K)-y)/e;
    J(:,4) = (fProject(x+[0;0,0,e,0,0],P_M,K)-y)/e;
    J(:,5) = (fProject(x+[0;0,0,0,e,0],P_M,K)-y)/e;
    J(:,6) = (fProject(x+[0;0,0,0,0,e],P_M,K)-y)/e;
    
    dx = pinv(J)*dy;
    
    if abs (norm(dx)/norm(x)) <1e-6
        break;
    end
    
    x = x+dx;
end