SimPolygon2D.cpp
Go to the documentation of this file.
1 
6 #include <iostream>
7 #include <random>
8 
10 
11 namespace gtsam {
12 
13 using namespace std;
14 
15 const size_t max_it = 100000;
16 std::minstd_rand SimPolygon2D::rng(42u);
17 
18 /* ************************************************************************* */
19 void SimPolygon2D::seedGenerator(unsigned long seed) {
20  rng = std::minstd_rand(seed);
21 }
22 
23 /* ************************************************************************* */
26  result.landmarks_.push_back(pA);
27  result.landmarks_.push_back(pB);
28  result.landmarks_.push_back(pC);
29  return result;
30 }
31 
32 /* ************************************************************************* */
33 SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, double width) {
35  result.landmarks_.push_back(p);
36  result.landmarks_.push_back(p + Point2(width, 0.0));
37  result.landmarks_.push_back(p + Point2(width, height));
38  result.landmarks_.push_back(p + Point2(0.0, height));
39  return result;
40 }
41 
42 /* ************************************************************************* */
43 bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
44  if (p.size() != size()) return false;
45  for (size_t i=0; i<size(); ++i)
46  if (!traits<Point2>::Equals(landmarks_[i], p.landmarks_[i], tol))
47  return false;
48  return true;
49 }
50 
51 /* ************************************************************************* */
52 void SimPolygon2D::print(const string& s) const {
53  cout << "SimPolygon " << s << ": " << endl;
54  for(const Point2& p: landmarks_)
56 }
57 
58 /* ************************************************************************* */
59 vector<SimWall2D> SimPolygon2D::walls() const {
60  vector<SimWall2D> result;
61  for (size_t i=0; i<size()-1; ++i)
62  result.push_back(SimWall2D(landmarks_[i], landmarks_[i+1]));
63  result.push_back(SimWall2D(landmarks_[size()-1], landmarks_[0]));
64  return result;
65 }
66 
67 /* ************************************************************************* */
68 bool SimPolygon2D::contains(const Point2& c) const {
69  vector<SimWall2D> edges = walls();
70  bool initialized = false;
71  bool lastSide = false;
72  for(const SimWall2D& ab: edges) {
73  // compute cross product of ab and ac
74  Point2 dab = ab.b() - ab.a();
75  Point2 dac = c - ab.a();
76  double cross = dab.x() * dac.y() - dab.y() * dac.x();
77  if (std::abs(cross) < 1e-6) // check for on one of the edges
78  return true;
79  bool side = cross > 0;
80  // save the first side found
81  if (!initialized) {
82  lastSide = side;
83  initialized = true;
84  continue;
85  }
86 
87  // to be inside the polygon, point must be on the same side of all lines
88  if (lastSide != side)
89  return false;
90  }
91  return true;
92 }
93 
94 /* ************************************************************************* */
96  for(const Point2& a: landmarks_)
97  if (p.contains(a))
98  return true;
99  for(const Point2& a: p.landmarks_)
100  if (contains(a))
101  return true;
102  return false;
103 }
104 
105 /* ***************************************************************** */
106 bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
107  for(const SimPolygon2D& poly: obstacles)
108  if (poly.contains(p))
109  return true;
110  return false;
111 }
112 
113 /* ************************************************************************* */
114 bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
115  for(const SimPolygon2D& poly: obstacles)
116  if (poly.overlaps(p))
117  return true;
118  return false;
119 }
120 
121 /* ************************************************************************* */
123  double side_len, double mean_side_len, double sigma_side_len,
124  double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
125  // get the current set of landmarks
126  Point2Vector lms;
127  double d2 = side_len/2.0;
128  lms.push_back(Point2( d2, d2));
129  lms.push_back(Point2(-d2, d2));
130  lms.push_back(Point2(-d2,-d2));
131  lms.push_back(Point2( d2,-d2));
132 
133  for(const SimPolygon2D& poly: existing_polys)
134  lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
135 
136  for (size_t i=0; i<max_it; ++i) {
137 
138  // find a random pose for line AB
139  Pose2 xA(randomAngle(), randomBoundedPoint2(side_len, lms, existing_polys, min_vertex_dist));
140 
141  // extend line by random dist and angle to get BC
142  double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
143  double tABC = randomAngle().theta();
144  Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC).finished());
145 
146  // extend from B to find C
147  double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
148  Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);
149 
150  // use triangle equality to verify non-degenerate triangle
151  double dAC = distance2(xA.t(), xC.t());
152 
153  // form a triangle and test if it meets requirements
154  SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
155 
156  // check inside walls, long enough edges, far away from landmarks
157  const double thresh = mean_side_len / 2.0;
158  if ((dAB + dBC + thresh > dAC) && // triangle inequality
159  (dAB + dAC + thresh > dBC) &&
160  (dAC + dBC + thresh > dAB) &&
161  insideBox(side_len, test_tri.landmark(0)) &&
162  insideBox(side_len, test_tri.landmark(1)) &&
163  insideBox(side_len, test_tri.landmark(2)) &&
164  distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len &&
165  !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
166  !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
167  !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
168  !anyOverlaps(test_tri, existing_polys)) {
169  return test_tri;
170  }
171  }
172  throw runtime_error("Could not find space for a triangle");
173  return SimPolygon2D::createTriangle(Point2(99,99), Point2(99,99), Point2(99,99));
174 }
175 
176 /* ************************************************************************* */
178  double side_len, double mean_side_len, double sigma_side_len,
179  double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
180  // get the current set of landmarks
181  Point2Vector lms;
182  double d2 = side_len/2.0;
183  lms.push_back(Point2( d2, d2));
184  lms.push_back(Point2(-d2, d2));
185  lms.push_back(Point2(-d2,-d2));
186  lms.push_back(Point2( d2,-d2));
187  for(const SimPolygon2D& poly: existing_polys)
188  lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
189 
190  const Point2 lower_corner(-side_len,-side_len);
191  const Point2 upper_corner( side_len, side_len);
192 
193  for (size_t i=0; i<max_it; ++i) {
194 
195  // pick height and width to be viable distances
196  double height = randomDistance(mean_side_len, sigma_side_len, min_side_len);
197  double width = randomDistance(mean_side_len, sigma_side_len, min_side_len);
198 
199  // find a starting point - limited to region viable for this height/width
200  Point2 pA = randomBoundedPoint2(lower_corner, upper_corner - Point2(width, height),
201  lms, existing_polys, min_vertex_dist);
202 
203  // verify
204  SimPolygon2D rect = SimPolygon2D::createRectangle(pA, height, width);
205 
206  // check inside walls, long enough edges, far away from landmarks
207  if (insideBox(side_len, rect.landmark(0)) &&
208  insideBox(side_len, rect.landmark(1)) &&
209  insideBox(side_len, rect.landmark(2)) &&
210  insideBox(side_len, rect.landmark(3)) &&
211  !nearExisting(lms, rect.landmark(0), min_vertex_dist) &&
212  !nearExisting(lms, rect.landmark(1), min_vertex_dist) &&
213  !nearExisting(lms, rect.landmark(2), min_vertex_dist) &&
214  !nearExisting(lms, rect.landmark(3), min_vertex_dist) &&
215  !anyOverlaps(rect, existing_polys)) {
216  return rect;
217  }
218  }
219  throw runtime_error("Could not find space for a rectangle");
220  return SimPolygon2D::createRectangle(Point2(99,99), 100, 100);
221 }
222 
223 /* ***************************************************************** */
225  std::uniform_real_distribution<> gen_t(-s/2.0, s/2.0);
226  return Point2(gen_t(rng), gen_t(rng));
227 }
228 
229 /* ***************************************************************** */
231  // modified range to avoid degenerate cases in triangles:
232  std::uniform_real_distribution<> gen_r(-M_PI, M_PI);
233  return Rot2::fromAngle(gen_r(rng));
234 }
235 
236 /* ***************************************************************** */
237 double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
238  std::normal_distribution<> norm_dist(mu, sigma);
239  double d = -10.0;
240  for (size_t i=0; i<max_it; ++i) {
241  d = std::abs(norm_dist(rng));
242  if (d > min_dist)
243  return d;
244  }
245  cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma
246  << " min_dist = " << min_dist << endl;
247  throw runtime_error("Failed to find a viable distance");
248  return std::abs(norm_dist(rng));
249 }
250 
251 /* ***************************************************************** */
253  const vector<SimPolygon2D>& obstacles) {
254  for (size_t i=0; i<max_it; ++i) {
255  Point2 p = randomPoint2(boundary_size);
256  if (!anyContains(p, obstacles))
257  return p;
258  }
259  throw runtime_error("Failed to find a place for a landmark!");
260  return Point2(0,0);
261 }
262 
263 /* ***************************************************************** */
265  const Point2Vector& landmarks, double min_landmark_dist) {
266  for (size_t i=0; i<max_it; ++i) {
267  Point2 p = randomPoint2(boundary_size);
268  if (!nearExisting(landmarks, p, min_landmark_dist))
269  return p;
270  }
271  throw runtime_error("Failed to find a place for a landmark!");
272  return Point2(0,0);
273 }
274 
275 /* ***************************************************************** */
277  const Point2Vector& landmarks,
278  const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
279  for (size_t i=0; i<max_it; ++i) {
280  Point2 p = randomPoint2(boundary_size);
281  if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
282  return p;
283  }
284  throw runtime_error("Failed to find a place for a landmark!");
285  return Point2(0,0);
286 }
287 
288 /* ***************************************************************** */
290  const Point2& LL_corner, const Point2& UR_corner,
291  const Point2Vector& landmarks,
292  const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
293 
294  std::uniform_real_distribution<> gen_x(0.0, UR_corner.x() - LL_corner.x());
295  std::uniform_real_distribution<> gen_y(0.0, UR_corner.y() - LL_corner.y());
296 
297  for (size_t i=0; i<max_it; ++i) {
298  Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
299  if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
300  return p;
301  }
302  throw runtime_error("Failed to find a place for a landmark!");
303  return Point2(0,0);
304 }
305 
306 /* ***************************************************************** */
307 Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector<SimPolygon2D>& obstacles) {
308  return Pose2(randomAngle(), randomBoundedPoint2(boundary_size, obstacles));
309 }
310 
311 /* ***************************************************************** */
312 bool SimPolygon2D::insideBox(double s, const Point2& p) {
313  return std::abs(p.x()) < s/2.0 && std::abs(p.y()) < s/2.0;
314 }
315 
316 /* ***************************************************************** */
318  const Point2& p, double threshold) {
319  for(const Point2& Sp: S)
320  if (distance2(Sp, p) < threshold)
321  return true;
322  return false;
323 }
324 
325 } //\namespace gtsam
326 
vector< MFAS::KeyPair > edges
Definition: testMFAS.cpp:25
std::vector< SimWall2D > walls() const
const size_t max_it
bool equals(const SimPolygon2D &p, double tol=1e-5) const
Point2 pB(size_t i)
Vector2 Point2
Definition: Point2.h:32
double mu
static std::mt19937 rng
static double randomDistance(double mu, double sigma, double min_dist=-1.0)
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:106
static SimPolygon2D createTriangle(const Point2 &pA, const Point2 &pB, const Point2 &pC)
bool overlaps(const SimPolygon2D &p) const
Definition: BFloat16.h:88
Point2Vector landmarks_
Definition: SimPolygon2D.h:22
const Point2 & t() const
translation
Definition: Pose2.h:255
DiscreteKey S(1, 2)
static Point2 randomBoundedPoint2(double boundary_size, const Point2Vector &landmarks, double min_landmark_dist)
const Point2 & landmark(size_t i) const
Definition: SimPolygon2D.h:59
static std::minstd_rand rng
Definition: SimPolygon2D.h:23
Polygons for simulation use.
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
static Rot2 randomAngle()
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
void print(const std::string &s="") const
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
static SimPolygon2D createRectangle(const Point2 &p, double height, double width)
static Pose2 randomFreePose(double boundary_size, const std::vector< SimPolygon2D > &obstacles)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len, double min_vertex_dist, double min_side_len, const std::vector< SimPolygon2D > &existing_polys)
traits
Definition: chartTesting.h:28
static bool anyOverlaps(const SimPolygon2D &p, const std::vector< SimPolygon2D > &obstacles)
Point2 pA(size_t i)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
float * p
size_t size() const
Definition: SimPolygon2D.h:60
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
static const double sigma
static void seedGenerator(unsigned long seed)
const G double tol
Definition: Group.h:86
#define abs(x)
Definition: datatypes.h:17
bool contains(const Point2 &p) const
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
static SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len, double min_vertex_dist, double min_side_len, const std::vector< SimPolygon2D > &existing_polys)
static bool anyContains(const Point2 &p, const std::vector< SimPolygon2D > &obstacles)
static bool insideBox(double s, const Point2 &p)
static bool nearExisting(const Point2Vector &S, const Point2 &p, double threshold)
static Point2 randomPoint2(double s)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:46