57 {
58
59 int steps = 5;
60 double stepLen = 1.0;
61 int boundArea = 50;
62
63 double maxSensorRangeLandmarks = 2.5 * stepLen;
64
65 int landMarksPerSquareMeter = 1;
66 double observationProb = 0.8;
67
68 int landmarksRange = 2;
69
70 Vector2d transNoise(0.05, 0.01);
72 Vector2d landmarkNoise(0.05, 0.05);
73
74 Vector2d bound(boundArea, boundArea);
75
76 VectorXd probLimits;
77 probLimits.resize(MO_NUM_ELEMS);
78 for (int i = 0; i < probLimits.size(); ++i)
79 probLimits[i] = (i + 1) / (double)MO_NUM_ELEMS;
80
81 Matrix3d covariance;
82 covariance.fill(0.);
83 covariance(0, 0) = transNoise[0] * transNoise[0];
84 covariance(1, 1) = transNoise[1] * transNoise[1];
85 covariance(2, 2) = rotNoise * rotNoise;
86 Matrix3d information = covariance.inverse();
87
88 SE2 maxStepTransf(stepLen * steps, 0, 0);
93 Simulator::GridPose firstPose;
94 firstPose.id = 0;
95 firstPose.truePose = SE2(0, 0, 0);
96 firstPose.simulatorPose = SE2(0, 0, 0);
97 poses.push_back(firstPose);
98 cerr << "Simulator: sampling nodes ...";
99 while ((
int)
poses.size() < numNodes) {
100
101 for (
int i = 1; i < steps && (int)
poses.size() < numNodes; ++i) {
103 poses.back(), SE2(stepLen, 0, 0), transNoise, rotNoise);
104 poses.push_back(nextGridPose);
105 }
106 if ((
int)
poses.size() == numNodes)
break;
107
108
110 int motionDirection = 0;
111 while (probLimits[motionDirection] < sampleMove &&
112 motionDirection + 1 < MO_NUM_ELEMS) {
113 motionDirection++;
114 }
115
116 SE2 nextMotionStep =
getMotion(motionDirection, stepLen);
117 Simulator::GridPose nextGridPose =
119
120
121 SE2 nextStepFinalPose = nextGridPose.truePose * maxStepTransf;
122 if (fabs(nextStepFinalPose.translation().x()) >= bound[0] ||
123 fabs(nextStepFinalPose.translation().y()) >= bound[1]) {
124
125
126 for (int i = 0; i < MO_NUM_ELEMS; ++i) {
128 nextGridPose =
130 nextStepFinalPose = nextGridPose.truePose * maxStepTransf;
131 if (fabs(nextStepFinalPose.translation().x()) < bound[0] &&
132 fabs(nextStepFinalPose.translation().y()) < bound[1])
133 break;
134 }
135 }
136
137 poses.push_back(nextGridPose);
138 }
139 cerr << "done." << endl;
140
141
142 cerr << "Simulator: Creating landmarks ... ";
144 for (PosesVector::const_iterator it =
poses.begin(); it !=
poses.end();
145 ++it) {
146 int ccx = (int)round(it->truePose.translation().x());
147 int ccy = (int)round(it->truePose.translation().y());
148 for (int a = -landmarksRange; a <= landmarksRange; a++)
149 for (int b = -landmarksRange; b <= landmarksRange; b++) {
150 int cx = ccx + a;
151 int cy = ccy + b;
153 if (landmarksForCell.size() == 0) {
154 for (int i = 0; i < landMarksPerSquareMeter; ++i) {
155 Landmark* l = new Landmark();
156 double offx, offy;
157 do {
160 }
while (
hypot_sqr(offx, offy) < 0.25 * 0.25);
161 l->truePose[0] = cx + offx;
162 l->truePose[1] = cy + offy;
163 landmarksForCell.push_back(l);
164 }
165 }
166 }
167 }
168 cerr << "done." << endl;
169
170 cerr << "Simulator: Simulating landmark observations for the poses ... ";
171 double maxSensorSqr = maxSensorRangeLandmarks * maxSensorRangeLandmarks;
172 int globalId = 0;
173 for (PosesVector::iterator it =
poses.begin(); it !=
poses.end(); ++it) {
174 Simulator::GridPose& pv = *it;
175 int cx = (int)round(it->truePose.translation().x());
176 int cy = (int)round(it->truePose.translation().y());
177 int numGridCells = (int)(maxSensorRangeLandmarks) + 1;
178
179 pv.id = globalId++;
180 SE2 trueInv = pv.truePose.inverse();
181
182 for (int xx = cx - numGridCells; xx <= cx + numGridCells; ++xx)
183 for (int yy = cy - numGridCells; yy <= cy + numGridCells; ++yy) {
185 if (landmarksForCell.size() == 0) continue;
186 for (size_t i = 0; i < landmarksForCell.size(); ++i) {
187 Landmark* l = landmarksForCell[i];
188 double dSqr =
189 hypot_sqr(pv.truePose.translation().x() - l->truePose.x(),
190 pv.truePose.translation().y() - l->truePose.y());
191 if (dSqr > maxSensorSqr) continue;
193 if (obs > observationProb)
194 continue;
195 if (l->id < 0) l->id = globalId++;
196 if (l->seenBy.size() == 0) {
197 Vector2d trueObservation = trueInv * l->truePose;
198 Vector2d observation = trueObservation;
201 l->simulatedPose = pv.simulatorPose * observation;
202 }
203 l->seenBy.push_back(pv.id);
204 pv.landmarks.push_back(l);
205 }
206 }
207 }
208 cerr << "done." << endl;
209
210
212 cerr << "Simulator: Adding odometry measurements ... ";
213 for (
size_t i = 1; i <
poses.size(); ++i) {
214 const GridPose& prev =
poses[i - 1];
215 const GridPose& p =
poses[i];
216
219
220 edge.from = prev.id;
221 edge.to = p.id;
222 edge.trueTransf = prev.truePose.inverse() * p.truePose;
223 edge.simulatorTransf = prev.simulatorPose.inverse() * p.simulatorPose;
224 edge.information = information;
225 }
226 cerr << "done." << endl;
227
230
231 {
232 cerr << "Simulator: add landmark observations ... ";
233 Matrix2d covariance;
234 covariance.fill(0.);
235 covariance(0, 0) = landmarkNoise[0] * landmarkNoise[0];
236 covariance(1, 1) = landmarkNoise[1] * landmarkNoise[1];
237 Matrix2d information = covariance.inverse();
238
239 for (
size_t i = 0; i <
poses.size(); ++i) {
240 const GridPose& p =
poses[i];
241 for (size_t j = 0; j < p.landmarks.size(); ++j) {
242 Landmark* l = p.landmarks[j];
243 if (l->seenBy.size() > 0 && l->seenBy[0] == p.id) {
245 }
246 }
247 }
248
249 for (
size_t i = 0; i <
poses.size(); ++i) {
250 const GridPose& p =
poses[i];
251 SE2 trueInv = (p.truePose * sensorOffset).inverse();
252 for (size_t j = 0; j < p.landmarks.size(); ++j) {
253 Landmark* l = p.landmarks[j];
254 Vector2d observation;
255 Vector2d trueObservation = trueInv * l->truePose;
256 observation = trueObservation;
257 if (l->seenBy.size() > 0 &&
258 l->seenBy[0] ==
259 p.id) {
260 observation =
261 (p.simulatorPose * sensorOffset).inverse() * l->simulatedPose;
262 } else {
263
266 }
267
270
271 le.from = p.id;
272 le.to = l->id;
273 le.trueMeas = trueObservation;
274 le.simulatorMeas = observation;
275 le.information = information;
276 }
277 }
278 cerr << "done." << endl;
279 }
280
281
282 for (LandmarkGrid::iterator it = grid.begin(); it != grid.end(); ++it) {
283 for (std::map<int, Simulator::LandmarkPtrVector>::iterator itt =
284 it->second.begin();
285 itt != it->second.end(); ++itt) {
288 }
289 }
290}
static double uniformRand(double lowerBndr, double upperBndr)
const PosesVector & poses() const
GridPose generateNewPose(const GridPose &prev, const SE2 &trueMotion, const Eigen::Vector2d &transNoise, double rotNoise)
std::vector< Landmark * > LandmarkPtrVector
std::vector< GridPose > PosesVector
const LandmarkVector & landmarks() const
std::vector< Landmark > LandmarkVector
SE2 getMotion(int motionDirection, double stepLen)
std::map< int, std::map< int, Simulator::LandmarkPtrVector > > LandmarkGrid
