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

Cleaning up extraneous namespace references

release/4.3a0
Alex Cunningham 2013-03-23 20:19:39 +00:00
parent da334ed8a2
commit 045072746d
6 changed files with 46 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -15,7 +15,6 @@
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
using namespace gtsam;
const size_t max_it = 100000; const size_t max_it = 100000;
boost::minstd_rand SimPolygon2D::rng(42u); boost::minstd_rand SimPolygon2D::rng(42u);
@ -128,7 +127,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<gtsam::Point2> lms; std::vector<Point2> 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));
@ -150,7 +149,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
// extend from B to find C // extend from B to find C
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);
Pose2 xC = xB.retract(gtsam::delta(3, 0, dBC)); Pose2 xC = xB.retract(delta(3, 0, dBC));
// use triangle equality to verify non-degenerate triangle // use triangle equality to verify non-degenerate triangle
double dAC = xA.t().dist(xC.t()); double dAC = xA.t().dist(xC.t());
@ -183,7 +182,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<gtsam::Point2> lms; std::vector<Point2> 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));
@ -267,7 +266,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */ /* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
const std::vector<gtsam::Point2>& landmarks, double min_landmark_dist) { const std::vector<Point2>& 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))
@ -279,7 +278,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */ /* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
const std::vector<gtsam::Point2>& landmarks, const std::vector<Point2>& 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);
@ -292,8 +291,8 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */ /* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2( Point2 SimPolygon2D::randomBoundedPoint2(
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner, const Point2& LL_corner, const Point2& UR_corner,
const std::vector<gtsam::Point2>& landmarks, const std::vector<Point2>& 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());
@ -319,9 +318,9 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
} }
/* ***************************************************************** */ /* ***************************************************************** */
bool SimPolygon2D::nearExisting(const std::vector<gtsam::Point2>& S, bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
const Point2& p, double threshold) { const Point2& p, double threshold) {
BOOST_FOREACH(const gtsam::Point2& Sp, S) BOOST_FOREACH(const Point2& Sp, S)
if (Sp.dist(p) < threshold) if (Sp.dist(p) < threshold)
return true; return true;
return false; return false;

42
gtsam_unstable/geometry/SimPolygon2D.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
*/ */
class SimPolygon2D { class SimPolygon2D {
protected: protected:
std::vector<gtsam::Point2> landmarks_; std::vector<Point2> landmarks_;
static boost::minstd_rand rng; static boost::minstd_rand rng;
public: public:
@ -30,13 +30,13 @@ public:
static void seedGenerator(unsigned long seed); static void seedGenerator(unsigned long seed);
/** Named constructor for creating triangles */ /** Named constructor for creating triangles */
static SimPolygon2D createTriangle(const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::Point2& pC); static SimPolygon2D createTriangle(const Point2& pA, const Point2& pB, const Point2& pC);
/** /**
* Named constructor for creating axis-aligned rectangles * Named constructor for creating axis-aligned rectangles
* @param p is the lower-left corner * @param p is the lower-left corner
*/ */
static SimPolygon2D createRectangle(const gtsam::Point2& p, double height, double width); static SimPolygon2D createRectangle(const Point2& p, double height, double width);
/** /**
* Randomly generate a triangle that does not conflict with others * Randomly generate a triangle that does not conflict with others
@ -55,9 +55,9 @@ public:
double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys); double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
// access to underlying points // access to underlying points
const gtsam::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<gtsam::Point2>& vertices() const { return landmarks_; } const std::vector<Point2>& 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;
@ -73,7 +73,7 @@ public:
* Polygons are closed, convex shapes. * Polygons are closed, convex shapes.
* @return true if the given point is contained by this polygon * @return true if the given point is contained by this polygon
*/ */
bool contains(const gtsam::Point2& p) const; bool contains(const Point2& p) const;
/** /**
* Checks two polygons to determine if they overlap * Checks two polygons to determine if they overlap
@ -82,48 +82,48 @@ public:
bool overlaps(const SimPolygon2D& p) const; bool overlaps(const SimPolygon2D& p) const;
/** returns true iff p is contained in any of a set of polygons */ /** returns true iff p is contained in any of a set of polygons */
static bool anyContains(const gtsam::Point2& p, const std::vector<SimPolygon2D>& obstacles); static bool anyContains(const Point2& p, const std::vector<SimPolygon2D>& obstacles);
/** returns true iff polygon p overlaps with any of a set of polygons */ /** returns true iff polygon p overlaps with any of a set of polygons */
static bool anyOverlaps(const SimPolygon2D& p, const std::vector<SimPolygon2D>& obstacles); static bool anyOverlaps(const SimPolygon2D& p, const std::vector<SimPolygon2D>& obstacles);
/** returns true iff p is inside a square centered at zero with side s */ /** returns true iff p is inside a square centered at zero with side s */
static bool insideBox(double s, const gtsam::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<gtsam::Point2>& S, static bool nearExisting(const std::vector<Point2>& S,
const gtsam::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 */
static gtsam::Point2 randomPoint2(double s); static Point2 randomPoint2(double s);
/** randomly generate a Rot2 with a uniform distribution over theta */ /** randomly generate a Rot2 with a uniform distribution over theta */
static gtsam::Rot2 randomAngle(); static Rot2 randomAngle();
/** generate a distance from a normal distribution given a mean and sigma */ /** generate a distance from a normal distribution given a mean and sigma */
static double randomDistance(double mu, double sigma, double min_dist = -1.0); static double randomDistance(double mu, double sigma, double min_dist = -1.0);
/** 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 gtsam::Point2 randomBoundedPoint2(double boundary_size, static Point2 randomBoundedPoint2(double boundary_size,
const std::vector<gtsam::Point2>& landmarks, double min_landmark_dist); const std::vector<Point2>& 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 gtsam::Point2 randomBoundedPoint2(double boundary_size, static Point2 randomBoundedPoint2(double boundary_size,
const std::vector<gtsam::Point2>& landmarks, const std::vector<Point2>& 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 */
static gtsam::Point2 randomBoundedPoint2(double boundary_size, static Point2 randomBoundedPoint2(double boundary_size,
const std::vector<SimPolygon2D>& obstacles); const std::vector<SimPolygon2D>& obstacles);
/** 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 gtsam::Point2 randomBoundedPoint2( static Point2 randomBoundedPoint2(
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner, const Point2& LL_corner, const Point2& UR_corner,
const std::vector<gtsam::Point2>& landmarks, const std::vector<Point2>& 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 */
static gtsam::Pose2 randomFreePose(double boundary_size, const std::vector<SimPolygon2D>& obstacles); static Pose2 randomFreePose(double boundary_size, const std::vector<SimPolygon2D>& obstacles);
}; };

11
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -11,7 +11,6 @@
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
void SimWall2D::print(const std::string& s) const { void SimWall2D::print(const std::string& s) const {
@ -26,7 +25,7 @@ bool SimWall2D::equals(const SimWall2D& other, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool SimWall2D::intersects(const SimWall2D& B, boost::optional<gtsam::Point2&> pt) const { bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) const {
const bool debug = false; const bool debug = false;
const SimWall2D& A = *this; const SimWall2D& A = *this;
@ -101,7 +100,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<gtsam::Point2&> p
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Point2 SimWall2D::midpoint() const { Point2 SimWall2D::midpoint() const {
Point2 vec = b_ - a_; Point2 vec = b_ - a_;
return a_ + vec * 0.5 * vec.norm(); return a_ + vec * 0.5 * vec.norm();
} }
@ -123,9 +122,9 @@ Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const
} }
/* ***************************************************************** */ /* ***************************************************************** */
std::pair<gtsam::Pose2, bool> moveWithBounce(const gtsam::Pose2& cur_pose, double step_size, std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
const std::vector<SimWall2D> walls, gtsam::Sampler& angle_drift, const std::vector<SimWall2D> walls, Sampler& angle_drift,
gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias) { Sampler& reflect_noise, const Rot2& bias) {
// calculate angle to change by // calculate angle to change by
Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());

22
gtsam_unstable/geometry/SimWall2D.h
View File

@ -17,14 +17,14 @@ namespace gtsam {
*/ */
class SimWall2D { class SimWall2D {
protected: protected:
gtsam::Point2 a_, b_; Point2 a_, b_;
public: public:
/** default constructor makes canonical wall */ /** default constructor makes canonical wall */
SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {} SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {}
/** constructors using endpoints */ /** constructors using endpoints */
SimWall2D(const gtsam::Point2& a, const gtsam::Point2& b) SimWall2D(const Point2& a, const Point2& b)
: a_(a), b_(b) {} : a_(a), b_(b) {}
SimWall2D(double ax, double ay, double bx, double by) SimWall2D(double ax, double ay, double bx, double by)
@ -35,33 +35,33 @@ namespace gtsam {
bool equals(const SimWall2D& other, double tol=1e-9) const; bool equals(const SimWall2D& other, double tol=1e-9) const;
/** access */ /** access */
gtsam::Point2 a() const { return a_; } Point2 a() const { return a_; }
gtsam::Point2 b() const { return b_; } Point2 b() const { return b_; }
/** scales a wall to produce a new wall */ /** scales a wall to produce a new wall */
SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); } SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
/** geometry */ /** geometry */
double length() const { return a_.dist(b_); } double length() const { return a_.dist(b_); }
gtsam::Point2 midpoint() const; Point2 midpoint() const;
/** /**
* intersection check between two segments * intersection check between two segments
* returns true if they intersect, with the intersection * returns true if they intersect, with the intersection
* point in the optional second argument * point in the optional second argument
*/ */
bool intersects(const SimWall2D& wall, boost::optional<gtsam::Point2&> pt=boost::none) const; bool intersects(const SimWall2D& wall, boost::optional<Point2&> pt=boost::none) const;
/** /**
* norm is a 2D point representing the norm of the wall * norm is a 2D point representing the norm of the wall
*/ */
gtsam::Point2 norm() const; Point2 norm() const;
/** /**
* reflection around a point of impact with a wall from a starting (init) point * reflection around a point of impact with a wall from a starting (init) point
* at a given impact point (intersection), returning the angle away from the impact * at a given impact point (intersection), returning the angle away from the impact
*/ */
gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const; Rot2 reflection(const Point2& init, const Point2& intersection) const;
}; };
@ -78,8 +78,8 @@ namespace gtsam {
* @return the next pose for the robot * @return the next pose for the robot
* NOTE: samplers cannot be const * NOTE: samplers cannot be const
*/ */
std::pair<gtsam::Pose2, bool> moveWithBounce(const gtsam::Pose2& cur_pose, double step_size, std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
const std::vector<SimWall2D> walls, gtsam::Sampler& angle_drift, const std::vector<SimWall2D> walls, Sampler& angle_drift,
gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias = gtsam::Rot2()); Sampler& reflect_noise, const Rot2& bias = Rot2());
} // \namespace gtsam } // \namespace gtsam

1
gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
View File

@ -9,7 +9,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam;
const double tol=1e-5; const double tol=1e-5;

1
gtsam_unstable/geometry/tests/testSimWall2D.cpp
View File

@ -8,7 +8,6 @@
#include <gtsam_unstable/geometry/SimWall2D.h> #include <gtsam_unstable/geometry/SimWall2D.h>
using namespace gtsam;
using namespace gtsam; using namespace gtsam;
const double tol = 1e-5; const double tol = 1e-5;