diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 42d5e7517..8433f19b0 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -64,7 +64,7 @@ #cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4 // Publish flag about Eigen typedef -#cmakedefine GTSAM_USE_VECTOR3_POINTS +#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 368ae6c98..2ee2e5581 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -106,7 +106,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (uncalibrate(pn).distance(pi) <= tol) + if (distance(uncalibrate(pn), pi) <= tol) break; const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; const double rr = xx + yy; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 12060c12d..232f442f6 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -144,7 +144,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (uncalibrate(pn).distance(pi) <= tol) break; + if (distance(uncalibrate(pn), pi) <= tol) break; const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 2d27b4dc7..86e01d71e 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -24,7 +24,7 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { +Matrix26 PinholeBase::Dpose(const Vector2& pn, double d) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); double uv = u * v, uu = u * u, vv = v * v; @@ -34,7 +34,7 @@ Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { } /* ************************************************************************* */ -Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) { +Matrix23 PinholeBase::Dpoint(const Vector2& pn, double d, const Matrix3& Rt) { // optimized version of derivatives, see CalibratedCamera.nb const double u = pn.x(), v = pn.y(); Matrix23 Dpn_point; @@ -85,20 +85,20 @@ const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { } /* ************************************************************************* */ -Point2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { +Vector2 PinholeBase::Project(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { double d = 1.0 / pc.z(); const double u = pc.x() * d, v = pc.y() * d; if (Dpoint) *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); + return Vector2(u, v); } /* ************************************************************************* */ -Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { +Vector2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { if (Dpoint) { Matrix32 Dpoint3_pc; Matrix23 Duv_point3; - Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); + Vector2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3); *Dpoint = Duv_point3 * Dpoint3_pc; return uv; } else @@ -106,14 +106,14 @@ Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { } /* ************************************************************************* */ -pair PinholeBase::projectSafe(const Point3& pw) const { +pair PinholeBase::projectSafe(const Point3& pw) const { const Point3 pc = pose().transform_to(pw); - const Point2 pn = Project(pc); + const Vector2 pn = Project(pc); return make_pair(pn, pc.z() > 0); } /* ************************************************************************* */ -Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, +Vector2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { Matrix3 Rt; // calculated by transform_to if needed @@ -122,7 +122,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, if (q.z() <= 0) throw CheiralityException(); #endif - const Point2 pn = Project(q); + const Vector2 pn = Project(q); if (Dpose || Dpoint) { const double d = 1.0 / q.z(); @@ -135,7 +135,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, } /* ************************************************************************* */ -Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, +Vector2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 2> Dpoint) const { // world to camera coordinate @@ -146,7 +146,7 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, // camera to normalized image coordinate Matrix2 Dpn_pc; - const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); + const Vector2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); // chain the Jacobian matrices if (Dpose) { @@ -161,7 +161,7 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, return pn; } /* ************************************************************************* */ -Point3 PinholeBase::backproject_from_camera(const Point2& p, +Point3 PinholeBase::backproject_from_camera(const Vector2& p, const double depth) { return Point3(p.x() * depth, p.y() * depth, depth); } @@ -178,7 +178,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, } /* ************************************************************************* */ -Point2 CalibratedCamera::project(const Point3& point, +Vector2 CalibratedCamera::project(const Point3& point, OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { return project2(point, Dcamera, Dpoint); } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index b1e5917b2..bba438e70 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include @@ -28,8 +29,6 @@ namespace gtsam { -class Point2; - class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -55,7 +54,7 @@ public: * Some classes template on either PinholeCamera or StereoCamera, * and this typedef informs those classes what "project" returns. */ - typedef Point2 Measurement; + typedef Vector2 Measurement; private: @@ -71,7 +70,7 @@ protected: * @param pn projection in normalized coordinates * @param d disparity (inverse depth) */ - static Matrix26 Dpose(const Point2& pn, double d); + static Matrix26 Dpose(const Vector2& pn, double d); /** * Calculate Jacobian with respect to point @@ -79,7 +78,7 @@ protected: * @param d disparity (inverse depth) * @param Rt transposed rotation matrix */ - static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); + static Matrix23 Dpoint(const Vector2& pn, double d, const Matrix3& Rt); /// @} @@ -170,7 +169,7 @@ public: * Does *not* throw a CheiralityException, even if pc behind image plane * @param pc point in camera coordinates */ - static Point2 Project(const Point3& pc, // + static Vector2 Project(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); /** @@ -178,18 +177,18 @@ public: * Does *not* throw a CheiralityException, even if pc behind image plane * @param pc point in camera coordinates */ - static Point2 Project(const Unit3& pc, // + static Vector2 Project(const Unit3& pc, // OptionalJacobian<2, 2> Dpoint = boost::none); /// Project a point into the image and check depth - std::pair projectSafe(const Point3& pw) const; + std::pair projectSafe(const Point3& pw) const; /** Project point into the image * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = + Vector2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /** Project point at infinity into the image @@ -197,12 +196,12 @@ public: * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Point2 project2(const Unit3& point, + Vector2 project2(const Unit3& point, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - static Point3 backproject_from_camera(const Point2& p, const double depth); + static Point3 backproject_from_camera(const Vector2& p, const double depth); /// @} /// @name Advanced interface @@ -326,11 +325,11 @@ public: * @deprecated * Use project2, which is more consistently named across Pinhole cameras */ - Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = + Vector2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& pn, double depth) const { + Point3 backproject(const Vector2& pn, double depth) const { return pose().transform_from(backproject_from_camera(pn, depth)); } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 9327b91e8..efe54766d 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -22,6 +22,35 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +double norm(const Point2& p, OptionalJacobian<1,2> H) { + double r = std::sqrt(p.x() * p.x() + p.y() * p.y()); + if (H) { + if (fabs(r) > 1e-10) + *H << p.x() / r, p.y() / r; + else + *H << 1, 1; // really infinity, why 1 ? + } + return r; +} + +/* ************************************************************************* */ +double distance(const Point2& p, const Point2& q, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) { + Point2 d = q - p; + if (H1 || H2) { + Matrix12 H; + double r = norm(d, H); + if (H1) *H1 = -H; + if (H2) *H2 = H; + return r; + } else { + return norm(d); + } +} + +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS + /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; @@ -34,45 +63,26 @@ bool Point2::equals(const Point2& q, double tol) const { /* ************************************************************************* */ double Point2::norm(OptionalJacobian<1,2> H) const { - double r = std::sqrt(x() * x() + y() * y()); - if (H) { - if (fabs(r) > 1e-10) - *H << x() / r, y() / r; - else - *H << 1, 1; // really infinity, why 1 ? - } - return r; + return norm(*this, H); } /* ************************************************************************* */ double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, OptionalJacobian<1,2> H2) const { - Point2 d = point - *this; - if (H1 || H2) { - Matrix12 H; - double r = d.norm(H); - if (H1) *H1 = -H; - if (H2) *H2 = H; - return r; - } else - return d.norm(); + return distance(point, H1, H2); } -/* - * Calculate f and h, respectively the parallel and perpendicular distance of - * the intersections of two circles along and from the line connecting the centers. - * Both are dimensionless fractions of the distance d between the circle centers. - * If the circles do not intersect or they are identical, returns boost::none. - * If one solution (touching circles, as determined by tol), h will be exactly zero. - * h is a good measure for how accurate the intersection will be, as when circles touch - * or nearly touch, the intersection is ill-defined with noisy radius measurements. - * @param R_d : R/d, ratio of radius of first circle to distance between centers - * @param r_d : r/d, ratio of radius of second circle to distance between centers - * @param tol: absolute tolerance below which we consider touching circles - */ +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << '(' << p.x() << ", " << p.y() << ')'; + return os; +} + +#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS + /* ************************************************************************* */ // Math inspired by http://paulbourke.net/geometry/circlesphere/ -boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, +boost::optional circleCircleIntersection(double R_d, double r_d, double tol) { double R2_d2 = R_d*R_d; // Yes, RD-D2 ! @@ -87,7 +97,7 @@ boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, } /* ************************************************************************* */ -list Point2::CircleCircleIntersection(Point2 c1, Point2 c2, +list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh) { list solutions; @@ -116,27 +126,21 @@ list Point2::CircleCircleIntersection(Point2 c1, Point2 c2, } /* ************************************************************************* */ -list Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, +list circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { // distance between circle centers. - double d = c1.distance(c2); + double d = distance(c1, c2); // centers coincide, either no solution or infinite number of solutions. if (d<1e-9) return list(); // Calculate f and h given normalized radii double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d; - boost::optional fh = CircleCircleIntersection(R_d,r_d); + boost::optional fh = circleCircleIntersection(R_d,r_d); // Call version that takes fh - return CircleCircleIntersection(c1, c2, fh); -} - -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point2& p) { - os << '(' << p.x() << ", " << p.y() << ')'; - return os; + return circleCircleIntersection(c1, c2, fh); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 790a91c5f..1ae426afd 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -61,43 +61,13 @@ public: /// construct from 2D vector explicit Point2(const Vector2& v):Vector2(v) {} + /// @} + /// @name Declare circle intersection functionality + /// @{ - /* - * @brief Circle-circle intersection, given normalized radii. - * Calculate f and h, respectively the parallel and perpendicular distance of - * the intersections of two circles along and from the line connecting the centers. - * Both are dimensionless fractions of the distance d between the circle centers. - * If the circles do not intersect or they are identical, returns boost::none. - * If one solution (touching circles, as determined by tol), h will be exactly zero. - * h is a good measure for how accurate the intersection will be, as when circles touch - * or nearly touch, the intersection is ill-defined with noisy radius measurements. - * @param R_d : R/d, ratio of radius of first circle to distance between centers - * @param r_d : r/d, ratio of radius of second circle to distance between centers - * @param tol: absolute tolerance below which we consider touching circles - * @return optional Point2 with f and h, boost::none if no solution. - */ - static boost::optional CircleCircleIntersection(double R_d, double r_d, - double tol = 1e-9); - - /* - * @brief Circle-circle intersection, from the normalized radii solution. - * @param c1 center of first circle - * @param c2 center of second circle - * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. - */ - static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional); - - /** - * @brief Intersect 2 circles - * @param c1 center of first circle - * @param r1 radius of first circle - * @param c2 center of second circle - * @param r2 radius of second circle - * @param tol: absolute tolerance below which we consider touching circles - * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. - */ - static std::list CircleCircleIntersection(Point2 c1, double r1, - Point2 c2, double r2, double tol = 1e-9); + friend boost::optional circleCircleIntersection(double R_d, double r_d, double tol); + friend std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + friend std::list circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol); /// @} /// @name Testable @@ -162,6 +132,15 @@ public: static Vector2 Logmap(const Point2& p) { return p;} static Point2 Expmap(const Vector2& v) { return Point2(v);} inline double dist(const Point2& p2) const {return distance(p2);} + static boost::optional CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9) { + return circleCircleIntersection( R_d, r_d, tol); + } + static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional) { + return circleCircleIntersection( c1, c2, boost::optional); + } + static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9) { + return CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); + } /// @} #endif @@ -186,6 +165,14 @@ struct traits : public internal::VectorSpace { #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS +/// Distance of the point from the origin, with Jacobian +double norm(const Point2& p, OptionalJacobian<1, 2> H = boost::none); + +/// distance between two points +double distance(const Point2& p1, const Point2& q, + OptionalJacobian<1, 2> H1 = boost::none, + OptionalJacobian<1, 2> H2 = boost::none); + // Convenience typedef typedef std::pair Point2Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); @@ -198,5 +185,41 @@ inline Point2 operator*(double s, const Point2& p) { return p * s; } +/* + * @brief Circle-circle intersection, given normalized radii. + * Calculate f and h, respectively the parallel and perpendicular distance of + * the intersections of two circles along and from the line connecting the centers. + * Both are dimensionless fractions of the distance d between the circle centers. + * If the circles do not intersect or they are identical, returns boost::none. + * If one solution (touching circles, as determined by tol), h will be exactly zero. + * h is a good measure for how accurate the intersection will be, as when circles touch + * or nearly touch, the intersection is ill-defined with noisy radius measurements. + * @param R_d : R/d, ratio of radius of first circle to distance between centers + * @param r_d : r/d, ratio of radius of second circle to distance between centers + * @param tol: absolute tolerance below which we consider touching circles + * @return optional Point2 with f and h, boost::none if no solution. + */ +boost::optional circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); + +/* + * @brief Circle-circle intersection, from the normalized radii solution. + * @param c1 center of first circle + * @param c2 center of second circle + * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. + */ +std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + +/** + * @brief Intersect 2 circles + * @param c1 center of first circle + * @param r1 radius of first circle + * @param c2 center of second circle + * @param r2 radius of second circle + * @param tol: absolute tolerance below which we consider touching circles + * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. + */ +std::list circleCircleIntersection(Point2 c1, double r1, + Point2 c2, double r2, double tol = 1e-9); + } // \ namespace gtsam diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 49704642e..ec4c3dce4 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -53,7 +53,7 @@ void Pose2::print(const string& s) const { /* ************************************************************************* */ bool Pose2::equals(const Pose2& q, double tol) const { - return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); + return equal_with_abs_tol(t_, q.t_, tol) && r_.equals(q.r_, tol); } /* ************************************************************************* */ @@ -249,7 +249,7 @@ double Pose2::range(const Point2& point, Point2 d = point - t_; if (!Hpose && !Hpoint) return d.norm(); Matrix12 D_r_d; - double r = d.norm(D_r_d); + double r = norm(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << -r_.c(), r_.s(), 0.0, @@ -267,7 +267,7 @@ double Pose2::range(const Pose2& pose, Point2 d = pose.t() - t_; if (!Hpose && !Hother) return d.norm(); Matrix12 D_r_d; - double r = d.norm(D_r_d); + double r = norm(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 8a9d44755..92d3f4814 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -75,7 +75,7 @@ public: /// @{ NavState deltaXij() const override { return deltaXij_; } Rot3 deltaRij() const override { return deltaXij_.attitude(); } - Vector3 deltaPij() const override { return deltaXij_.position().vector(); } + Vector3 deltaPij() const override { return deltaXij_.position(); } Vector3 deltaVij() const override { return deltaXij_.velocity(); } Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }