12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Used aligned container

release/4.3a0
Frank Dellaert 2018-09-27 10:40:44 -04:00
parent 90f688d94c
commit 4abb7dae6d
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -126,7 +126,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
double side_len, double mean_side_len, double sigma_side_len, double side_len, double mean_side_len, double sigma_side_len,
double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) { double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
// get the current set of landmarks // get the current set of landmarks
std::vector<Point2> lms; Point2Vector lms;
double d2 = side_len/2.0; 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));
@ -181,7 +181,7 @@ SimPolygon2D SimPolygon2D::randomRectangle(
double side_len, double mean_side_len, double sigma_side_len, double side_len, double mean_side_len, double sigma_side_len,
double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) { double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
// get the current set of landmarks // get the current set of landmarks
std::vector<Point2> lms; Point2Vector lms;
double d2 = side_len/2.0; 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));
@ -265,7 +265,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */ /* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
const std::vector<Point2>& landmarks, double min_landmark_dist) { const Point2Vector& landmarks, double min_landmark_dist) {
for (size_t i=0; i<max_it; ++i) { for (size_t i=0; i<max_it; ++i) {
Point2 p = randomPoint2(boundary_size); Point2 p = randomPoint2(boundary_size);
if (!nearExisting(landmarks, p, min_landmark_dist)) if (!nearExisting(landmarks, p, min_landmark_dist))
@ -277,7 +277,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */ /* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
const std::vector<Point2>& landmarks, const Point2Vector& landmarks,
const vector<SimPolygon2D>& obstacles, double min_landmark_dist) { const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
for (size_t i=0; i<max_it; ++i) { for (size_t i=0; i<max_it; ++i) {
Point2 p = randomPoint2(boundary_size); Point2 p = randomPoint2(boundary_size);
@ -291,7 +291,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */ /* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2( Point2 SimPolygon2D::randomBoundedPoint2(
const Point2& LL_corner, const Point2& UR_corner, const Point2& LL_corner, const Point2& UR_corner,
const std::vector<Point2>& landmarks, const Point2Vector& landmarks,
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) { 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_x(0.0, UR_corner.x() - LL_corner.x());
@ -317,7 +317,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
} }
/* ***************************************************************** */ /* ***************************************************************** */
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S, bool SimPolygon2D::nearExisting(const Point2Vector& S,
const Point2& p, double threshold) { const Point2& p, double threshold) {
for(const Point2& Sp: S) for(const Point2& Sp: S)
if (distance2(Sp, p) < threshold) if (distance2(Sp, p) < threshold)

12
gtsam_unstable/geometry/SimPolygon2D.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
*/ */
class GTSAM_UNSTABLE_EXPORT SimPolygon2D { class GTSAM_UNSTABLE_EXPORT SimPolygon2D {
protected: protected:
std::vector<Point2> landmarks_; Point2Vector landmarks_;
static boost::minstd_rand rng; static boost::minstd_rand rng;
public: public:
@ -57,7 +57,7 @@ public:
// access to underlying points // access to underlying points
const Point2& landmark(size_t i) const { return landmarks_[i]; } const Point2& landmark(size_t i) const { return landmarks_[i]; }
size_t size() const { return landmarks_.size(); } size_t size() const { return landmarks_.size(); }
const std::vector<Point2>& vertices() const { return landmarks_; } const Point2Vector& vertices() const { return landmarks_; }
// testable requirements // testable requirements
bool equals(const SimPolygon2D& p, double tol=1e-5) const; bool equals(const SimPolygon2D& p, double tol=1e-5) const;
@ -91,7 +91,7 @@ public:
static bool insideBox(double s, const Point2& p); static bool insideBox(double s, const Point2& p);
/** returns true iff p is within threshold of any point in S */ /** returns true iff p is within threshold of any point in S */
static bool nearExisting(const std::vector<Point2>& S, static bool nearExisting(const Point2Vector& S,
const Point2& p, double threshold); const Point2& p, double threshold);
/** pick a random point uniformly over a box of side s */ /** pick a random point uniformly over a box of side s */
@ -105,11 +105,11 @@ public:
/** pick a random point within a box that is further than dist d away from existing landmarks */ /** pick a random point within a box that is further than dist d away from existing landmarks */
static Point2 randomBoundedPoint2(double boundary_size, static Point2 randomBoundedPoint2(double boundary_size,
const std::vector<Point2>& landmarks, double min_landmark_dist); const Point2Vector& landmarks, double min_landmark_dist);
/** pick a random point within a box that meets above requirements, as well as staying out of obstacles */ /** pick a random point within a box that meets above requirements, as well as staying out of obstacles */
static Point2 randomBoundedPoint2(double boundary_size, static Point2 randomBoundedPoint2(double boundary_size,
const std::vector<Point2>& landmarks, const Point2Vector& landmarks,
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist); const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
/** pick a random point that only avoid obstacles */ /** pick a random point that only avoid obstacles */
@ -119,7 +119,7 @@ public:
/** pick a random point in box defined by lower left and upper right corners */ /** pick a random point in box defined by lower left and upper right corners */
static Point2 randomBoundedPoint2( static Point2 randomBoundedPoint2(
const Point2& LL_corner, const Point2& UR_corner, const Point2& LL_corner, const Point2& UR_corner,
const std::vector<Point2>& landmarks, const Point2Vector& landmarks,
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist); const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
/** pick a random pose in a bounded area that is not in an obstacle */ /** pick a random pose in a bounded area that is not in an obstacle */