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 {
using namespace std;
using namespace gtsam;
const size_t max_it = 100000;
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 min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
// get the current set of landmarks
std::vector<gtsam::Point2> lms;
std::vector<Point2> lms;
double d2 = side_len/2.0;
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
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
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 min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
// get the current set of landmarks
std::vector<gtsam::Point2> lms;
std::vector<Point2> lms;
double d2 = side_len/2.0;
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,
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) {
Point2 p = randomPoint2(boundary_size);
if (!nearExisting(landmarks, p, min_landmark_dist))
@ -279,7 +278,7 @@ 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) {
for (size_t i=0; i<max_it; ++i) {
Point2 p = randomPoint2(boundary_size);
@ -292,8 +291,8 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
/* ***************************************************************** */
Point2 SimPolygon2D::randomBoundedPoint2(
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner,
const std::vector<gtsam::Point2>& landmarks,
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());
@ -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) {
BOOST_FOREACH(const gtsam::Point2& Sp, S)
BOOST_FOREACH(const Point2& Sp, S)
if (Sp.dist(p) < threshold)
return true;
return false;

42
gtsam_unstable/geometry/SimPolygon2D.h
View File

@ -18,7 +18,7 @@ namespace gtsam {
*/
class SimPolygon2D {
protected:
std::vector<gtsam::Point2> landmarks_;
std::vector<Point2> landmarks_;
static boost::minstd_rand rng;
public:
@ -30,13 +30,13 @@ public:
static void seedGenerator(unsigned long seed);
/** 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
* @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
@ -55,9 +55,9 @@ public:
double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
// 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(); }
const std::vector<gtsam::Point2>& vertices() const { return landmarks_; }
const std::vector<Point2>& vertices() const { return landmarks_; }
// testable requirements
bool equals(const SimPolygon2D& p, double tol=1e-5) const;
@ -73,7 +73,7 @@ public:
* Polygons are closed, convex shapes.
* @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
@ -82,48 +82,48 @@ public:
bool overlaps(const SimPolygon2D& p) const;
/** 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 */
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 */
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 */
static bool nearExisting(const std::vector<gtsam::Point2>& S,
const gtsam::Point2& p, double threshold);
static bool nearExisting(const std::vector<Point2>& S,
const Point2& p, double threshold);
/** 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 */
static gtsam::Rot2 randomAngle();
static Rot2 randomAngle();
/** generate a distance from a normal distribution given a mean and sigma */
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 */
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
const std::vector<gtsam::Point2>& landmarks, double min_landmark_dist);
static Point2 randomBoundedPoint2(double boundary_size,
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 */
static gtsam::Point2 randomBoundedPoint2(double boundary_size,
const std::vector<gtsam::Point2>& landmarks,
static Point2 randomBoundedPoint2(double boundary_size,
const std::vector<Point2>& landmarks,
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
/** 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);
/** pick a random point in box defined by lower left and upper right corners */
static gtsam::Point2 randomBoundedPoint2(
const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner,
const std::vector<gtsam::Point2>& landmarks,
static Point2 randomBoundedPoint2(
const Point2& LL_corner, const Point2& UR_corner,
const std::vector<Point2>& landmarks,
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
/** 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 {
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
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 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_;
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,
const std::vector<SimWall2D> walls, gtsam::Sampler& angle_drift,
gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias) {
std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
const std::vector<SimWall2D> walls, Sampler& angle_drift,
Sampler& reflect_noise, const Rot2& bias) {
// calculate angle to change by
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 {
protected:
gtsam::Point2 a_, b_;
Point2 a_, b_;
public:
/** default constructor makes canonical wall */
SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {}
/** constructors using endpoints */
SimWall2D(const gtsam::Point2& a, const gtsam::Point2& b)
SimWall2D(const Point2& a, const Point2& b)
: a_(a), b_(b) {}
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;
/** access */
gtsam::Point2 a() const { return a_; }
gtsam::Point2 b() const { return b_; }
Point2 a() const { return a_; }
Point2 b() const { return b_; }
/** scales a wall to produce a new wall */
SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
/** geometry */
double length() const { return a_.dist(b_); }
gtsam::Point2 midpoint() const;
Point2 midpoint() const;
/**
* intersection check between two segments
* returns true if they intersect, with the intersection
* 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
*/
gtsam::Point2 norm() const;
Point2 norm() const;
/**
* 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
*/
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
* NOTE: samplers cannot be const
*/
std::pair<gtsam::Pose2, bool> moveWithBounce(const gtsam::Pose2& cur_pose, double step_size,
const std::vector<SimWall2D> walls, gtsam::Sampler& angle_drift,
gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias = gtsam::Rot2());
std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
const std::vector<SimWall2D> walls, Sampler& angle_drift,
Sampler& reflect_noise, const Rot2& bias = Rot2());
} // \namespace gtsam

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

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

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

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