✅作者简介:热爱科研的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 % 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 月。 ❤️部分理论引用网络文献,若有侵权联系博主删除 ❤️ 关注我领取海量matlab电子书和数学建模资料⛄ 运行结果
⛄ 参考文献
⛄ Matlab代码关注