g2o
Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
g2o::tutorial::Simulator Class Reference

#include <simulator.h>

Classes

struct  GridEdge
 odometry constraint More...
 
struct  GridPose
 
struct  Landmark
 simulated landmark More...
 
struct  LandmarkEdge
 

Public Types

typedef std::vector< Landmark, Eigen::aligned_allocator< Landmark > > LandmarkVector
 
typedef std::vector< Landmark * > LandmarkPtrVector
 
typedef std::vector< GridPose, Eigen::aligned_allocator< GridPose > > PosesVector
 
typedef std::vector< GridEdge, Eigen::aligned_allocator< GridEdge > > GridEdgeVector
 
typedef std::vector< LandmarkEdge, Eigen::aligned_allocator< LandmarkEdge > > LandmarkEdgeVector
 

Public Member Functions

 Simulator ()
 
 ~Simulator ()
 
void simulate (int numPoses, const SE2 &sensorOffset=SE2())
 
const PosesVector & poses () const
 
const LandmarkVector & landmarks () const
 
const GridEdgeVector & odometry () const
 
const LandmarkEdgeVector & landmarkObservations () const
 

Protected Member Functions

GridPose generateNewPose (const GridPose &prev, const SE2 &trueMotion, const Eigen::Vector2d &transNoise, double rotNoise)
 
SE2 getMotion (int motionDirection, double stepLen)
 
SE2 sampleTransformation (const SE2 &trueMotion_, const Eigen::Vector2d &transNoise, double rotNoise)
 

Protected Attributes

PosesVector _poses
 
LandmarkVector _landmarks
 
GridEdgeVector _odometry
 
LandmarkEdgeVector _landmarkObservations
 

Detailed Description

Definition at line 41 of file simulator.h.

Member Typedef Documentation

typedef std::vector<GridEdge, Eigen::aligned_allocator<GridEdge> > g2o::tutorial::Simulator::GridEdgeVector

Definition at line 89 of file simulator.h.

typedef std::vector<LandmarkEdge, Eigen::aligned_allocator<LandmarkEdge> > g2o::tutorial::Simulator::LandmarkEdgeVector

Definition at line 100 of file simulator.h.

typedef std::vector<Landmark*> g2o::tutorial::Simulator::LandmarkPtrVector

Definition at line 62 of file simulator.h.

typedef std::vector<Landmark, Eigen::aligned_allocator<Landmark> > g2o::tutorial::Simulator::LandmarkVector

Definition at line 61 of file simulator.h.

typedef std::vector<GridPose, Eigen::aligned_allocator<GridPose> > g2o::tutorial::Simulator::PosesVector

Definition at line 75 of file simulator.h.

Constructor & Destructor Documentation

Simulator::Simulator ( )

Definition at line 50 of file simulator.cpp.

51  {
52  time_t seed = time(0);
53  Rand::seed_rand(static_cast<unsigned int>(seed));
54  }
g2o::tutorial::Rand::seed_rand
static void seed_rand()
Definition: rand.h:72
Simulator::~Simulator ( )

Definition at line 56 of file simulator.cpp.

57  {
58  }

Member Function Documentation

Simulator::GridPose Simulator::generateNewPose ( const GridPose &  prev,
const SE2 &  trueMotion,
const Eigen::Vector2d &  transNoise,
double  rotNoise 
)
protected

Definition at line 289 of file simulator.cpp.

References g2o::tutorial::Simulator::GridPose::id, g2o::tutorial::Simulator::GridPose::simulatorPose, and g2o::tutorial::Simulator::GridPose::truePose.

290  {
291  Simulator::GridPose nextPose;
292  nextPose.id = prev.id + 1;
293  nextPose.truePose = prev.truePose * trueMotion;
294  SE2 noiseMotion = sampleTransformation(trueMotion, transNoise, rotNoise);
295  nextPose.simulatorPose = prev.simulatorPose * noiseMotion;
296  return nextPose;
297  }
g2o::tutorial::Simulator::sampleTransformation
SE2 sampleTransformation(const SE2 &trueMotion_, const Eigen::Vector2d &transNoise, double rotNoise)
Definition: simulator.cpp:312
SE2 Simulator::getMotion ( int  motionDirection,
double  stepLen 
)
protected

Definition at line 299 of file simulator.cpp.

References M_PI, MO_LEFT, and MO_RIGHT.

300  {
301  switch (motionDirection) {
302  case MO_LEFT:
303  return SE2(stepLen, 0, 0.5*M_PI);
304  case MO_RIGHT:
305  return SE2(stepLen, 0, -0.5*M_PI);
306  default:
307  cerr << "Unknown motion direction" << endl;
308  return SE2(stepLen, 0, -0.5*M_PI);
309  }
310  }
MO_LEFT
MO_LEFT
Definition: simulator.h:45
MO_RIGHT
MO_RIGHT
Definition: simulator.h:45
M_PI
#define M_PI
Definition: misc.h:34
const LandmarkEdgeVector& g2o::tutorial::Simulator::landmarkObservations ( ) const
inline

Definition at line 111 of file simulator.h.

Referenced by main().

111 { return _landmarkObservations;}
g2o::tutorial::Simulator::_landmarkObservations
LandmarkEdgeVector _landmarkObservations
Definition: simulator.h:117
const LandmarkVector& g2o::tutorial::Simulator::landmarks ( ) const
inline

Definition at line 109 of file simulator.h.

Referenced by main().

109 { return _landmarks;}
g2o::tutorial::Simulator::_landmarks
LandmarkVector _landmarks
Definition: simulator.h:115
const GridEdgeVector& g2o::tutorial::Simulator::odometry ( ) const
inline

Definition at line 110 of file simulator.h.

Referenced by main().

110 { return _odometry;}
g2o::tutorial::Simulator::_odometry
GridEdgeVector _odometry
Definition: simulator.h:116
const PosesVector& g2o::tutorial::Simulator::poses ( ) const
inline

Definition at line 108 of file simulator.h.

Referenced by main().

108 { return _poses;}
SE2 Simulator::sampleTransformation ( const SE2 &  trueMotion_,
const Eigen::Vector2d &  transNoise,
double  rotNoise 
)
protected

Definition at line 312 of file simulator.cpp.

References gauss_rand(), and g2o::tutorial::SE2::toVector().

313  {
314  Vector3d trueMotion = trueMotion_.toVector();
315  SE2 noiseMotion(
316  trueMotion[0] + Rand::gauss_rand(0.0, transNoise[0]),
317  trueMotion[1] + Rand::gauss_rand(0.0, transNoise[1]),
318  trueMotion[2] + Rand::gauss_rand(0.0, rotNoise));
319  return noiseMotion;
320  }
g2o::tutorial::Rand::gauss_rand
static double gauss_rand(double mean, double sigma)
Definition: rand.h:50
void Simulator::simulate ( int  numPoses,
const SE2 &  sensorOffset = SE2() 
)

Definition at line 60 of file simulator.cpp.

References DEG2RAD, g2o::tutorial::Simulator::GridEdge::from, g2o::tutorial::Simulator::LandmarkEdge::from, gauss_rand(), g2o::hypot_sqr(), g2o::tutorial::Simulator::Landmark::id, g2o::tutorial::Simulator::GridPose::id, g2o::tutorial::Simulator::GridEdge::information, g2o::tutorial::Simulator::LandmarkEdge::information, g2o::tutorial::SE2::inverse(), g2o::tutorial::Simulator::GridPose::landmarks, g2o::tutorial::Simulator::Landmark::seenBy, g2o::tutorial::Simulator::Landmark::simulatedPose, g2o::tutorial::Simulator::LandmarkEdge::simulatorMeas, g2o::tutorial::Simulator::GridPose::simulatorPose, g2o::tutorial::Simulator::GridEdge::simulatorTransf, g2o::tutorial::Simulator::GridEdge::to, g2o::tutorial::Simulator::LandmarkEdge::to, g2o::tutorial::SE2::translation(), g2o::tutorial::Simulator::LandmarkEdge::trueMeas, g2o::tutorial::Simulator::Landmark::truePose, g2o::tutorial::Simulator::GridPose::truePose, g2o::tutorial::Simulator::GridEdge::trueTransf, and uniform_rand().

Referenced by main().

61  {
62  // simulate a robot observing landmarks while travelling on a grid
63  int steps = 5;
64  double stepLen = 1.0;
65  int boundArea = 50;
66 
67  double maxSensorRangeLandmarks = 2.5 * stepLen;
68 
69  int landMarksPerSquareMeter = 1;
70  double observationProb = 0.8;
71 
72  int landmarksRange=2;
73 
74  Vector2d transNoise(0.05, 0.01);
75  double rotNoise = DEG2RAD(2.);
76  Vector2d landmarkNoise(0.05, 0.05);
77 
78  Vector2d bound(boundArea, boundArea);
79 
80  VectorXd probLimits;
81  probLimits.resize(MO_NUM_ELEMS);
82  for (int i = 0; i < probLimits.size(); ++i)
83  probLimits[i] = (i + 1) / (double) MO_NUM_ELEMS;
84 
85  Matrix3d covariance;
86  covariance.fill(0.);
87  covariance(0, 0) = transNoise[0]*transNoise[0];
88  covariance(1, 1) = transNoise[1]*transNoise[1];
89  covariance(2, 2) = rotNoise*rotNoise;
90  Matrix3d information = covariance.inverse();
91 
92  SE2 maxStepTransf(stepLen * steps, 0, 0);
93  Simulator::PosesVector& poses = _poses;
94  poses.clear();
95  LandmarkVector& landmarks = _landmarks;
96  landmarks.clear();
97  Simulator::GridPose firstPose;
98  firstPose.id = 0;
99  firstPose.truePose = SE2(0,0,0);
100  firstPose.simulatorPose = SE2(0,0,0);
101  poses.push_back(firstPose);
102  cerr << "Simulator: sampling nodes ...";
103  while ((int)poses.size() < numNodes) {
104  // add straight motions
105  for (int i = 1; i < steps && (int)poses.size() < numNodes; ++i) {
106  Simulator::GridPose nextGridPose = generateNewPose(poses.back(), SE2(stepLen,0,0), transNoise, rotNoise);
107  poses.push_back(nextGridPose);
108  }
109  if ((int)poses.size() == numNodes)
110  break;
111 
112  // sample a new motion direction
113  double sampleMove = Rand::uniform_rand(0., 1.);
114  int motionDirection = 0;
115  while (probLimits[motionDirection] < sampleMove && motionDirection+1 < MO_NUM_ELEMS) {
116  motionDirection++;
117  }
118 
119  SE2 nextMotionStep = getMotion(motionDirection, stepLen);
120  Simulator::GridPose nextGridPose = generateNewPose(poses.back(), nextMotionStep, transNoise, rotNoise);
121 
122  // check whether we will walk outside the boundaries in the next iteration
123  SE2 nextStepFinalPose = nextGridPose.truePose * maxStepTransf;
124  if (fabs(nextStepFinalPose.translation().x()) >= bound[0] || fabs(nextStepFinalPose.translation().y()) >= bound[1]) {
125  //cerr << "b";
126  // will be outside boundaries using this
127  for (int i = 0; i < MO_NUM_ELEMS; ++i) {
128  nextMotionStep = getMotion(i, stepLen);
129  nextGridPose = generateNewPose(poses.back(), nextMotionStep, transNoise, rotNoise);
130  nextStepFinalPose = nextGridPose.truePose * maxStepTransf;
131  if (fabs(nextStepFinalPose.translation().x()) < bound[0] && fabs(nextStepFinalPose.translation().y()) < bound[1])
132  break;
133  }
134  }
135 
136  poses.push_back(nextGridPose);
137  }
138  cerr << "done." << endl;
139 
140  // creating landmarks along the trajectory
141  cerr << "Simulator: Creating landmarks ... ";
142  LandmarkGrid grid;
143  for (PosesVector::const_iterator it = poses.begin(); it != poses.end(); ++it) {
144  int ccx = (int)round(it->truePose.translation().x());
145  int ccy = (int)round(it->truePose.translation().y());
146  for (int a=-landmarksRange; a<=landmarksRange; a++)
147  for (int b=-landmarksRange; b<=landmarksRange; b++){
148  int cx=ccx+a;
149  int cy=ccy+b;
150  LandmarkPtrVector& landmarksForCell = grid[cx][cy];
151  if (landmarksForCell.size() == 0) {
152  for (int i = 0; i < landMarksPerSquareMeter; ++i) {
153  Landmark* l = new Landmark();
154  double offx, offy;
155  do {
156  offx = Rand::uniform_rand(-0.5*stepLen, 0.5*stepLen);
157  offy = Rand::uniform_rand(-0.5*stepLen, 0.5*stepLen);
158  } while (hypot_sqr(offx, offy) < 0.25*0.25);
159  l->truePose[0] = cx + offx;
160  l->truePose[1] = cy + offy;
161  landmarksForCell.push_back(l);
162  }
163  }
164  }
165  }
166  cerr << "done." << endl;
167 
168  cerr << "Simulator: Simulating landmark observations for the poses ... ";
169  double maxSensorSqr = maxSensorRangeLandmarks * maxSensorRangeLandmarks;
170  int globalId = 0;
171  for (PosesVector::iterator it = poses.begin(); it != poses.end(); ++it) {
172  Simulator::GridPose& pv = *it;
173  int cx = (int)round(it->truePose.translation().x());
174  int cy = (int)round(it->truePose.translation().y());
175  int numGridCells = (int)(maxSensorRangeLandmarks) + 1;
176 
177  pv.id = globalId++;
178  SE2 trueInv = pv.truePose.inverse();
179 
180  for (int xx = cx - numGridCells; xx <= cx + numGridCells; ++xx)
181  for (int yy = cy - numGridCells; yy <= cy + numGridCells; ++yy) {
182  LandmarkPtrVector& landmarksForCell = grid[xx][yy];
183  if (landmarksForCell.size() == 0)
184  continue;
185  for (size_t i = 0; i < landmarksForCell.size(); ++i) {
186  Landmark* l = landmarksForCell[i];
187  double dSqr = hypot_sqr(pv.truePose.translation().x() - l->truePose.x(), pv.truePose.translation().y() - l->truePose.y());
188  if (dSqr > maxSensorSqr)
189  continue;
190  double obs = Rand::uniform_rand(0.0, 1.0);
191  if (obs > observationProb) // we do not see this one...
192  continue;
193  if (l->id < 0)
194  l->id = globalId++;
195  if (l->seenBy.size() == 0) {
196  Vector2d trueObservation = trueInv * l->truePose;
197  Vector2d observation = trueObservation;
198  observation[0] += Rand::gauss_rand(0., landmarkNoise[0]);
199  observation[1] += Rand::gauss_rand(0., landmarkNoise[1]);
200  l->simulatedPose = pv.simulatorPose * observation;
201  }
202  l->seenBy.push_back(pv.id);
203  pv.landmarks.push_back(l);
204  }
205  }
206 
207  }
208  cerr << "done." << endl;
209 
210  // add the odometry measurements
211  _odometry.clear();
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 
217  _odometry.push_back(GridEdge());
218  GridEdge& edge = _odometry.back();
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 
228  _landmarks.clear();
229  _landmarkObservations.clear();
230  // add the landmark observations
231  {
232  cerr << "Simulator: add landmark observations ... ";
233  Matrix2d covariance; covariance.fill(0.);
234  covariance(0, 0) = landmarkNoise[0]*landmarkNoise[0];
235  covariance(1, 1) = landmarkNoise[1]*landmarkNoise[1];
236  Matrix2d information = covariance.inverse();
237 
238  for (size_t i = 0; i < poses.size(); ++i) {
239  const GridPose& p = poses[i];
240  for (size_t j = 0; j < p.landmarks.size(); ++j) {
241  Landmark* l = p.landmarks[j];
242  if (l->seenBy.size() > 0 && l->seenBy[0] == p.id) {
243  landmarks.push_back(*l);
244  }
245  }
246  }
247 
248  for (size_t i = 0; i < poses.size(); ++i) {
249  const GridPose& p = poses[i];
250  SE2 trueInv = (p.truePose * sensorOffset).inverse();
251  for (size_t j = 0; j < p.landmarks.size(); ++j) {
252  Landmark* l = p.landmarks[j];
253  Vector2d observation;
254  Vector2d trueObservation = trueInv * l->truePose;
255  observation = trueObservation;
256  if (l->seenBy.size() > 0 && l->seenBy[0] == p.id) { // write the initial position of the landmark
257  observation = (p.simulatorPose * sensorOffset).inverse() * l->simulatedPose;
258  } else {
259  // create observation for the LANDMARK using the true positions
260  observation[0] += Rand::gauss_rand(0., landmarkNoise[0]);
261  observation[1] += Rand::gauss_rand(0., landmarkNoise[1]);
262  }
263 
264  _landmarkObservations.push_back(LandmarkEdge());
265  LandmarkEdge& le = _landmarkObservations.back();
266 
267  le.from = p.id;
268  le.to = l->id;
269  le.trueMeas = trueObservation;
270  le.simulatorMeas = observation;
271  le.information = information;
272  }
273  }
274  cerr << "done." << endl;
275  }
276 
277 
278  // cleaning up
279  for (LandmarkGrid::iterator it = grid.begin(); it != grid.end(); ++it) {
280  for (std::map<int, Simulator::LandmarkPtrVector>::iterator itt = it->second.begin(); itt != it->second.end(); ++itt) {
281  Simulator::LandmarkPtrVector& landmarks = itt->second;
282  for (size_t i = 0; i < landmarks.size(); ++i)
283  delete landmarks[i];
284  }
285  }
286 
287  }
g2o::tutorial::Simulator::_landmarkObservations
LandmarkEdgeVector _landmarkObservations
Definition: simulator.h:117
g2o::tutorial::Simulator::getMotion
SE2 getMotion(int motionDirection, double stepLen)
Definition: simulator.cpp:299
g2o::tutorial::Simulator::poses
const PosesVector & poses() const
Definition: simulator.h:108
g2o::tutorial::Rand::uniform_rand
static double uniform_rand(double lowerBndr, double upperBndr)
Definition: rand.h:64
g2o::tutorial::Simulator::PosesVector
std::vector< GridPose, Eigen::aligned_allocator< GridPose > > PosesVector
Definition: simulator.h:75
g2o::tutorial::Simulator::_landmarks
LandmarkVector _landmarks
Definition: simulator.h:115
g2o::hypot_sqr
T hypot_sqr(T x, T y)
Definition: misc.h:70
g2o::tutorial::LandmarkGrid
std::map< int, std::map< int, Simulator::LandmarkPtrVector > > LandmarkGrid
Definition: simulator.cpp:48
DEG2RAD
#define DEG2RAD(x)
Definition: macros.h:31
g2o::tutorial::Simulator::landmarks
const LandmarkVector & landmarks() const
Definition: simulator.h:109
g2o::tutorial::Simulator::_odometry
GridEdgeVector _odometry
Definition: simulator.h:116
g2o::tutorial::Rand::gauss_rand
static double gauss_rand(double mean, double sigma)
Definition: rand.h:50
g2o::tutorial::Simulator::LandmarkVector
std::vector< Landmark, Eigen::aligned_allocator< Landmark > > LandmarkVector
Definition: simulator.h:61
g2o::tutorial::Simulator::generateNewPose
GridPose generateNewPose(const GridPose &prev, const SE2 &trueMotion, const Eigen::Vector2d &transNoise, double rotNoise)
Definition: simulator.cpp:289
g2o::tutorial::Simulator::LandmarkPtrVector
std::vector< Landmark * > LandmarkPtrVector
Definition: simulator.h:62

Member Data Documentation

LandmarkEdgeVector g2o::tutorial::Simulator::_landmarkObservations
protected

Definition at line 117 of file simulator.h.

LandmarkVector g2o::tutorial::Simulator::_landmarks
protected

Definition at line 115 of file simulator.h.

GridEdgeVector g2o::tutorial::Simulator::_odometry
protected

Definition at line 116 of file simulator.h.

PosesVector g2o::tutorial::Simulator::_poses
protected

Definition at line 114 of file simulator.h.

The documentation for this class was generated from the following files:
Generated on Mon Mar 18 2019 20:15:20 for g2o by   doxygen 1.8.11