✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
主要步骤是：
1) 生成建筑物遮罩：应用高度阈值将点云分为地面点和非地面点。这两组点用于生成两个建筑物遮罩，即主要和次要建筑物遮罩。
2) 线提取：从主建筑物遮罩中提取建筑物周围的线。
3) 形成初始/候选建筑物并扩展建筑物：使用提取的线形成初始建筑物，然后（取决于技术）使用来自多光谱图像的 NDVI 和/或熵来扩展候选建筑物。在扩展期间也使用辅助掩码。
4) 移除树木：应用一套规则来移除与建筑物一样高的树木，也可能具有类似树的形状和颜色。
⛄ 部分代码
function ekfSlam
%warming('off');
close all; clear all;
% global stuff
global xVehicleTrue;
global xVehicleEst;
global P;
global landFeatures;
global visionSensorSettings;
global xOdomLast;
global nSteps;
global uTrue;
global B;
global laserSensorSettings
% robot settings
B =1; % axis distance
% sensor settings
laserSensorSettings.maxBearing =25;
laserSensorSettings.maxRange = 100;
laserSensorSettings.deltaBearing =1;
% simulation parameters
nSteps =250;
worldSize =200;
nCornersLandFeatures =4;
cornersLandFeatures = zeros(2,1,nCornersLandFeatures); % make some space
cornersLandFeatures(:,:,1) = [0 0]';
cornersLandFeatures(:,:,2) = [0 120]';
cornersLandFeatures(:,:,3) = [120 120]';
cornersLandFeatures(:,:,4) = [120 0]';
wallPerimeter(:,:) =[ cornersLandFeatures(1,1,1) cornersLandFeatures(1,1,2) cornersLandFeatures(2,1,1) cornersLandFeatures(2,1,2);
cornersLandFeatures(1,1,2) cornersLandFeatures(1,1,3) cornersLandFeatures(2,1,2) cornersLandFeatures(2,1,3);
cornersLandFeatures(1,1,3) cornersLandFeatures(1,1,4) cornersLandFeatures(2,1,3) cornersLandFeatures(2,1,4);
cornersLandFeatures(1,1,4) cornersLandFeatures(1,1,1) cornersLandFeatures(2,1,4) cornersLandFeatures(2,1,1)];
display('Setup Complete');
figure(1); hold on; %grid off; axis off;
xWall = [wallPerimeter(:,1), wallPerimeter(:,2)];
yWall = [wallPerimeter(:,3), wallPerimeter(:,4)];
plot (cornersLandFeatures(1,:), cornersLandFeatures(2,:), 'r+'); hold on;
plot(xWall, yWall, '-b'); hold on;
axis([-worldSize/2 worldSize/2 -worldSize/2 worldSize/2]);
axis([-worldSize worldSize -worldSize worldSize]);
xVehicleTrue =[ 40 60 0]';
% filter states
xTrue=[xVehicleTrue];
xEst =[xVehicleTrue];
P = diag([3, 3 ,0.08]);
% Standard deviation error regarding observation
RTrue = diag([0.1, 0.5*pi/180]).^2;
REst = 2*RTrue;
% control input movement
deltaS =[0.28 0.30];
kr=0.0001;
% Gate value for measure acceptacnce
gateAccept =10;
F(nSteps) = struct('cdata',[],'colormap',[]);
% loop
for k =1:nSteps
% get true states
xVehicleTrue = xTrue;
xMapTrue =cornersLandFeatures;
% get estimated states
xVehicleEst = xEst(1:3); % get estimated state vehicle
xMapEst = xEst(4:end); % rest are the mapped corners estimated
% generate controls true value
u = getControl(xVehicleTrue,deltaS,0,0);
xVehicleTrue = moveRobot(xVehicleTrue,u);
% generate controls estimated value value
q = generateNoise(-0.005, 0.005, [3,1]); % noise
u = getControl(xVehicleEst,deltaS, 1, q);
xVehicleEst = moveRobot(xVehicleEst,u);
ds = (deltaS(1) + deltaS(2))/2;
Q = diag([kr*abs(deltaS(1)), kr*abs(deltaS(2))]);
%compute covariances
P_vv = J1(u)*P(1:3,1:3)*J1(u)' + J2(xVehicleEst,u,ds)*Q*J2(xVehicleEst,u,ds)';
P_vc = J1(u)*P(1:3,4:end); % vehicle to corners
P_cc = P(4:end,4:end); % corners to corners
%agragate strue
xTrue = xVehicleTrue;
% agregate estimated state again
xEst = [xVehicleEst; xMapEst];
P = [P_vv P_vc;
P_vc' P_cc];
% get observations
obsSigma2 = RTrue(2);
[obsTh, obsRho] =readLidar(xVehicleTrue,wallPerimeter,obsSigma2);
% extract the corners split merge
[zFeat] = getCornerFeatures(xVehicleEst,obsTh, obsRho, 9, 2);
xCorner=[];
yCorner=[];
z=[];
if(~isempty(zFeat))
[xCorner, yCorner] = transformPointToWorld(xVehicleEst, zFeat(2), zFeat(1), 0);
z =zFeat;
end
newObs=0;
% if valid observations is found
if(~isempty(z))
R=diag([1,0.1]);
gateLastAccept=gateAccept;
associateIDX =-1;
% get number feature in track
[nFeatures, c] = size(xMapEst);
% loop all features in track
nFeatures =nFeatures/2;
for f=1: nFeatures
% extract the feature value
xFeature = xMapEst(f:f+1);
%zEst= doObservationModel(xVehicleEst,xFeature);
% check if is associated
gateVal =nearestNeighbour(f,xEst,xVehicleEst,xFeature, z, P, R);
%keyboard
if(gateVal<gateLastAccept)
% valid association
associateIDX = f;
gateLastAccept =gateVal;
disp('Observation in gate with Mapped Feature ');
end
end %for
if(associateIDX >-1) % valid association found, update
xFeature = xMapEst(associateIDX:associateIDX+1);
zEst= doObservationModel(xVehicleEst,xFeature);
% get obs jacobians
[jHxv, jHxf] = getObsJacs(xVehicleEst, xFeature);
jH = zeros(2,length(xEst));
jH(:, associateIDX:associateIDX+1) = jHxf;
jH(:,1:3) = jHxv;
% do kalman update
INNOV = z-zEst;
INNOV(2) = angleWrapping(INNOV(2));
% compute covariance innovation
S=jH*P*jH' + REst;
% compute kalman gain
W = P*jH' * inv(S);
% update state
xEst = xEst + W*INNOV;
% update Covariance
P = P - W*S*W';
% ensure remains symmetric
P = 0.5*(P+P');
disp('Mapped feature associated with Observation');
else % not associated, add new
n = length(xEst);
% copmpute preditect position based on estimated state and
% sensor readings
xFeature = xVehicleEst(1:2) + [z(1)*cos(angleWrapping(z(2) + xVehicleEst(3))); z(1)*sin(angleWrapping(z(2) + xVehicleEst(3)))];
% add to state vector
xEst =[xEst; xFeature];
% get the new jacobian feature
[jGxv, jGz] = getNewFeatureJacs(xVehicleEst,zFeat);
M =[eye(n), zeros(n,2);
jGxv zeros(2,n-3), jGz];
P = M*blkdiag(P,REst)*M';
disp('New feature Added');
end
end
% Ploting zone
a = axis;
clf;
axis(a); hold on;
nStates = length(xEst); % get the total length
nLandMarks = (nStates-3)/2; % number landmarks in track
% plot world
plot (cornersLandFeatures(1,:), cornersLandFeatures(2,:), 'r+'); hold on;
plot(xWall, yWall, '-b'); hold on;
% plot vehicle
doVehicleGraphics(xEst(1:3), P(1:3,1:3), 3, [pi/2 0]);
% plot laser
plotPointCloud(xVehicleEst, obsTh, obsRho, 0); hold on;
% plot calculated corner
plot (xCorner, yCorner, 'g+','MarkerSize',10); hold on
%plot the projected line to the corner
if(~isempty(z))
h = line([xEst(1), xCorner], [xEst(2), yCorner]);
set(h, 'linestyle',':');
legend(sprintf('Estimated Range: %3.2f; Angle: %3.2f',z(1), z(2)*180/pi));
end
% plot landmarks
for(ii = 1:nLandMarks)
iF= 3+2*ii-1;
plot(xEst(iF), xEst(iF+1), 'b*');hold on;
plotEllipse(xEst(iF:iF+1), P(iF:iF+1,iF:iF+1),3);hold on;
end;
drawnow;
F(k) = getframe(gcf);
%save stuff
%xTrueSaved(k,:) = xTrue;
%xEstSaved(k,:) = xEst;
% iteraction
disp(sprintf('Interaction %d' ,k));
end
movie2avi(F,'ekfLidar.avi','compression','none');
end
⛄ 运行结果
⛄ 参考文献
1. M. Awrangjeb、M. Ravanbakhsh 和 CS Fraser，“Automatic detection of residential buildings using LIDAR data and multispectral imagery”，ISPRS Journal of Photogrammetry and Remote Sensing，65(5)，457-467， 2010 年 9 月
2. M. Awrangjeb、C. Zhang 和 CS Fraser，“复杂场景中的建筑物检测通过将建筑物与树木有效分离”，摄影测量工程与遥感 (PE&RS)，卷。78(7), 729-745, 2012 年 7 月。
