-
Notifications
You must be signed in to change notification settings - Fork 6
/
slam_sim2d.m
244 lines (183 loc) · 7.38 KB
/
slam_sim2d.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RFM-SLAM
% Copyright 2017
% Author: Saurav Agarwal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function slam_sim2d(runNumber, inpDat, odoNL, rbNL, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2D simulation for given environment
% Run robot in map through waypoints,
% write the ground truth, data to files.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Total Path Length = %f m \n',pathLength(inpDat.wp));
%% Setup parameters for simulation
global param;
param.doPLOT = 0;
param.plotFrequency = 5;
param.storeFrequency = 1; % We need this to be 1, because all sensed heading values (each pose) are required for GTSAM comparison
% Set sim parameters
param.odoNoiseLevelScale = odoNL; % [1,2,3,4] scale the motion odometery noises with this value
param.rbNoiseLevelScale = rbNL; % [1,2,3,4] scale the range bearing noises with this value
param.numSimLoops = 1; % number of loops robot will drive around trajectory
param.robot.maxVelocity = 10.0;
% param.numSamples = 10;
param.simulator.dt = 0.05;
param.knownOrientation = 1; % if 1, heading is known if 0 heading is not known
% param.headingUpdateInterval = 1; % time interval between heading updates
% param.maxObservations = 1000;
% param.minObservations = 50;
param.world.landmarks = struct();
% Put robot at initial point and set belief
param.robot.initialbelief.mean = zeros(3,1);
param.robot.initialbelief.covariance = diag([(0.01)^2, (0.01)^2, (0.05*pi/180)^2]);
% Create Structs to store data for GTSAM
odoData = struct('odo',[],'robotPose',[],'odoError',[]);
% odoData srores the odmetry data, its covariance matrix and robot expected
% poses based upon only odometry
obsData = struct();
% obsData stores the range and bearing data for landmarks with their IDs.
lmID = []; % It stores the full array of all the feature which were observed with their IDs.
% Put features into the world
for nf = 1:size(inpDat.lm,2)
param.world.landmarkIDs(nf) = nf;
param.world.landmarkPoses(:,nf) = inpDat.lm(:,nf);
end
%% Plotting setup
if param.doPLOT
param.animationPlot = figure;
plot(inpDat.lm(1,:),inpDat.lm(2,:),'b*')
hold on; axis equal; axis tight
plot(inpDat.wp(1,:),inpDat.wp(2,:), 'gs', inpDat.wp(1,:),inpDat.wp(2,:),'g.'); % plot waypoints
xlabel('X (m)'); ylabel('Y (m)');
set(param.animationPlot, 'name', 'PLUM-EKF Hybrid SLAM Simulator','WindowStyle', 'docked')
end
%% Initialize objects
% Initialize map
map = TwoDMap(varargin{1});
% If you have known features, insert them into the map
% map.mappedFeatureIDs = [param.world.landmarkIDs(1) param.world.landmarkIDs(20)];
% map.mappedFeaturePoses = [param.world.landmarkPoses(:,1) param.world.landmarkPoses(:,20)];
% create data association method
dataAssociationMethod = KnownDataAssociation();
% create sensors / obs models
orientationSensor = YawHeadingSensorModel();
obsModel = TwoDRangeBearingModel(map, param.world.landmarkIDs, param.world.landmarkPoses) ; % Init obs model
% create robot model
motionModel = TwoDBicycleRobot(); % Init motion model
bot = Robot(param.robot.initialbelief.mean, obsModel, motionModel); % Give sensor and motion model to robot
% create simulator, might be unecessary at this stage, helps in separation
sim = Simulator(bot); % Give sim the robot
%% Run looop
% filterNEES = [];
iwp = 1;
u = [0;0];
t = 0;
groundTruth = []; % to store actual robot traj
headingObs = [];
graphNodes = struct('idfs',[]);% store the ids seen at this node.
while iwp ~= 0
if t == 0
u = zeros(2,1);
else
u(1) = param.robot.maxVelocity;
[u(2),iwp] = motionModel.computeSteering( bot.state, inpDat.wp, iwp, u(2));
if iwp==0 && param.numSimLoops > 1, iwp=1; param.numSimLoops= param.numSimLoops-1; end % perform loops: if final waypoint reached, go back to first
end
bot = sim.moveRobot(u); % move robot
% add to ground truth
groundTruth = [groundTruth, bot.state];
% get the odometry readings and updated odometeric estimate, not that
% odometery update has to be based on previously esimated odometery
% state.
if t == 0
[odoVal, odoCov, odo_x_next] = sim.getOdometery(param.robot.initialbelief.mean, u);
odoData.robotPose(:,t+1) = param.robot.initialbelief.mean; % at t=0, we give the known data
else
[odoVal, odoCov, odo_x_next] = sim.getOdometery(odoData.robotPose(:,t), u);
odoData.odoError(:,:,t) = odoCov; % storing the odometry covariance or GTSAM
% pose by odometry data
odoData.odo(:,t) = odoVal;
odoData.robotPose(:,t+1) = odo_x_next;
end
[z, idfs] = sim.getObservation(); % get observation
if runNumber ==1
graphNodes(t+1).idfs = idfs;
end
%There are no duplicates (if I see id 20, it will put value 20 in position 20).
lmID(idfs) = idfs; % Need this for post processing analysis
% Storing the Observation in structure
obsData(t+1).rb = [idfs;z];
zOrientation = orientationSensor.getObservation(bot.state);
headingObs = [headingObs, zOrientation];
%fprintf('Time Step = %u \n', t);
if mod(t,param.plotFrequency) == 0 && param.doPLOT
motionModel.draw(bot.state,[0;0;0]);
obsModel.draw(bot.state, z);
drawnow;
end
t = t+1; % t needs to be incremented before exiting
end
%% File Handling
check1 = writedata2graph(t,param.knownOrientation,odoData,obsData,bot.observationModel.sigma_b,orientationSensor.sigma_b,headingObs,varargin{1});
if check1
fprintf('Graph write operation Successful \n');
else
fprintf('Graph write Operation Failed \n');
end
% You don't want to write ground truth and compute edges for each run
% So you can replace nan with 1 to create an output otherwise
% leave it at nan
if runNumber == 1
% Only build edges when orientation is not known
if param.knownOrientation == 0
gEdges = buildEdges(graphNodes);
else
gEdges = [];
end
% By doing 'find' operation, we remove the zeros. The values are 0 where
% the landmarks for that ID were not seen.
check2 = writegroundtruth2file([varargin{1},'../'], groundTruth, lmID(find(lmID(:))), param.world.landmarkPoses, gEdges);
if check2
fprintf('Ground truth write operation Successful \n');
else
fprintf('Ground truth Write Operation Failed \n');
end
end
end
function d = distance(p,q)
d = sqrt(sum((p-q).^2));
end
function L = pathLength(wp)
L = 0;
for i = 1:size(wp,2)-1
L = L + distance(wp(:,i),wp(:,i+1));
end
end
function gEdges = buildEdges(graphNodes)
% This function generates the graph edges
% Very useful to speed up analysis
fprintf('Start building data association edges...\n');
% get number of nodes.
N = length(graphNodes);
% get possible combinations
possibleEdges = nchoosek(1:N,2);
gEdges = [(1:N-1)' (2:N)']; %odometery edges
for e = 1:size(possibleEdges,1)
fromNode = possibleEdges(e,1);
toNode = possibleEdges(e,2);
if abs(fromNode-toNode) < 2
continue; % ignore odometery edges
end
zFrom = graphNodes(fromNode).idfs;
zTo = graphNodes(toNode).idfs;
if size(zFrom,2) < 2 || size(zTo,2) < 2
continue; % have to both see atleast 2 to have a constraint
end
[zints,~,~] = intersect(zFrom, zTo);
% if less than 2 common
if size(zints,2) < 2
continue;
end
gEdges(end+1,:) = [fromNode, toNode];
end
end