MATLAB函数mapping：从图像到世界坐标系的映射

该函数 mapping 用于将图像中的像素坐标映射到世界坐标系，它接受以下参数：

• image: 图像矩阵。
• y: 激光扫描点的y方向角度（欧拉角）。
• x: 激光扫描点的x方向角度（欧拉角）。
• las_dist: 激光扫描点的距离。
• ocam_model: 相机模型参数。

函数返回两个向量：

• x1: 映射后世界坐标系的x坐标。
• y1: 映射后世界坐标系的y坐标。

函数的主要步骤如下：

1. 获取图像的宽度和高度。
2. 将激光距离 las_dist 赋值给变量 Z。
3. 初始化一个计数器 a 用于存储映射后的坐标。
4. 初始化空向量 x1 和 y1 用于存储映射后的坐标。
5. 创建平移向量 t，表示激光点在相机坐标系中的位置。
6. 使用 compose_rotation 函数将欧拉角 (x, y, 0) 转换为旋转矩阵 r，并将平移向量 t 添加到旋转矩阵中。
7. 遍历图像的每个像素，对非零像素执行以下操作：
   • 获取当前像素的坐标 (i, j)。
   • 使用 cam2world 函数将图像平面上的坐标 (i, j) 转换为相机平面上的坐标 M。
   • 计算一些中间变量 a1, b1, c1, a2, b2, c2 用于后续计算。
   • 根据公式计算出相机平面上的二维坐标 (X, Y)。
   • 将计算出的 (X, Y) 保存到 x1 和 y1 向量中。
8. 返回 x1 和 y1 向量。

该代码假设 compose_rotation 和 cam2world 函数已经定义并且可用。

代码示例:

function [x1,y1] = mapping(image,y,x,las_dist,ocam_model)
[height,width] = size(image);
Z=las_dist;
a = 1;
x1=[];
y1=[];
t = [0;0;Z];
r = compose_rotation(-x, -y, 0);
r = [r(:,1),r(:,2),t];
for i=1:height      % working image region
    for j=1:width
        if image(i,j)>0 
            m=[i;j];       % image pixels
            M = cam2world(m,ocam_model); % transform from image plane to the camera plane          
            a1 = M(1)*r(2,1)-M(2)*r(1,1);
            b1 = M(1)*r(2,2)-M(2)*r(1,2);
            c1 = M(1)*r(2,3)-M(2)*r(1,3);
            a2 = M(3)*r(1,1)-M(1)*r(3,1);
            b2 = M(3)*r(1,2)-M(1)*r(3,2);
            c2 = M(3)*r(1,3)-M(1)*r(3,3);

            Y = (a2*c1-a1*c2)/(a1*b2-a2*b1);
            X = (-c1-b1*Y)/a1;
              
            y1(a)=X;
            x1(a)=-Y; 
            a=a+1;
        end
    end
end
end