省流说明
本文提供一个仿真环境，用于估算出激光扫描在物体上和背景上的覆盖面积
本文是另一个帖子的延续版本，实现原理请移步这里
激光扫描三维空间测量点模拟（Matlab源码）
- 黄色：是建立的前景物体；
- 红色：打在物体上的激光扫描点
- 蓝色：在该面上的覆盖区域
实现原理
计算交点的实现原理请移步另一个帖子，计算面积的实现原理如下：
主要是用了以下两个Matlab内置函数：
boundary 计算好的边界使用fill3填充，显示在图像中。
polyarea将boundary的输出作为输入，计算面积，可以分别得到每个平面上激光扫描的覆盖面积
Matlab程序实现
主程序——Environment_Main.m
clc;clear;
% 基本参数设置部分----------------------------------------------------------
% 屋子参数设置
roomX = 8000;
roomY = 9000;
roomZ = 4000;
% 绘图设置-----------------------------------------------------------------
axis equal;
axis([-roomX/2,roomX/2,-roomY/2,roomY/2,0,roomZ]);
hold on; grid on;
xlabel('x');ylabel('y');zlabel('z');
% 主程序-------------------------------------------------------------------
% 生成基础激光束--------------------------------------------------------------
stepH = 1; stepV =1; %stepH:俯仰步进角 stepV:水平步进角
LiDAR.laserCount = 0; % 生成的激光束计数
for a = -90:stepH:68 % 俯仰角
for b = -180:stepV:180
LiDAR.laserCount = LiDAR.laserCount+1;
c = 0;
RA = Base_RotateMatrix(a,b,c);
laser_point = RA * [100 0 0]';
laserPoints(LiDAR.laserCount,1:3) = laser_point';
end
end
LiDAR.Directions =[];
% 生成多个不同位置和位姿的激光束，并结合起来--------------------------------------------
% 主要是考虑扫描仪器可以在多个视点扫描
% 激光束zmin-----------------------------------------------------------------
localLocation = [0 0 1000]; % 激光扫描仪坐标原点
localRotation = [0 0 0]; % RPY旋转角
localDirections = (Base_RotateMatrix(localRotation(1),localRotation(2),localRotation(3))*laserPoints')';
localDirections(:,4:6) = ones(size(localDirections,1),1)*localLocation;
if(isempty(LiDAR.Directions))
LiDAR.Directions = localDirections;
else
LiDAR.Directions = [LiDAR.Directions;localDirections];
end
% % 激光束xmin-----------------------------------------------------------------
% localLocation = [-806.435 310.859 1250]; % 激光扫描仪坐标原点
% localRotation = [-31 3.39 -12.2]; % RPY旋转角
% localDirections = (Base_RotateMatrix(localRotation(1),localRotation(2),localRotation(3))*laserPoints')';
% localDirections(:,4:6) = ones(size(localDirections,1),1)*localLocation;
% if(isempty(LiDAR.Directions))
% LiDAR.Directions = localDirections;
% else
% LiDAR.Directions = [LiDAR.Directions;localDirections];
% end
% % 激光束xmax-----------------------------------------------------------------
% localLocation = [806.435 310.859 1250]; % 激光扫描仪坐标原点
% localRotation = [31 -3.39 -12.2]; % RPY旋转角
% localDirections = (Base_RotateMatrix(localRotation(1),localRotation(2),localRotation(3))*laserPoints')';
% localDirections(:,4:6) = ones(size(localDirections,1),1)*localLocation;
% if(isempty(LiDAR.Directions))
% LiDAR.Directions = localDirections;
% else
% LiDAR.Directions = [LiDAR.Directions;localDirections];
% end
% % 激光束ymin-----------------------------------------------------------------
% localLocation = [0 -919.479 1250]; % 激光扫描仪坐标原点
% localRotation = [0 0 34.4]; % RPY旋转角
% localDirections = (Base_RotateMatrix(localRotation(1),localRotation(2),localRotation(3))*laserPoints')';
% localDirections(:,4:6) = ones(size(localDirections,1),1)*localLocation;
% if(isempty(LiDAR.Directions))
% LiDAR.Directions = localDirections;
%
% else
% LiDAR.Directions = [LiDAR.Directions;localDirections];
% end
% % 激光束ymax-----------------------------------------------------------------
% localLocation = [-90.7261 717.68 1250]; % 激光扫描仪坐标原点
% localRotation = [-3.2 0.90 -31.6]; % RPY旋转角
% localDirections = (Base_RotateMatrix(localRotation(1),localRotation(2),localRotation(3))*laserPoints')';
% localDirections(:,4:6) = ones(size(localDirections,1),1)*localLocation;
% if(isempty(LiDAR.Directions))
% LiDAR.Directions = localDirections;
% else
% LiDAR.Directions = [LiDAR.Directions;localDirections];
% end
% % 激光束zmax-----------------------------------------------------------------
% localLocation = [0 0 1800]; % 激光扫描仪坐标原点
% localRotation = [0 0 0]; % RPY旋转角
% localDirections = (Base_RotateMatrix(localRotation(1),localRotation(2),localRotation(3))*laserPoints')';
% localDirections(:,4:6) = ones(size(localDirections,1),1)*localLocation;
% if(isempty(LiDAR.Directions))
% LiDAR.Directions = localDirections;
% else
% LiDAR.Directions = [LiDAR.Directions;localDirections];
% end
% 给Flag数组赋值------------------------------------------------------------
LiDAR.Flag = ones(size(LiDAR.Directions,1),1); % 用来标记哪些射线已经射到物体上
% ***在这里添加物体***----------------------------------------------------
% 物体-------------------------------------------------------------------
cube.scale = [600 600 400]; % 立方体大小
cube.Location = [2000 0 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 600]; % 立方体大小
cube.Location = [2000 -2000 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 800]; % 立方体大小
cube.Location = [0 -2000 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 1000]; % 立方体大小
cube.Location = [-2000 -2000 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 1200]; % 立方体大小
cube.Location = [-2000 0 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 1400]; % 立方体大小
cube.Location = [-2000 2000 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 1600]; % 立方体大小
cube.Location = [0 2000 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 物体-------------------------------------------------------------------
cube.scale = [600 600 1800]; % 立方体大小
cube.Location = [2000 2000 cube.scale(3)/2]; % 立方体位置
cube.Rotation = [0 0 0]; % 立方体旋转量
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Cube_Line(LiDAR,cube_output); % 计算射线和立方体的交点
% 和屋子墙壁的交点----------------------------------------------------------
cube.scale = [roomX,roomY,roomZ];
cube.Location = [0 0 roomZ/2];
cube.Rotation = [0 0 0];
cube_output = Environment_Driver_Box_Create(cube); % 生成立方体顶点坐标（用于显示）、平面参数（用于检测光）
% Environment_Driver_Box_Screen(cube_output); % 将立方体显示在图像中
LiDAR.Flag = Environment_Driver_Room_Line(LiDAR,cube_output); % 计算射线和立方体的交点
hold on;
% fill3([-roomX/2,roomX/2,roomX/2,-roomX/2],[roomY/2,roomY/2,-roomY/2,-roomY/2],[0,0,0,0],[1 1 1]);% 地面上色
% fill3([-roomX/2,roomX/2,roomX/2,-roomX/2],[roomY/2,roomY/2,roomY/2,roomY/2],[roomZ,roomZ,0,0],[1 1 1]);% 前墙面上色
% fill3([roomX/2,roomX/2,roomX/2,roomX/2],[-roomY/2,roomY/2,roomY/2,-roomY/2],[roomZ,roomZ,0,0],[1 1 1]);% 右墙面上色
%--------------------------------------------------------------------------
%scatter3(LiDAR.Location(1),LiDAR.Location(2),LiDAR.Location(3),200,'.');
clear;
函数：创建立方体——Environment_Driver_Box_Create.m
% 根据位置设定、旋转关系，创建立方体及需要的其它参数
function output = Environment_Driver_Box_Create(input)
% 8个交点的坐标
% E ——— F
% / / |
% / / |
% A ——— B G
% | /H | /
% | / | /
% D ————C
Point = zeros(6,3);
Point(1,:) = [input.scale(1)/2 -input.scale(2)/2 input.scale(3)/2]; % 点A
Point(2,:) = [input.scale(1)/2 input.scale(2)/2 input.scale(3)/2]; % 点B
Point(3,:) = [input.scale(1)/2 input.scale(2)/2 -input.scale(3)/2]; % 点C
Point(4,:) = [input.scale(1)/2 -input.scale(2)/2 -input.scale(3)/2]; % 点D
Point(5,:) = [-input.scale(1)/2 -input.scale(2)/2 input.scale(3)/2]; % 点E
Point(6,:) = [-input.scale(1)/2 input.scale(2)/2 input.scale(3)/2]; % 点F
Point(7,:) = [-input.scale(1)/2 input.scale(2)/2 -input.scale(3)/2]; % 点G
Point(8,:) = [-input.scale(1)/2 -input.scale(2)/2 -input.scale(3)/2]; % 点H
% 根据输入进行旋转
a = input.Rotation(2);b = input.Rotation(3);c = input.Rotation(1);
% RPY旋转矩阵
RA(1,1) = cosd(a)*cosd(b);
RA(2,1) = cosd(a)*sind(b);
RA(3,1) = -sind(a);
RA(1,2) = sind(c)*sind(a)*cosd(b)-cosd(c)*sind(b);
RA(2,2) = sind(c)*sind(a)*sind(b)+cosd(c)*cosd(b);
RA(3,2) = sind(c)*cosd(a);
RA(1,3) = cosd(c)*sind(a)*cosd(b)+sind(c)*sind(b);
RA(2,3) = cosd(c)*sind(a)*sind(b)-sind(c)*cosd(b);
RA(3,3) = cosd(c)*cosd(a);
% 根据旋转矩阵和位置关系对立方体坐标变换
Point = RA * Point'+input.Location';
output.point = zeros(3,8);
output.point = Point;
Point = Point';
output.plane(1,:) = Environment_Driver_Plane_Matching(Point([1 2 3 4],:)); % ABCD面 *因为要计算法线方向，这个顺序不能打乱
output.plane(2,:) = Environment_Driver_Plane_Matching(Point([6 2 1 5],:)); % FBAE面 *因为要计算法线方向，这个顺序不能打乱
output.plane(3,:) = Environment_Driver_Plane_Matching(Point([3 2 6 7],:)); % CBFG面 *因为要计算法线方向，这个顺序不能打乱
output.plane(4,:) = Environment_Driver_Plane_Matching(Point([8 4 3 7],:)); % HDCG面 *因为要计算法线方向，这个顺序不能打乱
output.plane(5,:) = Environment_Driver_Plane_Matching(Point([8 7 6 5],:)); % HGFE面 *因为要计算法线方向，这个顺序不能打乱
output.plane(6,:) = Environment_Driver_Plane_Matching(Point([1 4 8 5],:)); % ADHE面 *因为要计算法线方向，这个顺序不能打乱
output.line(1,:) = horzcat(Point(1,:),Point(2,:),Point(3,:),Point(4,:)); % ABCD面的四个坐标，后面获取直线方程
output.line(2,:) = horzcat(Point(6,:),Point(2,:),Point(1,:),Point(5,:)); % FBAE面的四个坐标，后面获取直线方程
output.line(3,:) = horzcat(Point(3,:),Point(2,:),Point(6,:),Point(7,:)); % CBFG面的四个坐标，后面获取直线方程
output.line(4,:) = horzcat(Point(8,:),Point(4,:),Point(3,:),Point(7,:)); % HDCG面的四个坐标，后面获取直线方程
output.line(5,:) = horzcat(Point(8,:),Point(7,:),Point(6,:),Point(5,:)); % HGFE面的四个坐标，后面获取直线方程
output.line(6,:) = horzcat(Point(1,:),Point(4,:),Point(8,:),Point(5,:)); % ADHE面的四个坐标，后面获取直线方程
output.surfaceArea = (norm(Point(1,:)-Point(2,:))*norm(Point(3,:)-Point(2,:))+norm(Point(2,:)-Point(1,:))*norm(Point(2,:)-Point(6,:))+norm(Point(2,:)-Point(3,:))*norm(Point(2,:)-Point(6,:)))*2;
% 用来将面上的点云三维点转化到二维平面上以计算其包围面积
output.x(1,:) = Point(2,:)-Point(1,:);
output.x(1,:) = output.x(1,:)/norm(output.x(1,:));
output.y(1,:) = Point(2,:)-Point(3,:);
output.y(1,:) = output.y(1,:)/norm(output.y(1,:));
output.x(2,:) = Point(2,:)-Point(6,:);
output.x(2,:) = output.x(2,:)/norm(output.x(2,:));
output.y(2,:) = Point(2,:)-Point(1,:);
output.y(2,:) = output.y(2,:)/norm(output.y(2,:));
output.x(3,:) = Point(2,:)-Point(3,:);
output.x(3,:) = output.x(3,:)/norm(output.x(3,:));
output.y(3,:) = Point(2,:)-Point(6,:);
output.y(3,:) = output.y(3,:)/norm(output.y(3,:));
output.x(4,:) = Point(4,:)-Point(8,:);
output.x(4,:) = output.x(4,:)/norm(output.x(4,:));
output.y(4,:) = Point(4,:)-Point(3,:);
output.y(4,:) = output.y(4,:)/norm(output.y(4,:));
output.x(5,:) = Point(7,:)-Point(8,:);
output.x(5,:) = output.x(5,:)/norm(output.x(5,:));
output.y(5,:) = Point(7,:)-Point(6,:);
output.y(5,:) = output.y(5,:)/norm(output.y(5,:));
output.x(6,:) = Point(4,:)-Point(1,:);
output.x(6,:) = output.x(6,:)/norm(output.x(6,:));
output.y(6,:) = Point(4,:)-Point(8,:);
output.y(6,:) = output.y(6,:)/norm(output.y(6,:));
output.MF1 = [[output.x(1,:)' output.y(1,:)' [0 0 0]' Point(2,:)'];[0 0 0 1]];
output.MF2 = [[output.x(2,:)' output.y(2,:)' [0 0 0]' Point(2,:)'];[0 0 0 1]];
output.MF3 = [[output.x(3,:)' output.y(3,:)' [0 0 0]' Point(2,:)'];[0 0 0 1]];
output.MF4 = [[output.x(4,:)' output.y(4,:)' [0 0 0]' Point(4,:)'];[0 0 0 1]];
output.MF5 = [[output.x(5,:)' output.y(5,:)' [0 0 0]' Point(7,:)'];[0 0 0 1]];
output.MF6 = [[output.x(6,:)' output.y(6,:)' [0 0 0]' Point(4,:)'];[0 0 0 1]];
output.centerPoint(1,:) = (Point(1,:)+Point(2,:)+Point(3,:)+Point(4,:))/4; % ABCD面的中心点
output.centerPoint(2,:) = (Point(6,:)+Point(2,:)+Point(1,:)+Point(5,:))/4; % FBAE面的中心点
output.centerPoint(3,:) = (Point(3,:)+Point(2,:)+Point(6,:)+Point(7,:))/4; % CBFG面的中心点
output.centerPoint(4,:) = (Point(8,:)+Point(4,:)+Point(3,:)+Point(7,:))/4; % HDCG面的中心点
output.centerPoint(5,:) = (Point(8,:)+Point(7,:)+Point(6,:)+Point(5,:))/4; % HGFE面的中心点
output.centerPoint(6,:) = (Point(1,:)+Point(4,:)+Point(8,:)+Point(5,:))/4; % ADHE面的中心点
end
函数：显示立方体——Environment_Driver_Box_Screen
% 根据输入的立方体8个点来显示在屏幕中
function Environment_Driver_Box_Screen(input)
local.point = input.point;
fillcolor= 1;
hold on;
fill3(local.point(1,[1 2 3 4]),local.point(2,[1 2 3 4]),local.point(3,[1 2 3 4]),fillcolor);
fill3(local.point(1,[1 4 8 5]),local.point(2,[1 4 8 5]),local.point(3,[1 4 8 5]),fillcolor);
fill3(local.point(1,[8 7 6 5]),local.point(2,[8 7 6 5]),local.point(3,[8 7 6 5]),fillcolor);
fill3(local.point(1,[6 7 3 2]),local.point(2,[6 7 3 2]),local.point(3,[6 7 3 2]),fillcolor);
fill3(local.point(1,[1 2 6 5]),local.point(2,[1 2 6 5]),local.point(3,[1 2 6 5]),fillcolor);
fill3(local.point(1,[3 4 8 7]),local.point(2,[3 4 8 7]),local.point(3,[3 4 8 7]),fillcolor);
end
函数：立方体平面拟合——Environment_Driver_Plane_Matching
% 输入四个点，计算该立方体平面参数
function output = Environment_Driver_Plane_Matching(input)
% B(p1) /\ /| G(p2)
% | /
% | /
% C ————>p
p1 = input(2,:)-input(1,:);
p2 = input(2,:)-input(3,:);
p = cross(p1,p2);
p = p/norm(p);
output(1,1:3) = p';
output(1,4) = -input(1,:)*p';
end
函数：立方体与射线交点——Environment_Driver_Cube_Line.m
% 本函数用来计算射线和一个正方体的所有表面交点
function output = Environment_Driver_Cube_Line(LiDAR,cube)
% 初始化----------------------------------------------------------------
InitialPoint = zeros(5000,3); % 初始化矩阵，用来存放所有交点
PlanePoint1 = zeros(1,3);
PlanePoint2 = zeros(1,3);
PlanePoint3 = zeros(1,3);
PlanePoint4 = zeros(1,3);
PlanePoint5 = zeros(1,3);
PlanePoint6 = zeros(1,3);
hold on;
% 遍历所有激光射线------------------------------------------------------
for i=1:1:size(LiDAR.Directions,1)
if(LiDAR.Flag(i) == 0) % 该射线已经打到别的物体上，则不考虑该射线
continue;
end
laserDistance = 100000; % 只留表面点
savePoint = [0 0 0];
saveFlag = 0;
planeFlag = 0;
% 当前激光束打在六个平面上的点的坐标---------------------------------
for j=1:1:6
% 防止射线从相反的方向与平面相交---------------------------------
% refLine = cube.centerPoint(j,:) - LiDAR.Directions(i,4:6); % 从雷达原点到平面中点的向量
% sigma = acos(dot(refLine,LiDAR.Directions(i,1:3))/(norm(refLine)*norm(LiDAR.Directions(i,1:3))));
% if(sigma*180/pi>=90)
% continue;
% end
% 计算与该平面的交点--------------------------------------------
n1 = abs(cube.plane(j,1:3)*LiDAR.Directions(i,1:3)');
n2 = abs(cube.plane(j,1:3)*LiDAR.Directions(i,4:6)'+cube.plane(j,4));
n = n1/n2;
if(n==0)
continue;
end
localPoint = LiDAR.Directions(i,4:6)+LiDAR.Directions(i,1:3)/n;
% 判断交点是否在平面内------------------------------------------
% 获取平面的长和宽，求该点和四条边的距离。该距离必须小于长和宽----
L1 = int32(norm(cube.line(j,1:3) - cube.line(j,4:6)));
L2 = int32(norm(cube.line(j,4:6) - cube.line(j,7:9)));
d1 = norm(cross(localPoint-cube.line(j,1:3),localPoint-cube.line(j,4:6)))/norm(cube.line(j,4:6)-cube.line(j,1:3));
d2 = norm(cross(localPoint-cube.line(j,4:6),localPoint-cube.line(j,7:9)))/norm(cube.line(j,4:6)-cube.line(j,7:9));
d3 = norm(cross(localPoint-cube.line(j,7:9),localPoint-cube.line(j,10:12)))/norm(cube.line(j,7:9)-cube.line(j,10:12));
d4 = norm(cross(localPoint-cube.line(j,10:12),localPoint-cube.line(j,1:3)))/norm(cube.line(j,10:12)-cube.line(j,1:3));
if(d1>L2||d3>L2||d2>L1||d4>L1)
continue;
end
% 判断点是否在平面上（不判断有时候会出BUG）----------------------
if(abs(cube.plane(j,1:3)*localPoint'+cube.plane(j,4))>5)
continue;
end
% 一条线与立方体相交会有两个点，只保留距离近的表面点--------------
localPointDistance = norm(localPoint-LiDAR.Directions(i,4:6));
if(localPointDistance<laserDistance)
laserDistance = localPointDistance;
savePoint = localPoint;
saveFlag = 1;
planeFlag = j;
end
% 调试用-------------------------------------------------------
% scatter3((LiDAR.Location(1)+LiDAR.Directions(i,1)),(LiDAR.Location(2)+LiDAR.Directions(i,2)),(LiDAR.Location(3)+LiDAR.Directions(i,3)),'.');
% scatter3(localPoint(1),localPoint(2),localPoint(3),'.');
end
% 保存所有满足要求的点到矩阵中---------------------------------------
if(saveFlag == 1)
InitialPoint = [InitialPoint;savePoint];
LiDAR.Flag(i) = 0;
% 按平面保存到不同的矩阵中，用于计算面积
switch(planeFlag)
case 1
PlanePoint1 = [PlanePoint1;savePoint];
case 2
PlanePoint2 = [PlanePoint2;savePoint];
case 3
PlanePoint3 = [PlanePoint3;savePoint];
case 4
PlanePoint4 = [PlanePoint4;savePoint];
case 5
PlanePoint5 = [PlanePoint5;savePoint];
case 6
PlanePoint6 = [PlanePoint6;savePoint];
end
end
end
PlanePoint1(1,:) = []; % 初始化时第一排是0，删除
PlanePoint2(1,:) = []; % 初始化时第一排是0，删除
PlanePoint3(1,:) = []; % 初始化时第一排是0，删除
PlanePoint4(1,:) = []; % 初始化时第一排是0，删除
PlanePoint5(1,:) = []; % 初始化时第一排是0，删除
PlanePoint6(1,:) = []; % 初始化时第一排是0，删除
cube.MF1(1:3,3) = cube.plane(1,1:3)';% 坐标变换矩阵，
cube.MF2(1:3,3) = cube.plane(2,1:3)';% 坐标变换矩阵
cube.MF3(1:3,3) = cube.plane(3,1:3)';% 坐标变换矩阵
cube.MF4(1:3,3) = cube.plane(4,1:3)';% 坐标变换矩阵
cube.MF5(1:3,3) = cube.plane(5,1:3)';% 坐标变换矩阵
cube.MF6(1:3,3) = cube.plane(6,1:3)';% 坐标变换矩阵
localPlanePoint1=inv(cube.MF1(1:3,1:3))*PlanePoint1'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint2=inv(cube.MF2(1:3,1:3))*PlanePoint2'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint3=inv(cube.MF3(1:3,1:3))*PlanePoint3'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint4=inv(cube.MF4(1:3,1:3))*PlanePoint4'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint5=inv(cube.MF5(1:3,1:3))*PlanePoint5'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint6=inv(cube.MF6(1:3,1:3))*PlanePoint6'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
S1=0;S2=0;S3=0;S4=0;S5=0;S6=0;
if(numel(localPlanePoint1)~=0) % 如果矩阵不为空
b1 = boundary(localPlanePoint1(1,:)',localPlanePoint1(2,:)');
S1 = polyarea(localPlanePoint1(1,b1)',localPlanePoint1(2,b1)');
fill3(PlanePoint1(b1,1)',PlanePoint1(b1,2)',PlanePoint1(b1,3)',0.5);
end
if(numel(localPlanePoint2)~=0) % 如果矩阵不为空
b2 = boundary(localPlanePoint2(1,:)',localPlanePoint2(2,:)');
S2 = polyarea(localPlanePoint2(1,b2)',localPlanePoint2(2,b2)');
fill3(PlanePoint2(b2,1)',PlanePoint2(b2,2)',PlanePoint2(b2,3)',0.5);
end
if(numel(localPlanePoint3)~=0) % 如果矩阵不为空
b3 = boundary(localPlanePoint3(1,:)',localPlanePoint3(2,:)');
S3 = polyarea(localPlanePoint3(1,b3)',localPlanePoint3(2,b3)');
fill3(PlanePoint3(b3,1)',PlanePoint3(b3,2)',PlanePoint3(b3,3)',0.5);
end
if(numel(localPlanePoint4)~=0) % 如果矩阵不为空
b4 = boundary(localPlanePoint4(1,:)',localPlanePoint4(2,:)');
S4 = polyarea(localPlanePoint4(1,b4)',localPlanePoint4(2,b4)');
fill3(PlanePoint4(b4,1)',PlanePoint4(b4,2)',PlanePoint4(b4,3)',0.5);
end
if(numel(localPlanePoint5)~=0) % 如果矩阵不为空
b5 = boundary(localPlanePoint5(1,:)',localPlanePoint5(2,:)');
S5 = polyarea(localPlanePoint5(1,b5)',localPlanePoint5(2,b5)');
fill3(PlanePoint5(b5,1)',PlanePoint5(b5,2)',PlanePoint5(b5,3)',0.5);
end
if(numel(localPlanePoint6)~=0) % 如果矩阵不为空
b6 = boundary(localPlanePoint6(1,:)',localPlanePoint6(2,:)');
S6 = polyarea(localPlanePoint6(1,b6)',localPlanePoint6(2,b6)');
fill3(PlanePoint6(b6,1)',PlanePoint6(b6,2)',PlanePoint6(b6,3)',0.5);
end
S = S1+S2+S3+S4+S5+S6;
fprintf('物体被测量面积为：%d',S);
% 绘图-----------------------------------------------------------------
scatter3(InitialPoint(:,1),InitialPoint(:,2),InitialPoint(:,3),'r','.');
% 返回已经打到物体上的射线----------------------------------------------
output = LiDAR.Flag;
end
函数：屋子墙壁（背景）与射线交点及其面积计算——Environment_Driver_Room_Line
背景也可以看做一个很大的立方体，只不过激光发射源在他的内部。
该函数和计算立方体交点函数的内容基本一致，但是在最终图像显示的时候，由于将六个面的覆盖范围都使用fill3填充会导致看不到里面物体的覆盖情况，故将该函数单独列出，以调整哪些平面不显示在最终结果中
% 本函数用来计算射线和屋子的特定表面交点
function output = Environment_Driver_Room_Line(LiDAR,cube)
% 初始化---------------------------------------------------------------
InitialPoint = zeros(5000,3); % 初始化矩阵，用来存放所有交点
PlanePoint1 = zeros(1,3);
PlanePoint2 = zeros(1,3);
PlanePoint3 = zeros(1,3);
PlanePoint4 = zeros(1,3);
PlanePoint5 = zeros(1,3);
PlanePoint6 = zeros(1,3);
hold on;
% 遍历所有激光射线------------------------------------------------------
for i=1:1:size(LiDAR.Directions,1)
if(LiDAR.Flag(i) == 0) % 该射线已经打到别的物体上，则不考虑该射线
continue;
end
laserDistance = 100000; % 只留表面点
savePoint = [0 0 0];
saveFlag = 0;
planeFlag = 0;
% 当前激光束打在六个平面上的点的坐标---------------------------------
for j=1:1:6
% 防止射线从相反的方向与平面相交---------------------------------
% refLine = LiDAR.Directions(i,4:6) - cube.centerPoint(j,:) ; % 从雷达原点到平面中点的向量
% sigma = acos(dot(refLine,LiDAR.Directions(i,1:3))/(norm(refLine)*norm(LiDAR.Directions(i,1:3))));
% if((sigma*180/pi)>=90)
% continue;
% end
% 计算与该平面的交点--------------------------------------------
n1 = abs(cube.plane(j,1:3)*LiDAR.Directions(i,1:3)');
n2 = abs(cube.plane(j,1:3)*LiDAR.Directions(i,4:6)'+cube.plane(j,4));
n = n1/n2;
if(n==0)
continue;
end
localPoint = LiDAR.Directions(i,4:6)+LiDAR.Directions(i,1:3)/n;
% 判断交点是否在平面内------------------------------------------
% 获取平面的长和宽，求该点和四条边的距离。该距离必须小于长和宽----
L1 = int32(norm(cube.line(j,1:3) - cube.line(j,4:6)));
L2 = int32(norm(cube.line(j,4:6) - cube.line(j,7:9)));
d1 = norm(cross(localPoint-cube.line(j,1:3),localPoint-cube.line(j,4:6)))/norm(cube.line(j,4:6)-cube.line(j,1:3));
d2 = norm(cross(localPoint-cube.line(j,4:6),localPoint-cube.line(j,7:9)))/norm(cube.line(j,4:6)-cube.line(j,7:9));
d3 = norm(cross(localPoint-cube.line(j,7:9),localPoint-cube.line(j,10:12)))/norm(cube.line(j,7:9)-cube.line(j,10:12));
d4 = norm(cross(localPoint-cube.line(j,10:12),localPoint-cube.line(j,1:3)))/norm(cube.line(j,10:12)-cube.line(j,1:3));
if(d1>L2||d3>L2||d2>L1||d4>L1)
continue;
end
% 判断点是否在平面上（不判断有时候会出BUG）----------------------
if(abs(cube.plane(j,1:3)*localPoint'+cube.plane(j,4))>5)
continue;
end
% 一条线与立方体相交会有两个点，只保留距离近的表面点--------------
localPointDistance = norm(localPoint-LiDAR.Directions(i,4:6));
if(localPointDistance<laserDistance)
laserDistance = localPointDistance;
savePoint = localPoint;
saveFlag = 1;
planeFlag = j;
end
% 调试用-------------------------------------------------------
% scatter3((LiDAR.Location(1)+LiDAR.Directions(i,1)),(LiDAR.Location(2)+LiDAR.Directions(i,2)),(LiDAR.Location(3)+LiDAR.Directions(i,3)),'.');
% scatter3(localPoint(1),localPoint(2),localPoint(3),'.');
end
% 保存所有满足要求的点到矩阵中---------------------------------------
if(saveFlag == 1)
InitialPoint = [InitialPoint;savePoint];
LiDAR.Flag(i) = 0;
% 按平面保存到不同的矩阵中，用于计算面积
switch(planeFlag)
case 1
PlanePoint1 = [PlanePoint1;savePoint];
case 2
PlanePoint2 = [PlanePoint2;savePoint];
case 3
PlanePoint3 = [PlanePoint3;savePoint];
case 4
PlanePoint4 = [PlanePoint4;savePoint];
case 5
PlanePoint5 = [PlanePoint5;savePoint];
case 6
PlanePoint6 = [PlanePoint6;savePoint];
end
end
end
PlanePoint1(1,:) = []; % 初始化时第一排是0，删除
PlanePoint2(1,:) = []; % 初始化时第一排是0，删除
PlanePoint3(1,:) = []; % 初始化时第一排是0，删除
PlanePoint4(1,:) = []; % 初始化时第一排是0，删除
PlanePoint5(1,:) = []; % 初始化时第一排是0，删除
PlanePoint6(1,:) = []; % 初始化时第一排是0，删除
cube.MF1(1:3,3) = cube.plane(1,1:3)';% 坐标变换矩阵，
cube.MF2(1:3,3) = cube.plane(2,1:3)';% 坐标变换矩阵
cube.MF3(1:3,3) = cube.plane(3,1:3)';% 坐标变换矩阵
cube.MF4(1:3,3) = cube.plane(4,1:3)';% 坐标变换矩阵
cube.MF5(1:3,3) = cube.plane(5,1:3)';% 坐标变换矩阵
cube.MF6(1:3,3) = cube.plane(6,1:3)';% 坐标变换矩阵
localPlanePoint1=inv(cube.MF1(1:3,1:3))*PlanePoint1'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint2=inv(cube.MF2(1:3,1:3))*PlanePoint2'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint3=inv(cube.MF3(1:3,1:3))*PlanePoint3'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint4=inv(cube.MF4(1:3,1:3))*PlanePoint4'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint5=inv(cube.MF5(1:3,1:3))*PlanePoint5'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
localPlanePoint6=inv(cube.MF6(1:3,1:3))*PlanePoint6'+cube.MF1(1:3,4);% 将每个平面上的三维点变换到一个平面上
S1=0;S2=0;S3=0;S4=0;S5=0;S6=0;
% 1：右墙面 2：房顶 3:前墙面 4：地面 5:左墙面 6：后墙面
if(numel(localPlanePoint1)~=0) % 如果矩阵不为空
b1 = boundary(localPlanePoint1(1,:)',localPlanePoint1(2,:)',1);
S1 = polyarea(localPlanePoint1(1,b1)',localPlanePoint1(2,b1)');
fill3(PlanePoint1(b1,1)',PlanePoint1(b1,2)',PlanePoint1(b1,3)',0.5);
end
if(numel(localPlanePoint2)~=0) % 如果矩阵不为空
b2 = boundary(localPlanePoint2(1,:)',localPlanePoint2(2,:)',1);
S2 = polyarea(localPlanePoint2(1,b2)',localPlanePoint2(2,b2)');
% fill3(PlanePoint2(b2,1)',PlanePoint2(b2,2)',PlanePoint2(b2,3)',0.5);
end
if(numel(localPlanePoint3)~=0) % 如果矩阵不为空
b3 = boundary(localPlanePoint3(1,:)',localPlanePoint3(2,:)',1);
S3 = polyarea(localPlanePoint3(1,b3)',localPlanePoint3(2,b3)');
fill3(PlanePoint3(b3,1)',PlanePoint3(b3,2)',PlanePoint3(b3,3)',0.5);
end
if(numel(localPlanePoint4)~=0) % 如果矩阵不为空
b4 = boundary(localPlanePoint4(1,:)',localPlanePoint4(2,:)',1);
S4 = polyarea(localPlanePoint4(1,b4)',localPlanePoint4(2,b4)');
fill3(PlanePoint4(b4,1)',PlanePoint4(b4,2)',PlanePoint4(b4,3)',0.5);
end
if(numel(localPlanePoint5)~=0) % 如果矩阵不为空
b5 = boundary(localPlanePoint5(1,:)',localPlanePoint5(2,:)',1);
S5 = polyarea(localPlanePoint5(1,b5)',localPlanePoint5(2,b5)');
% fill3(PlanePoint5(b5,1)',PlanePoint5(b5,2)',PlanePoint5(b5,3)',0.5);
end
if(numel(localPlanePoint6)~=0) % 如果矩阵不为空
b6 = boundary(localPlanePoint6(1,:)',localPlanePoint6(2,:)',1);
S6 = polyarea(localPlanePoint6(1,b6)',localPlanePoint6(2,b6)');
% fill3(PlanePoint6(b6,1)',PlanePoint6(b6,2)',PlanePoint6(b6,3)',0.5);
end
S = S1+S2+S3+S4+S5+S6;
fprintf('屋子面积为：%d\r',S);
% 绘图-----------------------------------------------------------------
scatter3(PlanePoint1(:,1),PlanePoint1(:,2),PlanePoint1(:,3),'r','.');
% scatter3(PlanePoint2(:,1),PlanePoint2(:,2),PlanePoint2(:,3),'r','.');
scatter3(PlanePoint3(:,1),PlanePoint3(:,2),PlanePoint3(:,3),'r','.');
scatter3(PlanePoint4(:,1),PlanePoint4(:,2),PlanePoint4(:,3),'r','.');
% scatter3(PlanePoint5(:,1),PlanePoint5(:,2),PlanePoint5(:,3),'r','.');
% scatter3(PlanePoint6(:,1),PlanePoint6(:,2),PlanePoint6(:,3),'r','.');
% 返回已经打到物体上的射线----------------------------------------------
output = LiDAR.Flag;
end
函数：旋转矩阵——Base_RotateMatrix
% RPY旋转矩阵
function RA = Base_RotateMatrix(a,b,c)
RA(1,1) = cosd(a)*cosd(b);
RA(2,1) = cosd(a)*sind(b);
RA(3,1) = -sind(a);
RA(1,2) = sind(c)*sind(a)*cosd(b)-cosd(c)*sind(b);
RA(2,2) = sind(c)*sind(a)*sind(b)+cosd(c)*cosd(b);
RA(3,2) = sind(c)*cosd(a);
RA(1,3) = cosd(c)*sind(a)*cosd(b)+sind(c)*sind(b);
RA(2,3) = cosd(c)*sind(a)*sind(b)-sind(c)*cosd(b);
RA(3,3) = cosd(c)*cosd(a);
end
听说点赞的兄弟找Bug一找一个准 O.o~
925
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
