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

SL…

于 2021-10-08 16:36:39 发布

阅读量595

点赞数

文章标签：深度学习 matlab 最小二乘法算法数学

本文链接：https://blog.csdn.net/weixin_43904073/article/details/120650053

版权

该博客详细介绍了如何使用最小二乘法来估计模型相对于相机的位姿。通过已知的相机内参、模型特征点的3D坐标以及图像中的像素坐标，利用迭代优化方法不断调整旋转和平移矩阵，直至残差误差低于设定阈值。代码示例展示了在MATLAB中实现这一过程，包括图像特征点的绘制和误差计算，最终确定模型的精确位置。

摘要由CSDN通过智能技术生成

已知条件为：

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 $\delta$ x is a very small value)

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

$p = K.M_{ext}._{}^{W}\textrm{P}$

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

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

K为instric parameters（focal point （xc,yc）; image center (ximg,yimg)） $\begin{bmatrix} f_{x}& 0& c_{x}\\ 0& f_{y}& c_{y}\\ 0& 0& 1 \end{bmatrix}$

需要优化的参数就是旋转和平移矩阵 $\begin{bmatrix} r& r& r& t\\ r& r& r& t\\ r& r& r& t\\ 0& 0& 0& 1 \end{bmatrix}$

%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 %d\nCurrent 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