diff --git a/.cproject b/.cproject index 1ce93a626..32cb2d277 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -309,6 +307,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +341,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +348,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +395,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +402,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +409,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +424,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -525,22 +517,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -557,6 +533,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -581,26 +573,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -685,34 +685,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -1063,7 +1055,6 @@ make - testGraph.run true false @@ -1071,7 +1062,6 @@ make - testJunctionTree.run true false @@ -1079,7 +1069,6 @@ make - testSymbolicBayesNetB.run true false @@ -1239,7 +1228,6 @@ make - testErrors.run true false @@ -1285,6 +1273,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1365,14 +1361,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1703,6 +1691,7 @@ make + testSimulated2DOriented.run true false @@ -1742,6 +1731,7 @@ make + testSimulated2D.run true false @@ -1749,6 +1739,7 @@ make + testSimulated3D.run true false @@ -1948,6 +1939,7 @@ make + tests/testGaussianISAM2 true false @@ -1969,6 +1961,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2170,7 +2258,6 @@ cpack - -G DEB true false @@ -2178,7 +2265,6 @@ cpack - -G RPM true false @@ -2186,7 +2272,6 @@ cpack - -G TGZ true false @@ -2194,7 +2279,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2320,98 +2404,42 @@ true true - + make - -j2 - testRot3.run + -j5 + check.geometry_unstable true true true - + make - -j2 - testRot2.run + -j5 + testSpirit.run true true true - + make - -j2 - testPose3.run + -j5 + testWrap.run true true true - + make - -j2 - timeRot3.run + -j5 + check.wrap true true true - + make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2455,38 +2483,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp new file mode 100644 index 000000000..ea1bf0b74 --- /dev/null +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -0,0 +1,76 @@ +/** + * @file BearingS2.cpp + * + * @date Jan 26, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +namespace gtsam { + +using namespace std; + +/* ************************************************************************* */ +void BearingS2::print(const std::string& s) const { + cout << s << " azimuth: " << azimuth_.theta() << " elevation: " << elevation_.theta() << endl; +} + +/* ************************************************************************* */ +bool BearingS2::equals(const BearingS2& x, double tol) const { + return azimuth_.equals(x.azimuth_, tol) && elevation_.equals(x.elevation_, tol); +} + +/* ************************************************************************* */ +BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { + // Cnb = DCMnb(Att); + Matrix Cnb = A.rotation().matrix().transpose(); + + // Cbc = [0,0,1;0,1,0;-1,0,0]; + Matrix Cbc = Matrix_(3,3, + 0.,0.,1., + 0.,1.,0., + -1.,0.,0.); + // p_rel_c = Cbc*Cnb*(PosObj - Pos); + Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector()); + + // FIXME: the matlab code checks for p_rel_c(0) greater than + + // azi = atan2(p_rel_c(2),p_rel_c(1)); + double azimuth = atan2(p_rel_c(1),p_rel_c(0)); + // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2)); + double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1))); + return BearingS2(azimuth, elevation); +} + +/* ************************************************************************* */ +BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) { + // Cnb = DCMnb(Att); + Matrix Cnb = A.rotation().matrix().transpose(); + + Vector p_rel_c = Cnb*(B.vector() - A.translation().vector()); + + // FIXME: the matlab code checks for p_rel_c(0) greater than + + // azi = atan2(p_rel_c(2),p_rel_c(1)); + double azimuth = atan2(p_rel_c(1),p_rel_c(0)); + // elev = atan2(p_rel_c(3),sqrt(p_rel_c(1)^2 + p_rel_c(2)^2)); + double elevation = atan2(p_rel_c(2),sqrt(p_rel_c(0) * p_rel_c(0) + p_rel_c(1) * p_rel_c(1))); + return BearingS2(azimuth, elevation); +} + +/* ************************************************************************* */ +BearingS2 BearingS2::retract(const Vector& v) const { + assert(v.size() == 2); + return BearingS2(azimuth_.retract(v.head(1)), elevation_.retract(v.tail(1))); +} + +/* ************************************************************************* */ +Vector BearingS2::localCoordinates(const BearingS2& x) const { + return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0), + elevation_.localCoordinates(x.elevation_)(0)); +} + +} // \namespace gtsam diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h new file mode 100644 index 000000000..a5bb1e214 --- /dev/null +++ b/gtsam_unstable/geometry/BearingS2.h @@ -0,0 +1,101 @@ +/** + * @file BearingS2.h + * + * @brief Manifold measurement between two points on a unit sphere + * + * @date Jan 26, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class BearingS2 : public DerivedValue { +protected: + Rot2 azimuth_, elevation_; + +public: + static const size_t dimension = 2; + + // constructors + + /** Default constructor - straight ahead */ + BearingS2() {} + + /** Build from components */ + BearingS2(double azimuth, double elevation) + : azimuth_(Rot2::fromAngle(azimuth)), elevation_(Rot2::fromAngle(elevation)) {} + + BearingS2(const Rot2& azimuth, const Rot2& elevation) + : azimuth_(azimuth), elevation_(elevation) {} + + // access + const Rot2& azimuth() const { return azimuth_; } + const Rot2& elevation() const { return elevation_; } + + /// @} + /// @name Measurements + /// @{ + + /** + * Observation function for downwards-facing camera + */ + // FIXME: will not work for TARGET = Point3 + template + static BearingS2 fromDownwardsObservation(const POSE& A, const TARGET& B) { + return fromDownwardsObservation(A.pose(), B.translation()); + } + + static BearingS2 fromDownwardsObservation(const Pose3& A, const Point3& B); + + /** Observation function with standard, forwards-facing camera */ + static BearingS2 fromForwardObservation(const Pose3& A, const Point3& B); + + /// @} + /// @name Testable + /// @{ + + /** print with optional string */ + void print(const std::string& s = "") const; + + /** assert equality up to a tolerance */ + bool equals(const BearingS2& x, double tol = 1e-9) const; + + /// @} + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 2 DOF - used to autodetect sizes + inline static size_t Dim() { return dimension; } + + /// Dimensionality of tangent space = 2 DOF + inline size_t dim() const { return dimension; } + + /// Retraction from R^2 to BearingS2 manifold neighborhood around current pose + /// Tangent space parameterization is [azimuth elevation] + BearingS2 retract(const Vector& v) const; + + /// Local coordinates of BearingS2 manifold neighborhood around current pose + Vector localCoordinates(const BearingS2& p2) const; + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(azimuth_); + ar & BOOST_SERIALIZATION_NVP(elevation_); + } + +}; + +} // \namespace gtsam diff --git a/gtsam_unstable/geometry/CMakeLists.txt b/gtsam_unstable/geometry/CMakeLists.txt index 21ba37d41..dee1db0b4 100644 --- a/gtsam_unstable/geometry/CMakeLists.txt +++ b/gtsam_unstable/geometry/CMakeLists.txt @@ -4,10 +4,11 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry) # Components to link tests in this subfolder against set(geometry_local_libs - #geometry_unstable # No sources currently, so no convenience lib + geometry_unstable geometry base ccolamd + linear ) set (geometry_full_libs diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp new file mode 100644 index 000000000..2ab3a6a9e --- /dev/null +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -0,0 +1,158 @@ +/** + * @file Pose3Upright.cpp + * + * @date Jan 24, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +Pose3Upright::Pose3Upright(const Rot2& bearing, const Point3& t) +: T_(bearing, Point2(t.x(), t.y())), z_(t.z()) +{ +} + +/* ************************************************************************* */ +Pose3Upright::Pose3Upright(double x, double y, double z, double theta) +: T_(x, y, theta), z_(z) +{ +} + +/* ************************************************************************* */ +Pose3Upright::Pose3Upright(const Pose2& pose, double z) +: T_(pose), z_(z) +{ +} + +/* ************************************************************************* */ +Pose3Upright::Pose3Upright(const Pose3& x) +: T_(x.x(), x.y(), x.rotation().yaw()), z_(x.z()) +{ +} + +/* ************************************************************************* */ +void Pose3Upright::print(const std::string& s) const { + cout << s << "(" << T_.x() << ", " << T_.y() << ", " << z_ << ", " << T_.theta() << ")" << endl; +} + +/* ************************************************************************* */ +bool Pose3Upright::equals(const Pose3Upright& x, double tol) const { + return T_.equals(x.T_, tol) && fabs(z_ - x.z_) < tol; +} + +/* ************************************************************************* */ +Point3 Pose3Upright::translation() const { + return Point3(x(), y(), z()); +} + +/* ************************************************************************* */ +Point2 Pose3Upright::translation2() const { + return T_.t(); +} + +/* ************************************************************************* */ +Rot2 Pose3Upright::rotation2() const { + return T_.r(); +} + +/* ************************************************************************* */ +Rot3 Pose3Upright::rotation() const { + return Rot3::yaw(theta()); +} + +/* ************************************************************************* */ +Pose2 Pose3Upright::pose2() const { + return T_; +} + +/* ************************************************************************* */ +Pose3 Pose3Upright::pose() const { + return Pose3(rotation(), translation()); +} + +/* ************************************************************************* */ +Pose3Upright Pose3Upright::inverse(boost::optional H1) const { + Pose3Upright result(T_.inverse(H1), -z_); + if (H1) { + Matrix H1_ = -eye(4,4); + H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); + H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); + *H1 = H1_; + } + return result; +} + +/* ************************************************************************* */ +Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, + boost::optional H1, boost::optional H2) const { + if (!H1 && !H2) + return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); + Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_); + if (H1) { + Matrix H1_ = eye(4,4); + H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); + H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); + *H1 = H1_; + } + if (H2) *H2 = eye(4,4); + return result; +} + +/* ************************************************************************* */ +Pose3Upright Pose3Upright::between(const Pose3Upright& p2, + boost::optional H1, boost::optional H2) const { + if (!H1 && !H2) + return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); + Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_); + if (H1) { + Matrix H1_ = -eye(4,4); + H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); + H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); + *H1 = H1_; + } + if (H2) *H2 = eye(4,4); + return result; +} + +/* ************************************************************************* */ +Pose3Upright Pose3Upright::retract(const Vector& v) const { + assert(v.size() == 4); + Vector v1(3); v1 << v(0), v(1), v(3); + return Pose3Upright(T_.retract(v1), z_ + v(2)); +} + +/* ************************************************************************* */ +Vector Pose3Upright::localCoordinates(const Pose3Upright& p2) const { + Vector pose2 = T_.localCoordinates(p2.pose2()); + Vector result(4); + result << pose2(0), pose2(1), p2.z() - z_, pose2(2); + return result; +} + +/* ************************************************************************* */ +Pose3Upright Pose3Upright::Expmap(const Vector& xi) { + assert(xi.size() == 4); + Vector v1(3); v1 << xi(0), xi(1), xi(3); + return Pose3Upright(Pose2::Expmap(v1), xi(2)); +} + +/* ************************************************************************* */ +Vector Pose3Upright::Logmap(const Pose3Upright& p) { + Vector pose2 = Pose2::Logmap(p.pose2()); + Vector result(4); + result << pose2(0), pose2(1), p.z(), pose2(2); + return result; +} +/* ************************************************************************* */ + +} // \namespace gtsam + + + diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h new file mode 100644 index 000000000..d1bbab87c --- /dev/null +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -0,0 +1,144 @@ +/** + * @file Pose3Upright.h + * + * @brief Variation of a Pose3 in which the rotation is constained to purely yaw + * This state is essentially a Pose2 with a z component, with conversions to + * higher and lower dimensional states. + * + * @date Jan 24, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * A 3D Pose with fixed pitch and roll + * @ingroup geometry + * \nosubgrouping + */ +class Pose3Upright : public DerivedValue { +public: + static const size_t dimension = 4; + +protected: + + Pose2 T_; + double z_; + +public: + /// @name Standard Constructors + /// @{ + + /// Default constructor initializes at origin + Pose3Upright() : z_(0.0) {} + + /// Copy constructor + Pose3Upright(const Pose3Upright& x) : T_(x.T_), z_(x.z_) {} + Pose3Upright(const Rot2& bearing, const Point3& t); + Pose3Upright(double x, double y, double z, double theta); + Pose3Upright(const Pose2& pose, double z); + + /// Down-converts from a full Pose3 + Pose3Upright(const Pose3& fullpose); + + /// @} + /// @name Testable + /// @{ + + /** print with optional string */ + void print(const std::string& s = "") const; + + /** assert equality up to a tolerance */ + bool equals(const Pose3Upright& pose, double tol = 1e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + double x() const { return T_.x(); } + double y() const { return T_.y(); } + double z() const { return z_; } + double theta() const { return T_.theta(); } + + Point2 translation2() const; + Point3 translation() const; + Rot2 rotation2() const; + Rot3 rotation() const; + Pose2 pose2() const; + Pose3 pose() const; + + /// @} + /// @name Manifold + /// @{ + + /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes + inline static size_t Dim() { return dimension; } + + /// Dimensionality of tangent space = 4 DOF + inline size_t dim() const { return dimension; } + + /// Retraction from R^4 to Pose3Upright manifold neighborhood around current pose + /// Tangent space parameterization is [x y z theta] + Pose3Upright retract(const Vector& v) const; + + /// Local 3D coordinates of Pose3Upright manifold neighborhood around current pose + Vector localCoordinates(const Pose3Upright& p2) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Pose3Upright identity() { return Pose3Upright(); } + + /// inverse transformation with derivatives + Pose3Upright inverse(boost::optional H1=boost::none) const; + + ///compose this transformation onto another (first *this and then p2) + Pose3Upright compose(const Pose3Upright& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// compose syntactic sugar + inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); } + + /** + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives + */ + Pose3Upright between(const Pose3Upright& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map at identity - create a rotation from canonical coordinates + static Pose3Upright Expmap(const Vector& xi); + + /// Log map at identity - return the canonical coordinates of this rotation + static Vector Logmap(const Pose3Upright& p); + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(T_); + ar & BOOST_SERIALIZATION_NVP(z_); + } + +}; // \class Pose3Upright + +} // \namespace gtsam diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp new file mode 100644 index 000000000..8667d0807 --- /dev/null +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -0,0 +1,331 @@ +/** + * @file SimPolygon2D.cpp + * @author Alex Cunningham + */ + +#include +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +using namespace std; +using namespace gtsam; + +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 SimPolygon2D::walls() const { + vector result; + for (size_t i=0; i edges = walls(); + bool initialized = false; + bool lastSide = false; + BOOST_FOREACH(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 { + BOOST_FOREACH(const Point2& a, landmarks_) + if (p.contains(a)) + return true; + BOOST_FOREACH(const Point2& a, p.landmarks_) + if (contains(a)) + return true; + return false; +} + +/* ***************************************************************** */ +bool SimPolygon2D::anyContains(const Point2& p, const vector& obstacles) { + BOOST_FOREACH(const SimPolygon2D& poly, obstacles) + if (poly.contains(p)) + return true; + return false; +} + +/* ************************************************************************* */ +bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector& obstacles) { + BOOST_FOREACH(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& existing_polys) { + // get the current set of landmarks + std::vector 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)); + + BOOST_FOREACH(const SimPolygon2D& poly, existing_polys) + lms.insert(lms.begin(), poly.vertices().begin(), poly.vertices().end()); + + for (size_t i=0; i 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)) && + test_tri.landmark(1).dist(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& existing_polys) { + // get the current set of landmarks + std::vector 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)); + BOOST_FOREACH(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 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 norm_dist(mu, sigma); + boost::variate_generator > gen_d(rng, norm_dist); + double d = -10.0; + for (size_t i=0; i 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& obstacles) { + for (size_t i=0; i& landmarks, double min_landmark_dist) { + for (size_t i=0; i& landmarks, + const vector& obstacles, double min_landmark_dist) { + for (size_t i=0; i& landmarks, + const std::vector& 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& 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& S, + const Point2& p, double threshold) { + BOOST_FOREACH(const gtsam::Point2& Sp, S) + if (Sp.dist(p) < threshold) + return true; + return false; +} + +} //\namespace gtsam + diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h new file mode 100644 index 000000000..d8e56e998 --- /dev/null +++ b/gtsam_unstable/geometry/SimPolygon2D.h @@ -0,0 +1,136 @@ +/** + * @file SimPolygon2D.h + * @brief Polygons for simulation use + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * General polygon class for convex polygons + */ +class SimPolygon2D { +protected: + std::vector landmarks_; + static boost::minstd_rand rng; + +public: + + /** Don't use this constructor, use a named one instead */ + SimPolygon2D() {} + + /** seed the random number generator - only needs to be done once */ + 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); + + /** + * 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); + + /** + * Randomly generate a triangle that does not conflict with others + * Uniformly distributed over box area, with normally distributed lengths of edges + * THROWS: std::runtime_error if can't find a position + */ + 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& existing_polys); + + /** + * Randomly generate a rectangle that does not conflict with others + * Uniformly distributed over box area, with normally distributed lengths of edges + * THROWS: std::runtime_error if can't find a position + */ + 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& existing_polys); + + // access to underlying points + const gtsam::Point2& landmark(size_t i) const { return landmarks_[i]; } + size_t size() const { return landmarks_.size(); } + const std::vector& vertices() const { return landmarks_; } + + // testable requirements + bool equals(const SimPolygon2D& p, double tol=1e-5) const; + void print(const std::string& s="") const; + + /** + * Get a set of walls along the edges + */ + std::vector walls() const; + + /** + * Core function for randomly generating scenarios. + * Polygons are closed, convex shapes. + * @return true if the given point is contained by this polygon + */ + bool contains(const gtsam::Point2& p) const; + + /** + * Checks two polygons to determine if they overlap + * @return true iff at least one vertex of one polygon is contained in the other + */ + 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& obstacles); + + /** returns true iff polygon p overlaps with any of a set of polygons */ + static bool anyOverlaps(const SimPolygon2D& p, const std::vector& obstacles); + + /** returns true iff p is inside a square centered at zero with side s */ + static bool insideBox(double s, const gtsam::Point2& p); + + /** returns true iff p is within threshold of any point in S */ + static bool nearExisting(const std::vector& S, + const gtsam::Point2& p, double threshold); + + /** pick a random point uniformly over a box of side s */ + static gtsam::Point2 randomPoint2(double s); + + /** randomly generate a Rot2 with a uniform distribution over theta */ + static gtsam::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& 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& landmarks, + const std::vector& obstacles, double min_landmark_dist); + + /** pick a random point that only avoid obstacles */ + static gtsam::Point2 randomBoundedPoint2(double boundary_size, + const std::vector& 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& landmarks, + const std::vector& 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& obstacles); + +}; + +typedef std::vector SimPolygon2DVector; + +} //\namespace gtsam + +namespace gtsam { +typedef std::vector Point2Vector; +} // \namespace gtsam diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp new file mode 100644 index 000000000..d086aa735 --- /dev/null +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -0,0 +1,174 @@ +/** + * @file SimWall2D.cpp + * @author Alex Cunningham + */ + +#include + +#include +#include + +namespace gtsam { + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +void SimWall2D::print(const std::string& s) const { + std::cout << "SimWall2D " << s << ":" << std::endl; + a_.print(" a"); + b_.print(" b"); +} + +/* ************************************************************************* */ +bool SimWall2D::equals(const SimWall2D& other, double tol) const { + return a_.equals(other.a_, tol) && b_.equals(other.b_, tol); +} + +/* ************************************************************************* */ +bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) const { + const bool debug = false; + + const SimWall2D& A = *this; + + // translate so A.a is at the origin, then rotate so that A.b is along x axis + Pose2 transform(Rot2::relativeBearing(A.b() - A.a()), A.a()); + + // normalized points, Aa at origin, Ab at (length, 0.0) + double len = A.length(); + if (debug) cout << "len: " << len << endl; + Point2 Ba = transform.transform_to(B.a()), + Bb = transform.transform_to(B.b()); + if (debug) Ba.print("Ba"); + if (debug) Bb.print("Bb"); + + // check sides of line + if (Ba.y() * Bb.y() > 0.0 || + (Ba.x() * Bb.x() > 0.0 && Ba.x() < 0.0) || + (Ba.x() > len && Bb.x() > len)) { + if (debug) cout << "Failed first check" << endl; + return false; + } + + // check conditions for exactly on the same line + if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) { + if (pt) *pt = transform.transform_from(Ba); + if (debug) cout << "Ba on the line" << endl; + return true; + } else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) { + if (pt) *pt = transform.transform_from(Bb); + if (debug) cout << "Bb on the line" << endl; + return true; + } + + // handle vertical case to avoid calculating slope + if (fabs(Ba.x() - Bb.x()) < 1e-5) { + if (debug) cout << "vertical line" << endl; + if (Ba.x() < len && Ba.x() > 0.0) { + if (pt) *pt = transform.transform_from(Point2(Ba.x(), 0.0)); + if (debug) cout << " within range" << endl; + return true; + } else { + if (debug) cout << " not within range" << endl; + return false; + } + } + + // find lower point by y + Point2 low, high; + if (Ba.y() > Bb.y()) { + high = Ba; + low = Bb; + } else { + high = Bb; + low = Ba; + } + if (debug) high.print("high"); + if (debug) low.print("low"); + + // find x-intercept + double slope = (high.y() - low.y())/(high.x() - low.x()); + if (debug) cout << "slope " << slope << endl; + double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope; + if (debug) cout << "xintercept " << xint << endl; + if (xint > 0.0 && xint < len) { + if (pt) *pt = transform.transform_from(Point2(xint, 0.0)); + return true; + } else { + if (debug) cout << "xintercept out of range" << endl; + return false; + } +} + +/* ************************************************************************* */ +gtsam::Point2 SimWall2D::midpoint() const { + Point2 vec = b_ - a_; + return a_ + vec * 0.5 * vec.norm(); +} + +/* ************************************************************************* */ +Point2 SimWall2D::norm() const { + Point2 vec = b_ - a_; + return Point2(vec.y(), -vec.x()); +} + +/* ************************************************************************* */ +Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const { + // translate to put the intersection at the origin and the wall along the x axis + Rot2 wallAngle = Rot2::relativeBearing(b_ - a_); + Pose2 transform(wallAngle, intersection); + Point2 t_init = transform.transform_to(init); + Point2 t_goal(-t_init.x(), t_init.y()); + return Rot2::relativeBearing(wallAngle.rotate(t_goal)); +} + +/* ***************************************************************** */ +std::pair moveWithBounce(const gtsam::Pose2& cur_pose, double step_size, + const std::vector walls, gtsam::Sampler& angle_drift, + gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias) { + + // calculate angle to change by + Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); + Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0))); + + // create a segment to use for intersection checking + // find the closest intersection + SimWall2D traj(test_pose.t(), cur_pose.t()); + bool collision = false; Point2 intersection(1e+10, 1e+10); + SimWall2D closest_wall; + BOOST_FOREACH(const SimWall2D& wall, walls) { + Point2 cur_intersection; + if (wall.intersects(traj,cur_intersection)) { + collision = true; + if (cur_pose.t().dist(cur_intersection) < cur_pose.t().dist(intersection)) { + intersection = cur_intersection; + closest_wall = wall; + } + } + } + + // reflect off of wall with some noise + Pose2 pose(test_pose); + if (collision) { + + // make sure norm is on the robot's side + Point2 norm = closest_wall.norm(); + norm = norm / norm.norm(); + + // Simple check to make sure norm is on side closest robot + if (cur_pose.t().dist(intersection + norm) > cur_pose.t().dist(intersection - norm)) + norm = norm.inverse(); + + // using the reflection + const double inside_bias = 0.05; + pose = Pose2(closest_wall.reflection(cur_pose.t(), intersection), intersection + inside_bias * norm); + + // perturb the rotation for better exploration + pose = pose.retract(reflect_noise.sample()); + } + + return make_pair(pose, collision); +} +/* ***************************************************************** */ + +} // \namespace gtsam diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h new file mode 100644 index 000000000..f11e8dfd2 --- /dev/null +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -0,0 +1,85 @@ +/** + * @file SimWall2D.h + * @brief Implementation of walls for use with simulators + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /** + * General Wall class for walls defined around unordered endpoints + * Primarily to handle ray intersections + */ + class SimWall2D { + protected: + gtsam::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) + : a_(a), b_(b) {} + + SimWall2D(double ax, double ay, double bx, double by) + : a_(ax, ay), b_(bx, by) {} + + /** required by testable */ + void print(const std::string& s="") const; + bool equals(const SimWall2D& other, double tol=1e-9) const; + + /** access */ + gtsam::Point2 a() const { return a_; } + gtsam::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; + + /** + * 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 pt=boost::none) const; + + /** + * norm is a 2D point representing the norm of the wall + */ + gtsam::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; + + }; + + typedef std::vector SimWall2DVector; + + /** + * Calculates the next pose in a trajectory constrained by walls, with noise on + * angular drift and reflection noise + * @param cur_pose is the pose of the robot + * @param step_size is the size of the forward step the robot tries to take + * @param walls is a set of walls to use for bouncing + * @param angle_drift is a sampler for angle drift (dim=1) + * @param reflect_noise is a sampler for scatter after hitting a wall (dim=3) + * @return the next pose for the robot + * NOTE: samplers cannot be const + */ + std::pair moveWithBounce(const gtsam::Pose2& cur_pose, double step_size, + const std::vector walls, gtsam::Sampler& angle_drift, + gtsam::Sampler& reflect_noise, const gtsam::Rot2& bias = gtsam::Rot2()); + +} // \namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testBearingS2.cpp b/gtsam_unstable/geometry/tests/testBearingS2.cpp new file mode 100644 index 000000000..48d6e29af --- /dev/null +++ b/gtsam_unstable/geometry/tests/testBearingS2.cpp @@ -0,0 +1,60 @@ +/** + * @file testBearingS2.cpp + * + * @brief Tests for a bearing measurement on S2 for 3D bearing measurements + * + * @date Jan 26, 2012 + * @author Alex Cunningham + */ + +#include + +#include + +#include + +using namespace gtsam; + +static const double tol=1e-5; + +/* ************************************************************************* */ +TEST( testBearingS2, basics ) { + BearingS2 origin; + EXPECT(assert_equal(Rot2(), origin.elevation(), tol)); + EXPECT(assert_equal(Rot2(), origin.azimuth(), tol)); + + double expAzi = 0.2, expEle = 0.3; + BearingS2 actual1(expAzi, expEle); + EXPECT(assert_equal(Rot2::fromAngle(expEle), actual1.elevation(), tol)); + EXPECT(assert_equal(Rot2::fromAngle(expAzi), actual1.azimuth(), tol)); +} + +/* ************************************************************************* */ +TEST( testBearingS2, equals ) { + BearingS2 origin, b1(0.2, 0.3), b2(b1), b3(0.1, 0.3), b4(0.2, 0.5); + EXPECT(assert_equal(origin, origin, tol)); + EXPECT(assert_equal(b1, b1, tol)); + EXPECT(assert_equal(b1, b2, tol)); + EXPECT(assert_equal(b2, b1, tol)); + + EXPECT(assert_inequal(b1, b3, tol)); + EXPECT(assert_inequal(b3, b1, tol)); + EXPECT(assert_inequal(b1, b4, tol)); + EXPECT(assert_inequal(b4, b1, tol)); +} + +/* ************************************************************************* */ +TEST( testBearingS2, manifold ) { + BearingS2 origin, b1(0.2, 0.3); + EXPECT_LONGS_EQUAL(2, origin.dim()); + + EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol)); + EXPECT(assert_equal(origin, origin.retract(zero(2)), tol)); + + EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol)); + EXPECT(assert_equal(b1, b1.retract(zero(2)), tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp new file mode 100644 index 000000000..806f7dc16 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -0,0 +1,137 @@ +/** + * @file testPose3Upright.cpp + * + * @date Jan 24, 2012 + * @author Alex Cunningham + */ + +#include + +#include +#include + +#include + +using namespace gtsam; + +static const double tol = 1e-5; + +/* ************************************************************************* */ +TEST( testPose3Upright, basics ) { + Pose3Upright origin; + EXPECT_DOUBLES_EQUAL(0.0, origin.x(), tol); + EXPECT_DOUBLES_EQUAL(0.0, origin.y(), tol); + EXPECT_DOUBLES_EQUAL(0.0, origin.z(), tol); + EXPECT_DOUBLES_EQUAL(0.0, origin.theta(), tol); + + Pose3Upright actual1(Rot2::fromAngle(0.1), Point3(1.0, 2.0, 3.0)); + EXPECT_DOUBLES_EQUAL(1.0, actual1.x(), tol); + EXPECT_DOUBLES_EQUAL(2.0, actual1.y(), tol); + EXPECT_DOUBLES_EQUAL(3.0, actual1.z(), tol); + EXPECT_DOUBLES_EQUAL(0.1, actual1.theta(), tol); + + Pose3Upright actual2(1.0, 2.0, 3.0, 0.1); + EXPECT_DOUBLES_EQUAL(1.0, actual2.x(), tol); + EXPECT_DOUBLES_EQUAL(2.0, actual2.y(), tol); + EXPECT_DOUBLES_EQUAL(3.0, actual2.z(), tol); + EXPECT_DOUBLES_EQUAL(0.1, actual2.theta(), tol); +} + +/* ************************************************************************* */ +TEST( testPose3Upright, equals ) { + Pose3Upright origin, actual1(1.0, 2.0, 3.0, 0.1), + actual2(1.0, 2.0, 3.0, 0.1), actual3(4.0,-7.0, 3.0, 0.3); + EXPECT(assert_equal(origin, origin, tol)); + EXPECT(assert_equal(actual1, actual1, tol)); + EXPECT(assert_equal(actual1, actual2, tol)); + EXPECT(assert_equal(actual2, actual1, tol)); + + EXPECT(assert_inequal(actual1, actual3, tol)); + EXPECT(assert_inequal(actual3, actual1, tol)); + EXPECT(assert_inequal(actual1, origin, tol)); + EXPECT(assert_inequal(origin, actual1, tol)); +} + +/* ************************************************************************* */ +TEST( testPose3Upright, conversions ) { + Pose3Upright pose(1.0, 2.0, 3.0, 0.1); + EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol)); + EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol)); + EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol)); + EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol)); + EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol)); + EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); +} + +/* ************************************************************************* */ +TEST( testPose3Upright, manifold ) { + Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0); + EXPECT_LONGS_EQUAL(4, origin.dim()); + + EXPECT(assert_equal(origin, origin.retract(zero(4)), tol)); + EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); + EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); + + Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; + EXPECT(assert_equal(x2, x1.retract(delta12), tol)); + EXPECT(assert_equal(x1, x2.retract(delta21), tol)); + + EXPECT(assert_equal(delta12, x1.localCoordinates(x2), tol)); + EXPECT(assert_equal(delta21, x2.localCoordinates(x1), tol)); +} + +/* ************************************************************************* */ +TEST( testPose3Upright, lie ) { + Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1); + EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol)); + EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol)); + + EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol)); +} + +/* ************************************************************************* */ +Pose3Upright between_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.between(x2); } +TEST( testPose3Upright, between ) { + Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3); + Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z()); + EXPECT(assert_equal(expected, x1.between(x2), tol)); + + Matrix actualH1, actualH2, numericH1, numericH2; + x1.between(x2, actualH1, actualH2); + numericH1 = numericalDerivative21(between_proxy, x1, x2, 1e-5); + numericH2 = numericalDerivative22(between_proxy, x1, x2, 1e-5); + EXPECT(assert_equal(numericH1, actualH1, tol)); + EXPECT(assert_equal(numericH2, actualH2, tol)); +} + +/* ************************************************************************* */ +Pose3Upright compose_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.compose(x2); } +TEST( testPose3Upright, compose ) { + Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3); + Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z()); + EXPECT(assert_equal(x2, x1.compose(expected), tol)); + + Matrix actualH1, actualH2, numericH1, numericH2; + x1.compose(expected, actualH1, actualH2); + numericH1 = numericalDerivative21(compose_proxy, x1, expected, 1e-5); + numericH2 = numericalDerivative22(compose_proxy, x1, expected, 1e-5); + EXPECT(assert_equal(numericH1, actualH1, tol)); + EXPECT(assert_equal(numericH2, actualH2, tol)); +} + +/* ************************************************************************* */ +Pose3Upright inverse_proxy(const Pose3Upright& x1) { return x1.inverse(); } +TEST( testPose3Upright, inverse ) { + Pose3Upright x1(1.0, 2.0, 3.0, 0.1); + Pose3Upright expected(x1.pose2().inverse(), - x1.z()); + EXPECT(assert_equal(expected, x1.inverse(), tol)); + + Matrix actualH1, numericH1; + x1.inverse(actualH1); + numericH1 = numericalDerivative11(inverse_proxy, x1, 1e-5); + EXPECT(assert_equal(numericH1, actualH1, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp new file mode 100644 index 000000000..9a0ba204a --- /dev/null +++ b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp @@ -0,0 +1,139 @@ +/** + * @file testSimPolygon + * @author Alex Cunningham + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam; + +const double tol=1e-5; + +/* ************************************************************************* */ +TEST(testPolygon, triangle_basic) { + + // create a triangle from points, extract landmarks/walls, check occupancy + Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0); + + // construct and extract data + SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC); + LONGS_EQUAL(3, actTriangle.size()); + EXPECT(assert_equal(pA, actTriangle.landmark(0))); + EXPECT(assert_equal(pB, actTriangle.landmark(1))); + EXPECT(assert_equal(pC, actTriangle.landmark(2))); + + // get walls out + vector actWalls = actTriangle.walls(); + vector expWalls; + expWalls.push_back(SimWall2D(pA, pB)); + expWalls.push_back(SimWall2D(pB, pC)); + expWalls.push_back(SimWall2D(pC, pA)); + EXPECT(assert_container_equal(expWalls, actWalls, tol)); + + // check occupancy - used in sampling more points + // treat as closed polygon - points along edges also count + EXPECT(actTriangle.contains(Point2(0.25, 0.5))); + EXPECT(actTriangle.contains(pA)); + EXPECT(actTriangle.contains(pB)); + EXPECT(actTriangle.contains(pC)); + EXPECT(actTriangle.contains(Point2(1.0, 0.0))); + + EXPECT(!actTriangle.contains(Point2(1.0, 1.0))); + EXPECT(!actTriangle.contains(Point2(-1.0, 1.0))); +} + +/* ************************************************************************* */ +TEST(testPolygon, rectangle_basic) { + + // creates an axis-aligned rectangle given a lower left corner and a height and width + double height = 3.0, width = 2.0; + Point2 pA(1.0, 0.0), pB(3.0, 0.0), pC(3.0, 3.0), pD(1.0, 3.0); + + // construct and extract data + SimPolygon2D actRectangle = SimPolygon2D::createRectangle(pA, height, width); + LONGS_EQUAL(4, actRectangle.size()); + EXPECT(assert_equal(pA, actRectangle.landmark(0))); + EXPECT(assert_equal(pB, actRectangle.landmark(1))); + EXPECT(assert_equal(pC, actRectangle.landmark(2))); + EXPECT(assert_equal(pD, actRectangle.landmark(3))); + + // get walls out + vector actWalls = actRectangle.walls(); + vector expWalls; + expWalls.push_back(SimWall2D(pA, pB)); + expWalls.push_back(SimWall2D(pB, pC)); + expWalls.push_back(SimWall2D(pC, pD)); + expWalls.push_back(SimWall2D(pD, pA)); + EXPECT(assert_container_equal(expWalls, actWalls, tol)); + + // check occupancy - used in sampling more points + // treat as closed polygon - points along edges also count + EXPECT(actRectangle.contains(Point2(2.0, 1.0))); + EXPECT(actRectangle.contains(pA)); + EXPECT(actRectangle.contains(pB)); + EXPECT(actRectangle.contains(pC)); + EXPECT(actRectangle.contains(pD)); + EXPECT(actRectangle.contains(Point2(1.0, 0.0))); + + EXPECT(!actRectangle.contains(Point2(0.9, 0.5))); + EXPECT(!actRectangle.contains(Point2(-1.0, 1.0))); +} + +/* ************************************************************************* */ +TEST(testPolygon, triangle_generator) { + // generate random triangles in a bounded region with no overlap + double side_len = 10.0; // box of length 10, centered on origin + double mean_side_len = 2.0; // mean length of sides + double sigma_side_len = 0.5; // stddev for length of sides + double min_vertex_dist = 0.4; // minimum allowable distance between vertices + double min_side_len = 0.1; + + // initialize the random number generator for consistency + SimPolygon2D::seedGenerator(42u); + + vector existing_polys; + + SimPolygon2D actual = SimPolygon2D::randomTriangle(side_len, mean_side_len, sigma_side_len, + min_vertex_dist, min_side_len, existing_polys); + + // use a rectangle to check that it is within boundaries + SimPolygon2D bounding_rect = SimPolygon2D::createRectangle(Point2(-5.0,-5.0), side_len, side_len); + + EXPECT(bounding_rect.contains(actual.landmark(0))); + EXPECT(bounding_rect.contains(actual.landmark(1))); + EXPECT(bounding_rect.contains(actual.landmark(2))); +} + +/* ************************************************************************* */ +TEST(testPolygon, rectangle_generator) { + // generate random rectangles in a bounded region with no overlap + double side_len = 10.0; // box of length 10, centered on origin + double mean_side_len = 2.0; // mean length of sides + double sigma_side_len = 0.5; // stddev for length of sides + double min_vertex_dist = 0.4; // minimum allowable distance between vertices + double min_side_len = 0.1; + + // initialize the random number generator for consistency + SimPolygon2D::seedGenerator(42u); + + vector existing_polys; + + SimPolygon2D actual = SimPolygon2D::randomRectangle(side_len, mean_side_len, sigma_side_len, + min_vertex_dist, min_side_len, existing_polys); + + // use a rectangle to check that it is within boundaries + SimPolygon2D bounding_rect = SimPolygon2D::createRectangle(Point2(-5.0,-5.0), side_len, side_len); + + EXPECT(bounding_rect.contains(actual.landmark(0))); + EXPECT(bounding_rect.contains(actual.landmark(1))); + EXPECT(bounding_rect.contains(actual.landmark(2))); + EXPECT(bounding_rect.contains(actual.landmark(3))); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testSimWall2D.cpp b/gtsam_unstable/geometry/tests/testSimWall2D.cpp new file mode 100644 index 000000000..36eaa35e5 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testSimWall2D.cpp @@ -0,0 +1,66 @@ +/** + * @file testSimWall2D2D + * @author Alex Cunningham + */ + +#include +#include + +#include + +using namespace gtsam; +using namespace gtsam; + +const double tol = 1e-5; + +/* ************************************************************************* */ +TEST(testSimWall2D2D, construction ) { + Point2 a(1.0, 0.0), b(1.0, 2.0); + SimWall2D wall1(a, b), wall2(a.x(), a.y(), b.x(), b.y()); + EXPECT(assert_equal(a, wall1.a(), tol)); + EXPECT(assert_equal(a, wall2.a(), tol)); + EXPECT(assert_equal(b, wall1.b(), tol)); + EXPECT(assert_equal(b, wall2.b(), tol)); +} + +/* ************************************************************************* */ +TEST(testSimWall2D2D, equals ) { + Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3; + SimWall2D w1(p1, p2), w2(p1, p3); + EXPECT(assert_equal(w1, w1)); + EXPECT(assert_inequal(w1, w2)); + EXPECT(assert_inequal(w2, w1)); +} + +/* ************************************************************************* */ +TEST(testSimWall2D2D, intersection1 ) { + SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0); + Point2 pt; + EXPECT(w1.intersects(w2)); + EXPECT(w2.intersects(w1)); + w1.intersects(w2, pt); + EXPECT(assert_equal(Point2(4.0, 2.0), pt, tol)); +} + +/* ************************************************************************* */ +TEST(testSimWall2D2D, intersection2 ) { + SimWall2D traj(7.07107, 7.07107, 0, 0); + SimWall2D wall(1.5, 3, 1.5, 1); + EXPECT(wall.intersects(traj)); + EXPECT(traj.intersects(wall)); +} + +/* ************************************************************************* */ +TEST(testSimWall2D2D, reflection1 ) { + SimWall2D wall1(1.0, 1.0, 7.0, 1.0), wall2(7.0, 1.0, 1.0, 1.0); + Point2 init(2.0, 3.0), intersection(4.0, 1.0); + Rot2 actual1 = wall1.reflection(init, intersection); + Rot2 actual2 = wall2.reflection(init, intersection); + Rot2 expected = Rot2::fromDegrees(45); + EXPECT(assert_equal(expected, actual1, tol)); + EXPECT(assert_equal(expected, actual2, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 9100413fc..79e748bb0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -4,9 +4,11 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; +virtual class gtsam::Point2; +virtual class gtsam::Rot2; +virtual class gtsam::Pose2; virtual class gtsam::Point3; virtual class gtsam::Rot3; -virtual class gtsam::Pose2; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; virtual class gtsam::NonlinearFactor; @@ -77,6 +79,62 @@ virtual class PoseRTV : gtsam::Value { Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const; }; +#include +virtual class Pose3Upright : gtsam::Value { + Pose3Upright(); + Pose3Upright(const gtsam::Pose3Upright& x); + Pose3Upright(const gtsam::Rot2& bearing, const gtsam::Point3& t); + Pose3Upright(double x, double y, double z, double theta); + Pose3Upright(const gtsam::Pose2& pose, double z); + + void print(string s) const; + bool equals(const gtsam::Pose3Upright& pose, double tol) const; + + double x() const; + double y() const; + double z() const; + double theta() const; + + gtsam::Point2 translation2() const; + gtsam::Point3 translation() const; + gtsam::Rot2 rotation2() const; + gtsam::Rot3 rotation() const; + gtsam::Pose2 pose2() const; + gtsam::Pose3 pose() const; + + size_t dim() const; + gtsam::Pose3Upright retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3Upright& p2) const; + + static gtsam::Pose3Upright identity(); + gtsam::Pose3Upright inverse() const; + gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const; + gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const; + + static gtsam::Pose3Upright Expmap(Vector xi); + static Vector Logmap(const gtsam::Pose3Upright& p); +}; // \class Pose3Upright + +#include +virtual class BearingS2 : gtsam::Value { + BearingS2(); + BearingS2(double azimuth, double elevation); + BearingS2(const gtsam::Rot2& azimuth, const gtsam::Rot2& elevation); + + gtsam::Rot2 azimuth() const; + gtsam::Rot2 elevation() const; + + static gtsam::BearingS2 fromDownwardsObservation(const gtsam::Pose3& A, const gtsam::Point3& B); + static gtsam::BearingS2 fromForwardObservation(const gtsam::Pose3& A, const gtsam::Point3& B); + + void print(string s) const; + bool equals(const gtsam::BearingS2& x, double tol) const; + + size_t dim() const; + gtsam::BearingS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::BearingS2& p2) const; +}; + // Nonlinear factors from gtsam, for our Value types #include