330 lines
12 KiB
C++
330 lines
12 KiB
C++
/**
|
|
* @file SimPolygon2D.cpp
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <iostream>
|
|
#include <boost/random/linear_congruential.hpp>
|
|
#include <boost/random/uniform_real.hpp>
|
|
#include <boost/random/normal_distribution.hpp>
|
|
#include <boost/random/variate_generator.hpp>
|
|
|
|
#include <gtsam_unstable/geometry/SimPolygon2D.h>
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace std;
|
|
|
|
const size_t max_it = 100000;
|
|
boost::minstd_rand SimPolygon2D::rng(42u);
|
|
|
|
/* ************************************************************************* */
|
|
void SimPolygon2D::seedGenerator(unsigned long seed) {
|
|
rng = boost::minstd_rand(seed);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
SimPolygon2D SimPolygon2D::createTriangle(const Point2& pA, const Point2& pB, const Point2& pC) {
|
|
SimPolygon2D result;
|
|
result.landmarks_.push_back(pA);
|
|
result.landmarks_.push_back(pB);
|
|
result.landmarks_.push_back(pC);
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, double width) {
|
|
SimPolygon2D result;
|
|
result.landmarks_.push_back(p);
|
|
result.landmarks_.push_back(p + Point2(width, 0.0));
|
|
result.landmarks_.push_back(p + Point2(width, height));
|
|
result.landmarks_.push_back(p + Point2(0.0, height));
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
|
|
if (p.size() != size()) return false;
|
|
for (size_t i=0; i<size(); ++i)
|
|
if (!traits<Point2>::Equals(landmarks_[i], p.landmarks_[i], tol))
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void SimPolygon2D::print(const string& s) const {
|
|
cout << "SimPolygon " << s << ": " << endl;
|
|
for(const Point2& p: landmarks_)
|
|
traits<Point2>::Print(p, " ");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
vector<SimWall2D> SimPolygon2D::walls() const {
|
|
vector<SimWall2D> result;
|
|
for (size_t i=0; i<size()-1; ++i)
|
|
result.push_back(SimWall2D(landmarks_[i], landmarks_[i+1]));
|
|
result.push_back(SimWall2D(landmarks_[size()-1], landmarks_[0]));
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool SimPolygon2D::contains(const Point2& c) const {
|
|
vector<SimWall2D> edges = walls();
|
|
bool initialized = false;
|
|
bool lastSide = false;
|
|
for(const SimWall2D& ab: edges) {
|
|
// compute cross product of ab and ac
|
|
Point2 dab = ab.b() - ab.a();
|
|
Point2 dac = c - ab.a();
|
|
double cross = dab.x() * dac.y() - dab.y() * dac.x();
|
|
if (fabs(cross) < 1e-6) // check for on one of the edges
|
|
return true;
|
|
bool side = cross > 0;
|
|
// save the first side found
|
|
if (!initialized) {
|
|
lastSide = side;
|
|
initialized = true;
|
|
continue;
|
|
}
|
|
|
|
// to be inside the polygon, point must be on the same side of all lines
|
|
if (lastSide != side)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
|
|
for(const Point2& a: landmarks_)
|
|
if (p.contains(a))
|
|
return true;
|
|
for(const Point2& a: p.landmarks_)
|
|
if (contains(a))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
|
|
for(const SimPolygon2D& poly: obstacles)
|
|
if (poly.contains(p))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
|
|
for(const SimPolygon2D& poly: obstacles)
|
|
if (poly.overlaps(p))
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
SimPolygon2D SimPolygon2D::randomTriangle(
|
|
double side_len, double mean_side_len, double sigma_side_len,
|
|
double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
|
|
// get the current set of landmarks
|
|
std::vector<Point2> lms;
|
|
double d2 = side_len/2.0;
|
|
lms.push_back(Point2( d2, d2));
|
|
lms.push_back(Point2(-d2, d2));
|
|
lms.push_back(Point2(-d2,-d2));
|
|
lms.push_back(Point2( d2,-d2));
|
|
|
|
for(const SimPolygon2D& poly: existing_polys)
|
|
lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
|
|
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
|
|
// find a random pose for line AB
|
|
Pose2 xA(randomAngle(), randomBoundedPoint2(side_len, lms, existing_polys, min_vertex_dist));
|
|
|
|
// extend line by random dist and angle to get BC
|
|
double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
|
double tABC = randomAngle().theta();
|
|
Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC).finished());
|
|
|
|
// extend from B to find C
|
|
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
|
Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);
|
|
|
|
// use triangle equality to verify non-degenerate triangle
|
|
double dAC = distance2(xA.t(), xC.t());
|
|
|
|
// form a triangle and test if it meets requirements
|
|
SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
|
|
|
|
// check inside walls, long enough edges, far away from landmarks
|
|
const double thresh = mean_side_len / 2.0;
|
|
if ((dAB + dBC + thresh > dAC) && // triangle inequality
|
|
(dAB + dAC + thresh > dBC) &&
|
|
(dAC + dBC + thresh > dAB) &&
|
|
insideBox(side_len, test_tri.landmark(0)) &&
|
|
insideBox(side_len, test_tri.landmark(1)) &&
|
|
insideBox(side_len, test_tri.landmark(2)) &&
|
|
distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len &&
|
|
!nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
|
|
!nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
|
|
!nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
|
|
!anyOverlaps(test_tri, existing_polys)) {
|
|
return test_tri;
|
|
}
|
|
}
|
|
throw runtime_error("Could not find space for a triangle");
|
|
return SimPolygon2D::createTriangle(Point2(99,99), Point2(99,99), Point2(99,99));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
SimPolygon2D SimPolygon2D::randomRectangle(
|
|
double side_len, double mean_side_len, double sigma_side_len,
|
|
double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
|
|
// get the current set of landmarks
|
|
std::vector<Point2> lms;
|
|
double d2 = side_len/2.0;
|
|
lms.push_back(Point2( d2, d2));
|
|
lms.push_back(Point2(-d2, d2));
|
|
lms.push_back(Point2(-d2,-d2));
|
|
lms.push_back(Point2( d2,-d2));
|
|
for(const SimPolygon2D& poly: existing_polys)
|
|
lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end());
|
|
|
|
const Point2 lower_corner(-side_len,-side_len);
|
|
const Point2 upper_corner( side_len, side_len);
|
|
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
|
|
// pick height and width to be viable distances
|
|
double height = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
|
double width = randomDistance(mean_side_len, sigma_side_len, min_side_len);
|
|
|
|
// find a starting point - limited to region viable for this height/width
|
|
Point2 pA = randomBoundedPoint2(lower_corner, upper_corner - Point2(width, height),
|
|
lms, existing_polys, min_vertex_dist);
|
|
|
|
// verify
|
|
SimPolygon2D rect = SimPolygon2D::createRectangle(pA, height, width);
|
|
|
|
// check inside walls, long enough edges, far away from landmarks
|
|
if (insideBox(side_len, rect.landmark(0)) &&
|
|
insideBox(side_len, rect.landmark(1)) &&
|
|
insideBox(side_len, rect.landmark(2)) &&
|
|
insideBox(side_len, rect.landmark(3)) &&
|
|
!nearExisting(lms, rect.landmark(0), min_vertex_dist) &&
|
|
!nearExisting(lms, rect.landmark(1), min_vertex_dist) &&
|
|
!nearExisting(lms, rect.landmark(2), min_vertex_dist) &&
|
|
!nearExisting(lms, rect.landmark(3), min_vertex_dist) &&
|
|
!anyOverlaps(rect, existing_polys)) {
|
|
return rect;
|
|
}
|
|
}
|
|
throw runtime_error("Could not find space for a rectangle");
|
|
return SimPolygon2D::createRectangle(Point2(99,99), 100, 100);
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Point2 SimPolygon2D::randomPoint2(double s) {
|
|
boost::uniform_real<> gen_t(-s/2.0, s/2.0);
|
|
return Point2(gen_t(rng), gen_t(rng));
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Rot2 SimPolygon2D::randomAngle() {
|
|
boost::uniform_real<> gen_r(-M_PI, M_PI); // modified range to avoid degenerate cases in triangles
|
|
return Rot2::fromAngle(gen_r(rng));
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
|
|
boost::normal_distribution<double> norm_dist(mu, sigma);
|
|
boost::variate_generator<boost::minstd_rand&, boost::normal_distribution<double> > gen_d(rng, norm_dist);
|
|
double d = -10.0;
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
d = fabs(gen_d());
|
|
if (d > min_dist)
|
|
return d;
|
|
}
|
|
cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma
|
|
<< " min_dist = " << min_dist << endl;
|
|
throw runtime_error("Failed to find a viable distance");
|
|
return fabs(norm_dist(rng));
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
|
|
const vector<SimPolygon2D>& obstacles) {
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
Point2 p = randomPoint2(boundary_size);
|
|
if (!anyContains(p, obstacles))
|
|
return p;
|
|
}
|
|
throw runtime_error("Failed to find a place for a landmark!");
|
|
return Point2(0,0);
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
|
|
const std::vector<Point2>& landmarks, double min_landmark_dist) {
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
Point2 p = randomPoint2(boundary_size);
|
|
if (!nearExisting(landmarks, p, min_landmark_dist))
|
|
return p;
|
|
}
|
|
throw runtime_error("Failed to find a place for a landmark!");
|
|
return Point2(0,0);
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
|
|
const std::vector<Point2>& landmarks,
|
|
const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
Point2 p = randomPoint2(boundary_size);
|
|
if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
|
|
return p;
|
|
}
|
|
throw runtime_error("Failed to find a place for a landmark!");
|
|
return Point2(0,0);
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Point2 SimPolygon2D::randomBoundedPoint2(
|
|
const Point2& LL_corner, const Point2& UR_corner,
|
|
const std::vector<Point2>& landmarks,
|
|
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
|
|
|
|
boost::uniform_real<> gen_x(0.0, UR_corner.x() - LL_corner.x());
|
|
boost::uniform_real<> gen_y(0.0, UR_corner.y() - LL_corner.y());
|
|
|
|
for (size_t i=0; i<max_it; ++i) {
|
|
Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
|
|
if (!nearExisting(landmarks, p, min_landmark_dist) && !anyContains(p, obstacles))
|
|
return p;
|
|
}
|
|
throw runtime_error("Failed to find a place for a landmark!");
|
|
return Point2(0,0);
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector<SimPolygon2D>& obstacles) {
|
|
return Pose2(randomAngle(), randomBoundedPoint2(boundary_size, obstacles));
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
bool SimPolygon2D::insideBox(double s, const Point2& p) {
|
|
return fabs(p.x()) < s/2.0 && fabs(p.y()) < s/2.0;
|
|
}
|
|
|
|
/* ***************************************************************** */
|
|
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
|
|
const Point2& p, double threshold) {
|
|
for(const Point2& Sp: S)
|
|
if (distance2(Sp, p) < threshold)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
} //\namespace gtsam
|
|
|