diff --git a/CMakeLists.txt b/CMakeLists.txt index de9b4152f..3c93264f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) -option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) +option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) @@ -91,8 +91,8 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4) message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") endif() -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") endif() # Flags for choosing default packaging tools @@ -491,7 +491,7 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") -print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ") print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 43814731f..676dd42ec 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -48,8 +48,7 @@ public: virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { SimpleCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_); - return reprojectionError.vector(); + return camera.project(P_, H, boost::none, boost::none) - p_; } }; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 201ec188b..d68cedb74 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -70,7 +70,7 @@ int main() { // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); - x1_predict.print("X1 Predict"); + traits::Print(x1_predict, "X1 Predict"); @@ -91,7 +91,7 @@ int main() { // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); - x1_update.print("X1 Update"); + traits::Print(x1_update, "X1 Update"); @@ -101,13 +101,13 @@ int main() { difference = Point2(1,0); BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); - x2_predict.print("X2 Predict"); + traits::Print(x2_predict, "X2 Predict"); // Update Point2 z2(2.0, 0.0); PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); - x2_update.print("X2 Update"); + traits::Print(x2_update, "X2 Update"); @@ -117,13 +117,13 @@ int main() { difference = Point2(1,0); BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); - x3_predict.print("X3 Predict"); + traits::Print(x3_predict, "X3 Predict"); // Update Point2 z3(3.0, 0.0); PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); - x3_update.print("X3 Update"); + traits::Print(x3_update, "X3 Update"); return 0; } diff --git a/gtsam.h b/gtsam.h index f9a7483bb..320d140ea 100644 --- a/gtsam.h +++ b/gtsam.h @@ -266,23 +266,12 @@ class Point2 { // Group static gtsam::Point2 identity(); - gtsam::Point2 inverse() const; - gtsam::Point2 compose(const gtsam::Point2& p2) const; - gtsam::Point2 between(const gtsam::Point2& p2) const; - - // Manifold - gtsam::Point2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point2& p) const; - - // Lie Group - static gtsam::Point2 Expmap(Vector v); - static Vector Logmap(const gtsam::Point2& p); // Standard Interface double x() const; double y() const; Vector vector() const; - double dist(const gtsam::Point2& p2) const; + double distance(const gtsam::Point2& p2) const; double norm() const; // enabling serialization functionality 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..4ad1dffa2 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 (distance2(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..2071b8792 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 (distance2(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..026becebe 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) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index b1e5917b2..81f448a7c 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: diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 0df85d3d2..a5cb5d046 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -56,8 +56,7 @@ protected: // Project and fill error vector Vector b(ZDim * m); for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Z e = predicted[i] - measured[i]; - b.segment(row) = e.vector(); + b.segment(row) = traits::Local(measured[i], predicted[i]); } return b; } @@ -107,7 +106,8 @@ public: // Allocate result size_t m = this->size(); - std::vector z(m); + std::vector z; + z.reserve(m); // Allocate derivatives if (E) E->resize(ZDim * m, N); @@ -117,7 +117,7 @@ public: for (size_t i = 0; i < m; i++) { MatrixZD Fi; Eigen::Matrix Ei; - z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0); + z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0)); if (Fs) (*Fs)[i] = Fi; if (E) E->block(ZDim * i, 0) = Ei; } diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index bc6132812..2152a7c39 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) @@ -23,21 +23,11 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -void Point2::print(const string& s) const { - cout << s << *this << endl; -} - -/* ************************************************************************* */ -bool Point2::equals(const Point2& q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); -} - -/* ************************************************************************* */ -double Point2::norm(OptionalJacobian<1,2> H) const { - double r = sqrt(x_ * x_ + y_ * y_); +double norm2(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 << x_ / r, y_ / r; + *H << p.x() / r, p.y() / r; else *H << 1, 1; // really infinity, why 1 ? } @@ -45,34 +35,66 @@ double Point2::norm(OptionalJacobian<1,2> H) const { } /* ************************************************************************* */ -double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, - OptionalJacobian<1,2> H2) const { - Point2 d = point - *this; +double distance2(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 = d.norm(H); + double r = norm2(d, H); if (H1) *H1 = -H; if (H2) *H2 = H; return r; - } else + } else { return d.norm(); + } } -/* - * 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 - */ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS + +/* ************************************************************************* */ +void Point2::print(const string& s) const { + cout << s << *this << endl; +} + +/* ************************************************************************* */ +bool Point2::equals(const Point2& q, double tol) const { + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol); +} + +/* ************************************************************************* */ +double Point2::norm(OptionalJacobian<1,2> H) const { + return gtsam::norm2(*this, H); +} + +/* ************************************************************************* */ +double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) const { + return gtsam::distance2(*this, point, H1, H2); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << '(' << p.x() << ", " << p.y() << ')'; + return os; +} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +boost::optional CircleCircleIntersection(double R_d, double r_d, double tol) { + return circleCircleIntersection(R_d, r_d, tol); +} +std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh) { + return circleCircleIntersection(c1, c2, fh); +} +std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) { + return circleCircleIntersection(c1, r1, c2, r2, tol); +} +#endif + +#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 ! @@ -83,11 +105,11 @@ boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, // Hence, there are only solutions if >=0 if (h2<-tol) return boost::none; // allow *slightly* negative else if (h2 Point2::CircleCircleIntersection(Point2 c1, Point2 c2, +list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh) { list solutions; @@ -116,27 +138,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.dist(c2); + double d = distance2(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 3099a8bb3..fb250df6d 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.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) @@ -22,6 +22,14 @@ namespace gtsam { +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + + /// As of GTSAM 4, in order to make GTSAM more lean, + /// it is now possible to just typedef Point2 to Vector2 + typedef Vector2 Point2; + +#else + /** * A 2D point * Complies with the Testable Concept @@ -29,69 +37,30 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point2 { +class GTSAM_EXPORT Point2 : public Vector2 { private: - double x_, y_; - public: enum { dimension = 2 }; /// @name Standard Constructors /// @{ /// default constructor - Point2(): x_(0), y_(0) {} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + // Deprecated default constructor initializes to zero, in contrast to new behavior below + Point2() { setZero(); } +#else + Point2() {} +#endif - /// construct from doubles - Point2(double x, double y): x_(x), y_(y) {} + using Vector2::Vector2; /// @} /// @name Advanced Constructors /// @{ /// construct from 2D vector - explicit Point2(const Vector2& v) { - x_ = v(0); - y_ = v(1); - } - - /* - * @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); - + explicit Point2(const Vector2& v):Vector2(v) {} /// @} /// @name Testable /// @{ @@ -107,21 +76,7 @@ public: /// @{ /// identity - inline static Point2 identity() {return Point2();} - - /// inverse - inline Point2 operator- () const {return Point2(-x_,-y_);} - - /// add vector on right - inline Point2 operator +(const Vector2& v) const { - return Point2(x_ + v[0], y_ + v[1]); - } - - /// add - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - - /// subtract - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline static Point2 identity() {return Point2(0,0);} /// @} /// @name Vector Space @@ -137,51 +92,44 @@ public: double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, OptionalJacobian<1,2> H2 = boost::none) const; - /** @deprecated The following function has been deprecated, use distance above */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } - - /// multiply with a scalar - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - - /// divide by a scalar - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /// @} /// @name Standard Interface /// @{ /// equality - inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;} + inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();} /// get x - double x() const {return x_;} + inline double x() const {return (*this)[0];} /// get y - double y() const {return y_;} + inline double y() const {return (*this)[1];} - /// return vectorized form (column-wise). TODO: why does this function exist? - Vector2 vector() const { return Vector2(x_, y_); } + /// return vectorized form (column-wise). + const Vector2& vector() const { return *this; } /// @} - /// @name Deprecated - /// @{ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - Point2 inverse() const { return -(*this);} - Point2 compose(const Point2& q) const { return (*this)+q;} - Point2 between(const Point2& q) const { return q-(*this);} - Vector2 localCoordinates(const Point2& q) const { return between(q).vector();} - Point2 retract(const Vector2& v) const { return compose(Point2(v));} - static Vector2 Logmap(const Point2& p) { return p.vector();} - static Point2 Expmap(const Vector2& v) { return Point2(v);} - /// @} - /// Streaming GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + Point2 inverse() const { return -(*this); } + Point2 compose(const Point2& q) const { return (*this)+q;} + Point2 between(const Point2& q) const { return q-(*this);} + Vector2 localCoordinates(const Point2& q) const { return between(q);} + Point2 retract(const Vector2& v) const { return compose(Point2(v));} + 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); + static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); + static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); + /// @} +#endif + private: /// @name Advanced Interface @@ -192,13 +140,25 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - } + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);} - /// @} + /// @} }; +template<> +struct traits : public internal::VectorSpace { +}; + +#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS + +/// Distance of the point from the origin, with Jacobian +double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); + +/// distance between two points +double distance2(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); @@ -207,10 +167,45 @@ std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); typedef std::vector Point2Vector; /// multiply with scalar -inline Point2 operator*(double s, const Point2& p) {return p*s;} +inline Point2 operator*(double s, const Point2& p) { +return p * s; +} -template<> -struct traits : public internal::VectorSpace {}; +/* + * @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/Point3.cpp b/gtsam/geometry/Point3.cpp index df0f78283..7dc6bd704 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -21,7 +21,7 @@ using namespace std; namespace gtsam { -#ifndef GTSAM_USE_VECTOR3_POINTS +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS bool Point3::equals(const Point3 &q, double tol) const { return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); @@ -34,11 +34,11 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { - return gtsam::distance(*this,q,H1,H2); + return gtsam::distance3(*this,q,H1,H2); } double Point3::norm(OptionalJacobian<1,3> H) const { - return gtsam::norm(*this, H); + return gtsam::norm3(*this, H); } Point3 Point3::normalized(OptionalJacobian<3,3> H) const { @@ -80,8 +80,8 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, #endif /* ************************************************************************* */ -double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, - OptionalJacobian<1, 3> H2) { +double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { double d = (q - p1).norm(); if (H1) { *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z(); @@ -94,7 +94,7 @@ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, return d; } -double norm(const Point3 &p, OptionalJacobian<1, 3> H) { +double norm3(const Point3 &p, OptionalJacobian<1, 3> H) { double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { if (fabs(r) > 1e-10) @@ -106,7 +106,7 @@ double norm(const Point3 &p, OptionalJacobian<1, 3> H) { } Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) { - Point3 normalized = p / norm(p); + Point3 normalized = p / p.norm(); if (H) { // 3*3 Derivative double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index fd254e51c..7787b346e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -29,7 +29,7 @@ namespace gtsam { -#ifdef GTSAM_USE_VECTOR3_POINTS +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS /// As of GTSAM 4, in order to make GTSAM more lean, /// it is now possible to just typedef Point3 to Vector3 @@ -124,9 +124,9 @@ class GTSAM_EXPORT Point3 : public Vector3 { Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} - Vector3 localCoordinates(const Point3& q) const { return between(q).vector();} + Vector3 localCoordinates(const Point3& q) const { return between(q);} Point3 retract(const Vector3& v) const { return compose(Point3(v));} - static Vector3 Logmap(const Point3& p) { return p.vector();} + static Vector3 Logmap(const Point3& p) { return p;} static Point3 Expmap(const Vector3& v) { return Point3(v);} inline double dist(const Point3& q) const { return (q - *this).norm(); } Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);} @@ -153,19 +153,19 @@ struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; -#endif +#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS // Convenience typedef typedef std::pair Point3Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); /// distance between two points -double distance(const Point3& p1, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none); +double distance3(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); /// Distance of the point from the origin, with Jacobian -double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none); +double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); /// normalize, with optional Jacobian Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); @@ -193,7 +193,7 @@ struct Range { double operator()(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) { - return distance(p, q, H1, H2); + return distance3(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 34b146bee..2a52e98ba 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -53,21 +53,21 @@ 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); } /* ************************************************************************* */ Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { - if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); - Point2 v(xi(0),xi(1)); - double w = xi(2); + if (H) *H = Pose2::ExpmapDerivative(xi); + const Point2 v(xi(0),xi(1)); + const double w = xi(2); if (std::abs(w) < 1e-10) return Pose2(xi[0], xi[1], xi[2]); else { - Rot2 R(Rot2::fromAngle(w)); - Point2 v_ortho = R_PI_2 * v; // points towards rot center - Point2 t = (v_ortho - R.rotate(v_ortho)) / w; + const Rot2 R(Rot2::fromAngle(w)); + const Point2 v_ortho = R_PI_2 * v; // points towards rot center + const Point2 t = (v_ortho - R.rotate(v_ortho)) / w; return Pose2(R, t); } } @@ -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 = norm2(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 = norm2(d, D_r_d); if (Hpose) { Matrix23 D_d_pose; D_d_pose << @@ -311,7 +311,7 @@ boost::optional align(const vector& pairs) { if (n<2) return boost::none; // we need at least two pairs // calculate centroids - Point2 cp,cq; + Point2 cp(0,0), cq(0,0); for(const Point2Pair& pair: pairs) { cp += pair.first; cq += pair.second; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 31dfb479f..1ba384857 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.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) @@ -52,7 +52,9 @@ public: /// @{ /** default constructor = origin */ - Pose2() {} // default is origin + Pose2() : + r_(traits::Identity()), t_(traits::Identity()) { + } /** copy constructor */ Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} @@ -86,7 +88,7 @@ public: /// @{ /** Construct from canonical coordinates \f$ [T_x,T_y,\theta] \f$ (Lie algebra) */ - Pose2(const Vector& v) { + Pose2(const Vector& v) : Pose2() { *this = Expmap(v); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 3138dfd64..25cd661e8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -345,7 +345,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, return local.norm(); } else { Matrix13 D_r_local; - const double r = norm(local, D_r_local); + const double r = norm3(local, D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 79b13b93e..a5b64ef04 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -179,7 +179,7 @@ namespace gtsam { */ static Rot3 AxisAngle(const Point3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else return Rot3(SO3::AxisAngle(axis,angle)); #endif diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 9f21bb82a..cd05af519 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -78,7 +78,7 @@ public: /// Construct from 2D point in plane at focal length f /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point - explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) { + explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) { p_.normalize(); } diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index e9eb689e1..8de049fa4 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,7 +52,7 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( pn.equals(pn_hat, 1e-5)); + CHECK( traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c5a6be2d6..416665d46 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -48,7 +48,7 @@ TEST( Cal3DS2, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( pn.equals(pn_hat, 1e-5)); + CHECK( traits::Equals(pn, pn_hat, 1e-5)); } Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); } diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index de9a8b739..2c5ffd7fb 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -59,7 +59,7 @@ TEST( Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); - CHECK( p.equals(pn_hat, 1e-8)); + CHECK( traits::Equals(p, pn_hat, 1e-8)); } Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index cc7a0c0f8..c7e4e3ae7 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -152,7 +152,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity); Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity); - CHECK(assert_equal(Point2(), result)); + CHECK(assert_equal(Point2(0,0), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 0afa04411..478f5baf4 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -44,7 +44,7 @@ TEST(CameraSet, Pinhole) { EXPECT(!set.equals(set2)); // Check measurements - Point2 expected; + Point2 expected(0,0); ZZ z = set.project2(p); EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[1])); @@ -117,7 +117,7 @@ TEST(CameraSet, Pinhole) { Unit3 pointAtInfinity(0, 0, 1000); EXPECT( assert_equal(pointAtInfinity, - camera.backprojectPointAtInfinity(Point2()))); + camera.backprojectPointAtInfinity(Point2(0,0)))); actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E); EXPECT(assert_equal(expectedV, actualV)); LONGS_EQUAL(2, Fs.size()); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 99dcb95bf..a9b68bdec 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -153,12 +153,12 @@ TEST( PinholeCamera, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x.first)); + EXPECT(assert_equal(Point2(0,0), x.first)); EXPECT(x.second); } @@ -169,12 +169,12 @@ TEST( PinholeCamera, backprojectInfinity2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0)); Unit3 expected(0., 1., 0.); Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x)); + EXPECT(assert_equal(Point2(0,0), x)); } /* ************************************************************************* */ @@ -184,12 +184,12 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0)); Unit3 expected(0., 0., 1.); Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x)); + EXPECT(assert_equal(Point2(0,0), x)); } /* ************************************************************************* */ @@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) { double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, + EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 0d840de7e..ecbb92061 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -124,12 +124,12 @@ TEST( PinholePose, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Point2(), x.first)); + EXPECT(assert_equal(Point2(0,0), x.first)); EXPECT(x.second); } @@ -212,7 +212,7 @@ TEST( PinholePose, range0) { double result = camera.range(point1, D1, D2); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); + EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index b8f001f1c..28b7ddac6 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -84,7 +84,7 @@ TEST(PinholeSet, Pinhole) { EXPECT(!set.equals(set2)); // Check measurements - Point2 expected; + Point2 expected(0,0); ZZ z = set.project2(p); EXPECT(assert_equal(expected, z[0])); EXPECT(assert_equal(expected, z[1])); @@ -131,7 +131,7 @@ TEST(PinholeSet, Pinhole) { } EXPECT( assert_equal(pointAtInfinity, - camera.backprojectPointAtInfinity(Point2()))); + camera.backprojectPointAtInfinity(Point2(0,0)))); { PinholeSet::FBlocks Fs; Matrix E; diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 3a636b9bf..e2a5bcdea 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.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) @@ -27,6 +27,11 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) +//****************************************************************************** +TEST(Point2 , Constructor) { + Point2 p; +} + //****************************************************************************** TEST(Double , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -95,26 +100,26 @@ TEST( Point2, expmap) { /* ************************************************************************* */ TEST( Point2, arithmetic) { - EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); - EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); - EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); - EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); - EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); - EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); + EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); + EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); + EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); + EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); + EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); + EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); } /* ************************************************************************* */ TEST( Point2, unit) { Point2 p0(10, 0), p1(0, -10), p2(10, 10); - EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6)); - EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6)); - EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); + EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6)); + EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6)); + EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6)); } namespace { /* ************************************************************************* */ // some shared test values - Point2 x1, x2(1, 1), x3(1, 1); + Point2 x1(0,0), x2(1, 1), x3(1, 1); Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); /* ************************************************************************* */ @@ -126,19 +131,19 @@ TEST( Point2, norm ) { Point2 p0(cos(5.0), sin(5.0)); DOUBLES_EQUAL(1, p0.norm(), 1e-6); Point2 p1(4, 5), p2(1, 1); - DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6); + DOUBLES_EQUAL( 5, distance2(p1, p2), 1e-6); DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6); Matrix expectedH, actualH; double actual; // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] - actual = x1.norm(actualH); + actual = norm2(x1, actualH); EXPECT_DOUBLES_EQUAL(0, actual, 1e-9); expectedH = (Matrix(1, 2) << 1.0, 1.0).finished(); EXPECT(assert_equal(expectedH,actualH)); - actual = x2.norm(actualH); + actual = norm2(x2, actualH); EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); @@ -151,20 +156,20 @@ TEST( Point2, norm ) { /* ************************************************************************* */ namespace { double distance_proxy(const Point2& location, const Point2& point) { - return location.distance(point); + return distance2(location, point); } } TEST( Point2, distance ) { Matrix expectedH1, actualH1, expectedH2, actualH2; // establish distance is indeed zero - EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9); + EXPECT_DOUBLES_EQUAL(1, distance2(x1, l1), 1e-9); // establish distance is indeed 45 degrees - EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), distance2(x1, l2), 1e-9); // Another pair - double actual23 = x2.distance(l3, actualH1, actualH2); + double actual23 = distance2(x2, l3, actualH1, actualH2); EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9); // Check numerical derivatives @@ -174,7 +179,7 @@ TEST( Point2, distance ) { EXPECT(assert_equal(expectedH2,actualH2)); // Another test - double actual34 = x3.distance(l4, actualH1, actualH2); + double actual34 = distance2(x3, l4, actualH1, actualH2); EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9); // Check numerical derivatives @@ -190,42 +195,42 @@ TEST( Point2, circleCircleIntersection) { double offset = 0.994987; // Test intersections of circle moving from inside to outside - list inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1); + list inside = circleCircleIntersection(Point2(0,0),5,Point2(0,0),1); EXPECT_LONGS_EQUAL(0,inside.size()); - list touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1); + list touching1 = circleCircleIntersection(Point2(0,0),5,Point2(4,0),1); EXPECT_LONGS_EQUAL(1,touching1.size()); EXPECT(assert_equal(Point2(5,0), touching1.front())); - list common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1); + list common = circleCircleIntersection(Point2(0,0),5,Point2(5,0),1); EXPECT_LONGS_EQUAL(2,common.size()); EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6)); EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6)); - list touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1); + list touching2 = circleCircleIntersection(Point2(0,0),5,Point2(6,0),1); EXPECT_LONGS_EQUAL(1,touching2.size()); EXPECT(assert_equal(Point2(5,0), touching2.front())); // test rotated case - list rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1); + list rotated = circleCircleIntersection(Point2(0,0),5,Point2(0,5),1); EXPECT_LONGS_EQUAL(2,rotated.size()); EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6)); EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6)); // test r1 smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5); + list smaller = circleCircleIntersection(Point2(0,0),1,Point2(5,0),5); EXPECT_LONGS_EQUAL(2,smaller.size()); EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6)); EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6)); // test offset case, r1>r2 - list offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1); + list offset1 = circleCircleIntersection(Point2(1,1),5,Point2(6,1),1); EXPECT_LONGS_EQUAL(2,offset1.size()); EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6)); EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6)); // test offset case, r1 offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5); + list offset2 = circleCircleIntersection(Point2(6,1),1,Point2(1,1),5); EXPECT_LONGS_EQUAL(2,offset2.size()); EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6)); EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6)); @@ -233,12 +238,14 @@ TEST( Point2, circleCircleIntersection) { } /* ************************************************************************* */ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS TEST( Point2, stream) { Point2 p(1, 2); std::ostringstream os; os << p; EXPECT(os.str() == "(1, 2)"); } +#endif /* ************************************************************************* */ int main () { diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 9b6e53323..8dde2b5aa 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -26,6 +26,11 @@ GTSAM_CONCEPT_LIE_INST(Point3) static Point3 P(0.2, 0.7, -2); +//****************************************************************************** +TEST(Point3 , Constructor) { + Point3 p; +} + //****************************************************************************** TEST(Point3 , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -149,7 +154,7 @@ TEST( Point3, cross2) { } /* ************************************************************************* */ -#ifndef GTSAM_USE_VECTOR3_POINTS +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS TEST( Point3, stream) { Point3 p(1, 2, -3); std::ostringstream os; @@ -178,20 +183,20 @@ TEST (Point3, norm) { Matrix actualH; Point3 point(3,4,5); // arbitrary point double expected = sqrt(50); - EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8); + EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8); Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { - return distance(P,Q); + return distance3(P, Q); } TEST (Point3, distance) { Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Matrix H1, H2; - double d = distance(P, Q, H1, H2); + double d = distance3(P, Q, H1, H2); double expectedDistance = 55.542686; Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index bccfdc1c7..10fd431bc 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -43,7 +43,7 @@ TEST(Pose2 , Concept) { /* ************************************************************************* */ TEST(Pose2, constructors) { - Point2 p; + Point2 p(0,0); Pose2 pose(0,p); Pose2 origin; assert_equal(pose,origin); @@ -371,7 +371,7 @@ TEST(Pose2, compose_c) /* ************************************************************************* */ TEST(Pose2, inverse ) { - Point2 origin, t(1,2); + Point2 origin(0,0), t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Pose2 identity, lTg = gTl.inverse(); @@ -409,7 +409,7 @@ namespace { /* ************************************************************************* */ TEST( Pose2, matrix ) { - Point2 origin, t(1,2); + Point2 origin(0,0), t(1,2); Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y Matrix gMl = matrix(gTl); EXPECT(assert_equal((Matrix(3,3) << @@ -743,7 +743,7 @@ namespace { /* ************************************************************************* */ struct Triangle { size_t i_,j_,k_;}; - boost::optional align(const vector& ps, const vector& qs, + boost::optional align2(const vector& ps, const vector& qs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; vector correspondences; @@ -762,7 +762,7 @@ TEST(Pose2, align_4) { Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; - boost::optional actual = align(ps, qs, make_pair(t1,t2)); + boost::optional actual = align2(ps, qs, make_pair(t1,t2)); EXPECT(assert_equal(expected, *actual)); } diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 70b3069f2..edf122d3c 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -104,12 +104,12 @@ TEST( SimpleCamera, backproject2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down SimpleCamera camera(Pose3(rot, origin), K); - Point3 actual = camera.backproject(Point2(), 1.); + Point3 actual = camera.backproject(Point2(0,0), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); + CHECK(assert_equal(Point2(0,0), x.first)); CHECK(x.second); } diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index c3df95abc..86b7734f7 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -273,7 +273,7 @@ TEST( triangulation, onePose) { vector measurements; poses += Pose3(); - measurements += Point2(); + measurements += Point2(0,0); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), TriangulationUnderconstrainedException); @@ -282,7 +282,7 @@ TEST( triangulation, onePose) { //****************************************************************************** TEST( triangulation, StereotriangulateNonlinear ) { - Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); // two camera poses m1, m2 Matrix4 m1, m2; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 369c54bea..880c23fd1 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -112,7 +112,8 @@ std::pair triangulationGraph( Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension)); + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; graph.push_back(TriangulationFactor // @@ -457,7 +458,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, for(const CAMERA& camera: cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 - && distance(pose.translation(), point) + && distance3(pose.translation(), point) > params.landmarkDistanceThreshold) return TriangulationResult::Degenerate(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -471,7 +472,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, if (params.dynamicOutlierRejectionThreshold > 0) { const Point2& zi = measured.at(i); Point2 reprojectionError(camera.project(point) - zi); - totalReprojError += reprojectionError.vector().norm(); + totalReprojError += reprojectionError.norm(); } i += 1; } 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_; } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index e0c4da94e..e85aceb15 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -61,16 +61,14 @@ namespace gtsam { } /* ************************************************************************* */ - template - ExtendedKalmanFilter::ExtendedKalmanFilter(Key key_initial, T x_initial, - noiseModel::Gaussian::shared_ptr P_initial) { - - // Set the initial linearization point to the provided mean - x_ = x_initial; - + template + ExtendedKalmanFilter::ExtendedKalmanFilter( + Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial) + : x_(x_initial) // Set the initial linearization point + { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero - // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? + // TODO(Frank): is there a reason why noiseModel is not simply P_initial? int n = traits::GetDimension(x_initial); priorFactor_ = JacobianFactor::shared_ptr( new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n), diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 0d8d717bc..c6aa52ab2 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -260,7 +260,7 @@ public: std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) << ")," << "\n"; this->noiseModel_->print(); - value_.print("Value"); + traits::Print(value_, "Value"); } private: diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index dece9bad8..db98971ff 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -80,7 +80,7 @@ TEST(Expression, Leaves) { // Unary(Leaf) namespace unary { Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { - return Point2(); + return Point2(0,0); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; @@ -111,24 +111,43 @@ TEST(Expression, Unary3) { } /* ************************************************************************* */ +class Class : public Point3 { + public: + enum {dimension = 3}; + using Point3::Point3; + const Vector3& vector() const { return *this; } + inline static Class identity() { return Class(0,0,0); } + double norm(OptionalJacobian<1,3> H = boost::none) const { + return norm3(*this, H); + } + bool equals(const Class &q, double tol) const { + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); + } + void print(const string& s) const { cout << s << *this << endl;} +}; + +namespace gtsam { +template<> struct traits : public internal::VectorSpace {}; +} + // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm(>sam::norm, p); + Expression p(67); + Expression norm_(p, &Class::norm); // Create Values Values values; - values.insert(67, Point3(3, 4, 5)); + values.insert(67, Class(3, 4, 5)); // Check dims as map std::map map; - norm.dims(map); + norm_.dims(map); LONGS_EQUAL(1, map.size()); // Get value and Jacobians std::vector H(1); - double actual = norm.value(values, H); + double actual = norm_.value(values, H); // Check all EXPECT(actual == sqrt(50)); @@ -370,7 +389,7 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ norm_(>sam::norm3, Point3_(key)); const Double_ sum_ = norm_ + norm_; Values values; @@ -389,7 +408,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const Double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm3, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e668d27d3..e6b312b95 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -139,7 +139,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.transpose() << ")' and (" << pn_.vector().transpose() + << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" << std::endl; } @@ -162,7 +162,7 @@ public: // The point d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pn; + Point2 pn(0,0); if (!DE && !Dd) { Point3 _1T2 = E.direction().point3(); @@ -195,7 +195,7 @@ public: } Point2 reprojectionError = pn - pn_; - return f_ * reprojectionError.vector(); + return f_ * reprojectionError; } }; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 0f187db75..9356aceb2 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -107,7 +107,7 @@ public: */ void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -115,15 +115,14 @@ public: */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { try { - Point2 reprojError(camera.project2(point,H1,H2) - measured_); - return reprojError.vector(); + return camera.project2(point,H1,H2) - measured_; } catch( CheiralityException& e) { if (H1) *H1 = JacobianC::Zero(); @@ -145,8 +144,7 @@ public: try { const CAMERA& camera = values.at(key1); const LANDMARK& point = values.at(key2); - Point2 reprojError(camera.project2(point, H1, H2) - measured()); - b = -reprojError.vector(); + b = measured() - camera.project2(point, H1, H2); } catch (CheiralityException& e) { H1.setZero(); H2.setZero(); @@ -243,7 +241,7 @@ public: */ void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -251,7 +249,7 @@ public: */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ @@ -262,8 +260,7 @@ public: { try { Camera camera(pose3,calib); - Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); - return reprojError.vector(); + return camera.project(point, H1, H2, H3) - measured_; } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2, 6); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index dee8e925f..d13c28e11 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -56,7 +56,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - GenericProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + GenericProjectionFactor() : + measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor @@ -108,7 +110,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "GenericProjectionFactor, z = "; - measured_.print(); + traits::Print(measured_); if(this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); Base::print("", keyFormatter); @@ -119,7 +121,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); } @@ -134,16 +136,14 @@ namespace gtsam { PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H2, boost::none) - measured_; } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H2, boost::none) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 966ade343..68b42f210 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -84,7 +84,7 @@ public: * each degree of freedom. */ ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) - : Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma), + : Base(noiseModel::Isotropic::Sigma(traits::dimension, sigma), globalKey, transKey, localKey) {} virtual ~ReferenceFrameFactor(){} @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); + *Dlocal = -1* Matrix::Identity(traits::dimension, traits::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3128390c7..93d2ff218 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -189,7 +189,7 @@ public: bool areMeasurementsEqual = true; for (size_t i = 0; i < measured_.size(); i++) { - if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + if (traits::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) areMeasurementsEqual = false; break; } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index af7ca64c6..64b300b86 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -499,8 +499,8 @@ public: return Base::totalReprojectionError(cameras, *result_); else if (params_.degeneracyMode == HANDLE_INFINITY) { // Otherwise, manage the exceptions with rotation-only factors - const Point2& z0 = this->measured_.at(0); - Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0); + Unit3 backprojected = cameras.front().backprojectPointAtInfinity( + this->measured_.at(0)); return Base::totalReprojectionError(cameras, backprojected); } else // if we don't want to manage the exceptions we discard the factor diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index e97cd2730..811d92fbc 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -78,12 +78,11 @@ public: bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( throwCheirality), verboseCheirality_(verboseCheirality) { - if (model && model->dim() != Measurement::dimension) + if (model && model->dim() != traits::dimension) throw std::invalid_argument( "TriangulationFactor must be created with " - + boost::lexical_cast((int) Measurement::dimension) + + boost::lexical_cast((int) traits::dimension) + "-dimensional noise model."); - } /** Virtual destructor */ @@ -105,7 +104,7 @@ public: DefaultKeyFormatter) const { std::cout << s << "TriangulationFactor,"; camera_.print("camera"); - measured_.print("z"); + traits::Print(measured_, "z"); Base::print("", keyFormatter); } @@ -113,25 +112,24 @@ public: virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) - && this->measured_.equals(e->measured_, tol); + && traits::Equals(this->measured_, e->measured_, tol); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Measurement error(camera_.project2(point, boost::none, H2) - measured_); - return error.vector(); + return traits::Local(measured_, camera_.project2(point, boost::none, H2)); } catch (CheiralityException& e) { if (H2) - *H2 = Matrix::Zero(Measurement::dimension, 3); + *H2 = Matrix::Zero(traits::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return Eigen::Matrix::Constant(2.0 * camera_.calibration().fx()); + return Eigen::Matrix::dimension,1>::Constant(2.0 * camera_.calibration().fx()); } } @@ -153,14 +151,14 @@ public: // Allocate memory for Jacobian factor, do only once if (Ab.rows() == 0) { std::vector dimensions(1, 3); - Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true); - A.resize(Measurement::dimension,3); - b.resize(Measurement::dimension); + Ab = VerticalBlockMatrix(dimensions, traits::dimension, true); + A.resize(traits::dimension,3); + b.resize(traits::dimension); } // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = -(camera_.project2(point, boost::none, A) - measured_).vector(); + b = traits::Local(camera_.project2(point, boost::none, A), measured_); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 8ac6c48b6..2e3d613d6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -222,8 +222,7 @@ TEST (EssentialMatrixFactor2, factor) { // Check evaluation Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); const Point2 pi = PinholeBase::Project(P2); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -296,8 +295,7 @@ TEST (EssentialMatrixFactor3, factor) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -438,8 +436,7 @@ TEST (EssentialMatrixFactor2, extraTest) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); @@ -507,8 +504,7 @@ TEST (EssentialMatrixFactor3, extraTest) { // Check evaluation Point3 P1 = data.tracks[i].p; const Point2 pi = camera2.project(P1); - Point2 reprojectionError(pi - pB(i)); - Vector expected = reprojectionError.vector(); + Point2 expected(pi - pB(i)); Matrix Hactual1, Hactual2; double d(baseline / P1.z()); diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 96052bd0f..f69f4c113 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -55,8 +55,8 @@ struct traits : public Testable {}; TEST(SmartFactorBase, Pinhole) { PinholeFactor f= PinholeFactor(unit2); - f.add(Point2(), 1); - f.add(Point2(), 2); + f.add(Point2(0,0), 1); + f.add(Point2(0,0), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 7eefb2398..0488c0f49 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -300,20 +300,14 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { SfM_Track track1; for (size_t i = 0; i < 3; ++i) { - SfM_Measurement measures; - measures.first = i + 1; // cameras are from 1 to 3 - measures.second = measurements_cam1.at(i); - track1.measurements.push_back(measures); + track1.measurements.emplace_back(i + 1, measurements_cam1.at(i)); } SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); smartFactor1->add(track1); SfM_Track track2; for (size_t i = 0; i < 3; ++i) { - SfM_Measurement measures; - measures.first = i + 1; // cameras are from 1 to 3 - measures.second = measurements_cam2.at(i); - track2.measurements.push_back(measures); + track2.measurements.emplace_back(i + 1, measurements_cam2.at(i)); } SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); smartFactor2->add(track2); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index c1afe3882..c8c46ee7b 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other, const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; - double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 615b1d467..40c70696b 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -85,7 +85,7 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { static const double Speed = 330; Matrix13 D1, D2; - double distance = gtsam::distance(location_, microphone, D1, D2); + double distance = gtsam::distance3(location_, microphone, D1, D2); if (H1) // derivative of toa with respect to event *H1 << 1.0, D1 / Speed; diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index f610e4d0f..ba1445b20 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -46,7 +46,7 @@ SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, doubl bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const { if (p.size() != size()) return false; for (size_t i=0; i::Equals(landmarks_[i], p.landmarks_[i], tol)) return false; return true; } @@ -55,7 +55,7 @@ bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const { void SimPolygon2D::print(const string& s) const { cout << "SimPolygon " << s << ": " << endl; for(const Point2& p: landmarks_) - p.print(" "); + traits::Print(p, " "); } /* ************************************************************************* */ @@ -151,7 +151,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC); // use triangle equality to verify non-degenerate triangle - double dAC = xA.t().distance(xC.t()); + double dAC = distance2(xA.t(), xC.t()); // form a triangle and test if it meets requirements SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t()); @@ -164,7 +164,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( 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).distance(test_tri.landmark(2)) > min_side_len && + distance2(test_tri.landmark(1), 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) && @@ -260,7 +260,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -272,7 +272,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -285,7 +285,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size, return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -303,7 +303,7 @@ Point2 SimPolygon2D::randomBoundedPoint2( return p; } throw runtime_error("Failed to find a place for a landmark!"); - return Point2(); + return Point2(0,0); } /* ***************************************************************** */ @@ -320,7 +320,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) { bool SimPolygon2D::nearExisting(const std::vector& S, const Point2& p, double threshold) { for(const Point2& Sp: S) - if (Sp.distance(p) < threshold) + if (distance2(Sp, p) < threshold) return true; return false; } diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index e294a360d..111d23b91 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -14,13 +14,14 @@ using namespace std; /* ************************************************************************* */ void SimWall2D::print(const std::string& s) const { std::cout << "SimWall2D " << s << ":" << std::endl; - a_.print(" a"); - b_.print(" b"); + traits::Print(a_, " a"); + traits::Print(b_, " b"); } /* ************************************************************************* */ bool SimWall2D::equals(const SimWall2D& other, double tol) const { - return a_.equals(other.a_, tol) && b_.equals(other.b_, tol); + return traits::Equals(a_, other.a_, tol) && + traits::Equals(b_, other.b_, tol); } /* ************************************************************************* */ @@ -37,8 +38,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons 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"); + if (debug) traits::Print(Ba, "Ba"); + if (debug) traits::Print(Bb, "Bb"); // check sides of line if (Ba.y() * Bb.y() > 0.0 || @@ -73,7 +74,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons } // find lower point by y - Point2 low, high; + Point2 low(0,0), high(0,0); if (Ba.y() > Bb.y()) { high = Ba; low = Bb; @@ -81,8 +82,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons high = Bb; low = Ba; } - if (debug) high.print("high"); - if (debug) low.print("low"); + if (debug) traits::Print(high, "high"); + if (debug) traits::Print(low, "low"); // find x-intercept double slope = (high.y() - low.y())/(high.x() - low.x()); @@ -138,7 +139,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, Point2 cur_intersection; if (wall.intersects(traj,cur_intersection)) { collision = true; - if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) { + if (distance2(cur_pose.t(), cur_intersection) < distance2(cur_pose.t(), intersection)) { intersection = cur_intersection; closest_wall = wall; } @@ -154,7 +155,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, norm = norm / norm.norm(); // Simple check to make sure norm is on side closest robot - if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) + if (distance2(cur_pose.t(), intersection + norm) > distance2(cur_pose.t(), intersection - norm)) norm = - norm; // using the reflection diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index 38bba2ee3..c143bc36d 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -43,7 +43,7 @@ namespace gtsam { SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); } /** geometry */ - double length() const { return a_.distance(b_); } + double length() const { return distance2(a_, b_); } Point2 midpoint() const; /** diff --git a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp index 37cdfa0ba..6528f3f91 100644 --- a/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp +++ b/gtsam_unstable/geometry/tests/testSimPolygon2D.cpp @@ -16,7 +16,7 @@ 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); + Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0); // construct and extract data SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC); diff --git a/gtsam_unstable/geometry/tests/testSimWall2D.cpp b/gtsam_unstable/geometry/tests/testSimWall2D.cpp index 62f458402..3bde734b2 100644 --- a/gtsam_unstable/geometry/tests/testSimWall2D.cpp +++ b/gtsam_unstable/geometry/tests/testSimWall2D.cpp @@ -24,7 +24,7 @@ TEST(testSimWall2D2D, construction ) { /* ************************************************************************* */ TEST(testSimWall2D2D, equals ) { - Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3; + Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0); SimWall2D w1(p1, p2), w2(p1, p3); EXPECT(assert_equal(w1, w1)); EXPECT(assert_inequal(w1, w2)); @@ -34,7 +34,7 @@ TEST(testSimWall2D2D, equals ) { /* ************************************************************************* */ TEST(testSimWall2D2D, intersection1 ) { SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0); - Point2 pt; + Point2 pt(0,0); EXPECT(w1.intersects(w2)); EXPECT(w2.intersects(w1)); w1.intersects(w2, pt); diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 7509fe3b2..0d10b0123 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -24,17 +24,17 @@ namespace gtsam { * Ternary factor representing a visual measurement that includes inverse depth */ template -class InvDepthFactor3: public gtsam::NoiseModelFactor3 { +class InvDepthFactor3: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O - gtsam::Point2 measured_; ///< 2D measurement - boost::shared_ptr K_; ///< shared pointer to calibration object + Point2 measured_; ///< 2D measurement + boost::shared_ptr K_; ///< shared pointer to calibration object public: /// shorthand for base class type - typedef gtsam::NoiseModelFactor3 Base; + typedef NoiseModelFactor3 Base; /// shorthand for this class typedef InvDepthFactor3 This; @@ -43,7 +43,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactor3() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -55,8 +57,8 @@ public: * @param invDepthKey is the index of inverse depth * @param K shared pointer to the constant calibration */ - InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model, - const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) : + InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model, + const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) : Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {} /** Virtual destructor */ @@ -68,44 +70,43 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactor3", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); + return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives - gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const { + Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const { try { - InvDepthCamera3 camera(pose, K_); - gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); - return reprojectionError.vector(); + InvDepthCamera3 camera(pose, K_); + return camera.project(point, invDepth, H1, H2, H3) - measured_; } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,5); if (H3) *H2 = Matrix::Zero(2,1); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0).finished(); + return (Vector(1) << 0.0).finished(); } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } /** return the calibration object */ - inline const gtsam::Cal3_S2::shared_ptr calibration() const { + inline const Cal3_S2::shared_ptr calibration() const { return K_; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index e9f894faf..ad66eee5b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -41,7 +41,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant1() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -64,17 +66,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant1", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -86,22 +88,21 @@ public: Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0).finished(); + return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector6& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { if (H1) { (*H1) = numericalDerivative11( @@ -118,7 +119,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index ec2615ed6..c6b5d5af8 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -43,7 +43,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant2() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -67,17 +69,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant2", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && traits::Equals(this->referencePoint_, e->referencePoint_, tol); } @@ -89,22 +91,21 @@ public: Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } - return (gtsam::Vector(1) << 0.0).finished(); + return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector3& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { if (H1) { (*H1) = numericalDerivative11( @@ -121,7 +122,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index cc5878d85..3041f5f23 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -41,7 +41,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant3a() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -66,17 +68,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant3a", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -89,22 +91,21 @@ public: Point3 world_P_landmark = pose.transform_from(pose_P_landmark); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Vector3& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { if(H1) { (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); @@ -117,7 +118,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } @@ -160,7 +161,9 @@ public: typedef boost::shared_ptr shared_ptr; /// Default constructor - InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + InvDepthFactorVariant3b() : + measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } /** * Constructor @@ -185,17 +188,17 @@ public: * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "InvDepthFactorVariant3", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s, keyFormatter); - measured_.print(s + ".z"); + traits::Print(measured_, s + ".z"); } /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -208,23 +211,22 @@ public: Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark); // Project landmark into Pose2 PinholeCamera camera(pose2, *K_); - gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_); - return reprojectionError.vector(); + return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" << " moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl; - return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const { + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const { if(H1) (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); @@ -239,7 +241,7 @@ public: } /** return the measurement */ - const gtsam::Point2& imagePoint() const { + const Point2& imagePoint() const { return measured_; } diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index dc250fd9d..3e1263bb9 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -101,9 +101,9 @@ namespace gtsam { virtual ~MultiProjectionFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } /** * print @@ -143,20 +143,20 @@ namespace gtsam { // // if(body_P_sensor_) { // if(H1) { -// gtsam::Matrix H0; +// Matrix H0; // PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); // Point2 reprojectionError(camera.project(point, H1, H2) - measured_); // *H1 = *H1 * H0; -// return reprojectionError.vector(); +// return reprojectionError; // } else { // PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); // Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); +// return reprojectionError; // } // } else { // PinholeCamera camera(pose, *K_); // Point2 reprojectionError(camera.project(point, H1, H2) - measured_); -// return reprojectionError.vector(); +// return reprojectionError; // } // } @@ -168,20 +168,20 @@ namespace gtsam { try { if(body_P_sensor_) { if(H1) { - gtsam::Matrix H0; + Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); - return reprojectionError.vector(); + return reprojectionError; } } else { PinholeCamera camera(pose, *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); - return reprojectionError.vector(); + return reprojectionError; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index adfc1d108..19b8d56e6 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -54,7 +54,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {} + ProjectionFactorPPP() : + measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor @@ -94,9 +96,9 @@ namespace gtsam { virtual ~ProjectionFactorPPP() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } /** * print @@ -105,7 +107,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "ProjectionFactorPPP, z = "; - measured_.print(); + traits::Print(measured_); Base::print("", keyFormatter); } @@ -114,7 +116,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol) + && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol); } @@ -125,16 +127,15 @@ namespace gtsam { boost::optional H3 = boost::none) const { try { if(H1 || H2 || H3) { - gtsam::Matrix H0, H02; + Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), *K_); Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(transform), *K_); - Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H3, boost::none) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 2fd622ea1..369075abf 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -52,7 +52,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {} + ProjectionFactorPPPC() : + measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor @@ -89,9 +91,9 @@ namespace gtsam { virtual ~ProjectionFactorPPPC() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } /** * print @@ -100,7 +102,7 @@ namespace gtsam { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "ProjectionFactorPPPC, z = "; - measured_.print(); + traits::Print(measured_); Base::print("", keyFormatter); } @@ -109,7 +111,7 @@ namespace gtsam { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) - && this->measured_.equals(e->measured_, tol); + && traits::Equals(this->measured_, e->measured_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -120,16 +122,15 @@ namespace gtsam { boost::optional H4 = boost::none) const { try { if(H1 || H2 || H3 || H4) { - gtsam::Matrix H0, H02; + Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), K); Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; - return reprojectionError.vector(); + return reprojectionError; } else { PinholeCamera camera(pose.compose(transform), K); - Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); - return reprojectionError.vector(); + return camera.project(point, H1, H3, H4) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 0447f2e39..5e107ea58 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -101,11 +101,11 @@ public: // loop over all circles for(const Circle2& it: circles) { // distance between circle centers. - double d = circle1.center.dist(it.center); + double d = distance2(circle1.center, it.center); if (d < 1e-9) continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles - boost::optional fh = Point2::CircleCircleIntersection( + boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); // Check if this pair is better by checking h = fh->y() // if h is large, the intersections are well defined. @@ -116,15 +116,15 @@ public: } // use best fh to find actual intersection points - std::list intersections = Point2::CircleCircleIntersection( + std::list intersections = circleCircleIntersection( circle1.center, best_circle->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); for(const Circle2& it: circles) { - error1 += it.center.dist(p1); - error2 += it.center.dist(p2); + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; //gttoc_(triangulate); diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index aae4e413d..6c2f55195 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -48,9 +48,7 @@ public: Vector evaluateError(const Pose2& pose, const Point2& point, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Point2 d = pose.transform_to(point, H1, H2); - Point2 e = d - measured_; - return e.vector(); + return pose.transform_to(point, H1, H2) - measured_; } }; @@ -99,12 +97,12 @@ public: *H3 = D_e_point_g * D_point_g_base2; if (H4) *H4 = D_e_point_g * D_point_g_point; - return (d - measured_).vector(); + return d - measured_; } else { Pose2 pose_g = base1.compose(pose); Point2 point_g = base2.transform_from(point); Point2 d = pose_g.transform_to(point_g); - return (d - measured_).vector(); + return d - measured_; } } }; diff --git a/matlab.h b/matlab.h index 72889dc4b..5e144730d 100644 --- a/matlab.h +++ b/matlab.h @@ -91,7 +91,7 @@ Matrix extractPoint2(const Values& values) { Values::ConstFiltered points = values.filter(); Matrix result(points.size(), 2); for(const auto& key_value: points) - result.row(j++) = key_value.value.vector(); + result.row(j++) = key_value.value; return result; } diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp index 7af3f8cf6..99a97adc9 100644 --- a/python/handwritten/geometry/Point2.cpp +++ b/python/handwritten/geometry/Point2.cpp @@ -25,23 +25,26 @@ using namespace boost::python; using namespace gtsam; +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(distance_overloads, Point2::distance, 1, 3) +#endif void exportPoint2(){ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS class_("Point2", init<>()) .def(init()) .def(init()) .def("identity", &Point2::identity) - .def("dist", &Point2::dist) - .def("distance", &Point2::distance) + .def("distance", &Point2::distance, distance_overloads(args("q","H1","H2"))) .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) .def("norm", &Point2::norm) .def("print", &Point2::print, print_overloads(args("s"))) .def("unit", &Point2::unit) - .def("vector", &Point2::vector) + .def("vector", &Point2::vector, return_value_policy()) .def("x", &Point2::x) .def("y", &Point2::y) .def(self * other()) // __mult__ @@ -54,5 +57,5 @@ void exportPoint2(){ .def(repr(self)) .def(self == self) ; - -} \ No newline at end of file +#endif +} diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index de5c8e556..7935d6b37 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -25,31 +25,32 @@ using namespace boost::python; using namespace gtsam; +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1) +#endif void exportPoint3(){ +#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS class_("Point3") .def(init<>()) .def(init()) .def(init()) - .def("identity", &Point3::identity) - .staticmethod("identity") - .def("cross", &Point3::cross) - .def("distance", &Point3::distance) - .def("dot", &Point3::dot) - .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) - .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) - .def("normalized", &Point3::normalized) - .def("print", &Point3::print, print_overloads(args("s"))) -#ifndef GTSAM_USE_VECTOR3_POINTS .def("vector", &Point3::vector, return_value_policy()) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) -#endif + .def("print", &Point3::print, print_overloads(args("s"))) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("distance", &Point3::distance) + .def("cross", &Point3::cross) + .def("dot", &Point3::dot) + .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) + .def("normalized", &Point3::normalized) + .def("identity", &Point3::identity) + .staticmethod("identity") .def(self * other()) .def(other() * self) .def(self + self) @@ -58,7 +59,10 @@ class_("Point3") .def(self / other()) .def(self_ns::str(self)) .def(repr(self)) - .def(self == self) -; + .def(self == self); +#endif +class_("Point3Pair", init()) + .def_readwrite("first", &Point3Pair::first) + .def_readwrite("second", &Point3Pair::second); } diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 0012f5f45..3245652be 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -140,7 +140,7 @@ namespace simulated2D { /// Return error and optional derivative Vector evaluateError(const Pose& x, boost::optional H = boost::none) const { - return (prior(x, H) - measured_).vector(); + return (prior(x, H) - measured_); } virtual ~GenericPrior() {} @@ -186,7 +186,7 @@ namespace simulated2D { Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (odo(x1, x2, H1, H2) - measured_).vector(); + return (odo(x1, x2, H1, H2) - measured_); } virtual ~GenericOdometry() {} @@ -233,7 +233,7 @@ namespace simulated2D { Vector evaluateError(const Pose& x1, const Landmark& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); + return (mea(x1, x2, H1, H2) - measured_); } virtual ~GenericMeasurement() {} diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index ccc734cfd..7d399dc02 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -111,7 +111,7 @@ namespace simulated2D { * @return a scalar distance between values */ template - double range_trait(const T1& a, const T2& b) { return a.dist(b); } + double range_trait(const T1& a, const T2& b) { return distance2(a, b); } /** * Binary inequality constraint forcing the range between points diff --git a/tests/smallExample.h b/tests/smallExample.h index 215655593..d3a69b0bd 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -180,7 +180,7 @@ inline boost::shared_ptr sharedNonlinearFactorGraph( new NonlinearFactorGraph); // prior on x1 - Point2 mu; + Point2 mu(0,0); shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); nlfg->push_back(f1); @@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); - return (h(x) - z_).vector(); + return (h(x) - z_); } }; @@ -593,11 +593,11 @@ inline boost::tuple planarGraph(size_t N) { Values zeros; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) - zeros.insert(key(x, y), Point2()); + zeros.insert(key(x, y), Point2(0,0)); VectorValues xtrue; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) - xtrue.insert(key(x, y), Point2((double)x, (double)y).vector()); + xtrue.insert(key(x, y), Point2((double)x, (double)y)); // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index b0b748d95..a5d1a195c 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -181,7 +181,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_basics) { gtsam::Key key1 = 1, key2 = 2; - Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); + Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); EXPECT(!rangeBound.isGreaterThan()); @@ -220,7 +220,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { - Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); + Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); Symbol x1('x',1), x2('x',2); NonlinearFactorGraph graph; @@ -246,7 +246,7 @@ TEST( testBoundingConstraint, avoid_demo) { Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1); double radius = 1.0; - Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); + Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); Point2 odo(2.0, 0.0); NonlinearFactorGraph graph; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 4fda27cdb..b6b196acc 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -193,7 +193,7 @@ TEST(ExpressionFactor, Binary) { internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); - EXPECT(assert_equal(Point2(),value, 1e-9)); + EXPECT(assert_equal(Point2(0,0),value, 1e-9)); // trace.print(); // Expected Jacobians @@ -248,7 +248,7 @@ TEST(ExpressionFactor, Shallow) { internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); - EXPECT(assert_equal(Point2(),value, 1e-9)); + EXPECT(assert_equal(Point2(0,0),value, 1e-9)); // trace.print(); // Expected Jacobians diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 00ab4a16c..b3e8a3449 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -226,7 +226,7 @@ public: *H2 = Matrix::Identity(dim(),dim()); // Return the error between the prediction and the supplied value of p2 - return (p2 - prediction).vector(); + return (p2 - prediction); } }; @@ -400,7 +400,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { ExtendedKalmanFilter ekf(X(0), x_initial, P_initial); // Enter Predict-Update Loop - Point2 x_predict, x_update; + Point2 x_predict(0,0), x_update(0,0); for(unsigned int i = 0; i < 10; ++i){ // Create motion factor NonlinearMotionModel motionFactor(X(i), X(i+1)); diff --git a/tests/testLie.cpp b/tests/testLie.cpp index c153adf5f..a134a899c 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -40,7 +40,7 @@ template<> struct traits : internal::LieGroupTraits { << m.second.theta() << ")" << endl; } static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) { - return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol); + return traits::Equals(m1.first, m2.first, tol) && m1.second.equals(m2.second, tol); } }; } diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 65d26eb98..286e3ff5e 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -78,7 +78,7 @@ TEST(Manifold, Identity) { EXPECT_DOUBLES_EQUAL(0.0, traits::Identity(), 0.0); EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::Identity()))); EXPECT(assert_equal(Pose3(), traits::Identity())); - EXPECT(assert_equal(Point2(), traits::Identity())); + EXPECT(assert_equal(Point2(0,0), traits::Identity())); } //****************************************************************************** @@ -166,7 +166,7 @@ template<> struct traits : internal::ManifoldTraits TEST(Manifold, ProductManifold) { BOOST_CONCEPT_ASSERT((IsManifold)); - MyPoint2Pair pair1; + MyPoint2Pair pair1(Point2(0,0),Point2(0,0)); Vector4 d; d << 1,2,3,4; MyPoint2Pair expected(Point2(1,2),Point2(3,4)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 86080b673..95843e5ab 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -417,7 +417,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { graph.push_back(factor); Values initValues; - initValues.insert(key1, Point2()); + initValues.insert(key1, Point2(0,0)); initValues.insert(key2, badPt); Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); @@ -454,7 +454,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Values initialEstimate; initialEstimate.insert(x1, pt_x1); - initialEstimate.insert(x2, Point2()); + initialEstimate.insert(x2, Point2(0,0)); initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 3c49d54af..617a8cc1c 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -117,9 +117,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { new_factors += PriorFactor(lm3, landmark3, model2); // Initialize to origin - new_init.insert(lm1, Point2()); - new_init.insert(lm2, Point2()); - new_init.insert(lm3, Point2()); + new_init.insert(lm1, Point2(0,0)); + new_init.insert(lm2, Point2(0,0)); + new_init.insert(lm3, Point2(0,0)); } isamChol.update(new_factors, new_init); @@ -194,9 +194,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { new_factors += PriorFactor(lm3, landmark3, model2); // Initialize to origin - new_init.insert(lm1, Point2()); - new_init.insert(lm2, Point2()); - new_init.insert(lm3, Point2()); + new_init.insert(lm1, Point2(0,0)); + new_init.insert(lm2, Point2(0,0)); + new_init.insert(lm3, Point2(0,0)); } // Reconnect with observation later diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index 3a9b5297a..8950c636b 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -68,7 +68,7 @@ int main() { values.insert(2,Vector3(0,0,1)); typedef AdaptAutoDiff AdaptedSnavely; Expression expression(AdaptedSnavely(), Expression(1), Expression(2)); - f2 = boost::make_shared >(model, z.vector(), expression); + f2 = boost::make_shared >(model, z, expression); time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); return 0; diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 458f88db1..1578bb0a8 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -64,7 +64,7 @@ int main() long timeLog = clock(); Point2 measurement(0,0); for(int i = 0; i < n; i++) - measurement.localCoordinates(camera.project(point1)); + camera.project(point1)-measurement; long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl; diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 867953257..eb1a46606 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -62,11 +62,11 @@ int main(int argc, char* argv[]) { // readBAL converts to GTSAM format, so we need to convert back ! Pose3 openGLpose = gtsam2openGL(camera.pose()); Vector9 v9; - v9 << Pose3::Logmap(openGLpose), camera.calibration().vector(); + v9 << Pose3::Logmap(openGLpose), camera.calibration(); initial.insert(C(i++), v9); } for (const SfM_Track& track: db.tracks) { - Vector3 v3 = track.p.vector(); + Vector3 v3 = track.p; initial.insert(P(j++), v3); }