From a0abe68b6438881b804f7c0a5f5d6dac9ffe7968 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 29 Nov 2011 17:02:02 +0000 Subject: [PATCH] (in branch) Merged from trunk r7760-r7959 --- examples/PlanarSLAMSelfContained_advanced.cpp | 2 +- examples/SimpleRotation.cpp | 2 +- examples/easyPoint2KalmanFilter.cpp | 2 +- examples/elaboratePoint2KalmanFilter.cpp | 2 +- examples/vSLAMexample/vISAMexample.cpp | 4 +- gtsam.h | 6 + gtsam/base/Group.h | 5 +- gtsam/base/Lie.h | 202 +++++++++--------- gtsam/base/Manifold.h | 106 +++++---- gtsam/base/cholesky.cpp | 4 +- gtsam/base/cholesky.h | 2 +- gtsam/geometry/Point2.h | 94 ++++---- gtsam/geometry/Point3.h | 93 ++++---- gtsam/geometry/Pose2.h | 6 +- gtsam/geometry/Pose3.cpp | 4 +- gtsam/geometry/Pose3.h | 85 ++++---- gtsam/geometry/Rot2.h | 99 +++++---- gtsam/geometry/Rot3M.h | 118 +++++----- gtsam/geometry/Rot3Q.h | 118 +++++----- gtsam/geometry/StereoPoint2.h | 84 ++++---- gtsam/nonlinear/DoglegOptimizerImpl.h | 5 +- gtsam/nonlinear/GaussianISAM2.cpp | 1 - gtsam/nonlinear/GaussianISAM2.h | 14 +- gtsam/nonlinear/ISAM2-impl-inl.h | 44 ++-- gtsam/nonlinear/ISAM2-inl.h | 83 ++++--- gtsam/nonlinear/ISAM2.h | 38 ++-- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- gtsam/nonlinear/NonlinearISAM-inl.h | 21 +- gtsam/nonlinear/NonlinearISAM.h | 8 +- gtsam/nonlinear/NonlinearOptimizer-inl.h | 6 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 +- gtsam/nonlinear/TupleValues-inl.h | 23 -- gtsam/nonlinear/TupleValues.h | 25 ++- gtsam/nonlinear/Values-inl.h | 4 - gtsam/nonlinear/Values.h | 4 +- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam/slam/PartialPriorFactor.h | 56 ++--- gtsam/slam/PlanarSLAMOdometry.h | 2 +- gtsam/slam/ProjectionFactor.h | 5 + gtsam/slam/Simulated3D.cpp | 8 - gtsam/slam/planarSLAM.cpp | 3 - gtsam/slam/pose2SLAM.cpp | 2 - gtsam/slam/pose3SLAM.cpp | 2 - gtsam/slam/simulated2D.cpp | 8 - gtsam/slam/simulated2DOriented.cpp | 4 - gtsam/slam/smallExample.cpp | 1 - gtsam/slam/tests/testGeneralSFMFactor.cpp | 2 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- gtsam/slam/tests/testPose3SLAM.cpp | 5 +- gtsam/slam/visualSLAM.cpp | 3 +- myconfigure.ardrone | 1 + tests/testExtendedKalmanFilter.cpp | 2 +- tests/testGraph.cpp | 1 - tests/testNonlinearEquality.cpp | 2 - tests/testNonlinearFactor.cpp | 1 - tests/testNonlinearISAM.cpp | 2 +- tests/testRot3QOptimization.cpp | 2 +- tests/testTupleValues.cpp | 2 +- 58 files changed, 741 insertions(+), 701 deletions(-) create mode 100755 myconfigure.ardrone diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 549acabae..c8ee21997 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -34,7 +34,7 @@ #include // implementations for structures - needed if self-contained, and these should be included last -#include +#include #include #include #include diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index c31e91b5f..438459e64 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 3b8ee2980..f22d65f95 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include using namespace std; diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 73efc653c..8c8cbe147 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 300124c4e..ed3a68ba8 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -27,9 +27,7 @@ using namespace boost; #include #include #include -#include -#include -#include +#include #include "vSLAMutils.h" #include "Feature2D.h" diff --git a/gtsam.h b/gtsam.h index 52422cf3c..331bdd09a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -178,3 +178,9 @@ class PlanarSLAMOdometry { void print(string s) const; GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; }; + +class GaussianSequentialSolver { + GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); + GaussianBayesNet* eliminate(); + VectorValues* optimize(); +}; diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 3c56a541a..533c042c0 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -2,8 +2,6 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * A Group concept extends a Manifold - * * @date Nov 5, 2011 * @author Alex Cunningham */ @@ -13,7 +11,8 @@ namespace gtsam { /** - * Concept check for general Group structure + * This concept check enforces a Group structure on a variable type, + * in which we require the existence of basic algebraic operations. */ template class GroupConcept { diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 756790f16..932e069ef 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -14,28 +14,6 @@ * @brief Base class and basic functions for Lie types * @author Richard Roberts * @author Alex Cunningham - * - * This concept check provides a specialization on the Manifold type, - * in which the Manifolds represented require an algebra and group structure. - * All Lie types must also be a Manifold. - * - * The necessary functions to implement for Lie are defined - * below with additional details as to the interface. The - * concept checking function in class Lie will check whether or not - * the function exists and throw compile-time errors. - * - * Expmap around identity - * static T Expmap(const Vector& v); - * - * - * Logmap around identity - * static Vector Logmap(const T& p); - * - * Compute l0 s.t. l2=l1*l0, where (*this) is l1 - * A default implementation of between(*this, lp) is available: - * between_default() - * T between(const T& l2) const; - * */ @@ -46,89 +24,115 @@ namespace gtsam { - /** - * These core global functions can be specialized by new Lie types - * for better performance. - */ +/** + * These core global functions can be specialized by new Lie types + * for better performance. + */ - /** Compute l0 s.t. l2=l1*l0 */ - template - inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} +/** Compute l0 s.t. l2=l1*l0 */ +template +inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} - /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ - template - inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} +/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ +template +inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} - /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ - template - inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} +/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ +template +inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} - /** - * Concept check class for Lie group type - * - * T is the Lie type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class LieConcept { - private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { +/** + * Concept check class for Lie group type + * + * This concept check provides a specialization on the Manifold type, + * in which the Manifolds represented require an algebra and group structure. + * All Lie types must also be a Manifold. + * + * The necessary functions to implement for Lie are defined + * below with additional details as to the interface. The + * concept checking function in class Lie will check whether or not + * the function exists and throw compile-time errors. + * + * The exponential map is a specific retraction for Lie groups, + * which maps the tangent space in canonical coordinates back to + * the underlying manifold. Because we can enforce a group structure, + * a retraction of delta v, with tangent space centered at x1 can be performed + * as x2 = x1.compose(Expmap(v)). + * + * Expmap around identity + * static T Expmap(const Vector& v); + * + * Logmap around identity + * static Vector Logmap(const T& p); + * + * Compute l0 s.t. l2=l1*l0, where (*this) is l1 + * A default implementation of between(*this, lp) is available: + * between_default() + * T between(const T& l2) const; + * + * By convention, we use capital letters to designate a static function + * + * @tparam T is a Lie type, like Point2, Pose3, etc. + */ +template +class LieConcept { +private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); + /** expmap around identity */ + T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); + /** Logmap around identity */ + Vector logmap_identity_ret = T::Logmap(t); - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = t.between(t2); - } - - }; - - /** - * Three term approximation of the Baker�Campbell�Hausdorff formula - * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) - * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH - * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 - * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula - */ - /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? - template - T BCH(const T& X, const T& Y) { - static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; - T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = t.between(t2); } - /** - * Declaration of wedge (see Murray94book) used to convert - * from n exponential coordinates to n*n element of the Lie algebra - */ - template Matrix wedge(const Vector& x); +}; - /** - * Exponential map given exponential coordinates - * class T needs a wedge<> function and a constructor from Matrix - * @param x exponential coordinates, vector of size n - * @ return a T - */ - template - T expm(const Vector& x, int K=7) { - Matrix xhat = wedge(x); - return expm(xhat,K); - } +/** + * Three term approximation of the Baker�Campbell�Hausdorff formula + * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) + * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH + * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 + * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula + */ +/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? +template +T BCH(const T& X, const T& Y) { + static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; + T X_Y = bracket(X, Y); + return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, + bracket(X, X_Y)); +} + +/** + * Declaration of wedge (see Murray94book) used to convert + * from n exponential coordinates to n*n element of the Lie algebra + */ +template Matrix wedge(const Vector& x); + +/** + * Exponential map given exponential coordinates + * class T needs a wedge<> function and a constructor from Matrix + * @param x exponential coordinates, vector of size n + * @ return a T + */ +template +T expm(const Vector& x, int K=7) { + Matrix xhat = wedge(x); + return expm(xhat,K); +} } // namespace gtsam @@ -141,11 +145,11 @@ namespace gtsam { * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; #define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 87506f31f..430be4f7d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -12,23 +12,7 @@ /** * @file Manifold.h * @brief Base class and basic functions for Manifold types - * @author Richard Roberts * @author Alex Cunningham - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * - * Returns dimensionality of the tangent space - * inline size_t dim() const; - * - * Returns Retraction update of T - * T retract(const Vector& v) const; - * - * Returns inverse retraction operation - * Vector localCoordinates(const T& lp) const; - * */ #pragma once @@ -38,40 +22,70 @@ namespace gtsam { - /** - * Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * T is the Manifold type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class ManifoldConcept { - private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { +/** + * Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. + * + * A manifold defines a space in which there is a notion of a linear tangent space + * that can be centered around a given point on the manifold. These nonlinear + * spaces may have such properties as wrapping around (as is the case with rotations), + * which might make linear operations on parameters not return a viable element of + * the manifold. + * + * We perform optimization by computing a linear delta in the tangent space of the + * current estimate, and then apply this change using a retraction operation, which + * maps the change in tangent space back to the manifold itself. + * + * There may be multiple possible retractions for a given manifold, which can be chosen + * between depending on the computational complexity. The important criteria for + * the creation for the retract and localCoordinates functions is that they be + * inverse operations. + * + * Returns dimensionality of the tangent space, which may be smaller than the number + * of nonlinear parameters. + * size_t dim() const; + * + * Returns a new T that is a result of updating *this with the delta v after pulling + * the updated value back to the manifold T. + * T retract(const Vector& v) const; + * + * Returns the linear coordinates of lp in the tangent space centered around *this. + * Vector localCoordinates(const T& lp) const; + * + * By convention, we use capital letters to designate a static function + * @tparam T is a Lie type, like Point2, Pose3, etc. + */ +template +class ManifoldConcept { +private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); - } - }; + /** + * Returns local coordinates of another object + */ + Vector localCoords_ret = t.localCoordinates(t2); + } +}; } // namespace gtsam diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index a437c9e13..16f07f5ab 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -179,14 +179,14 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) throw NegativeMatrixException(); } - Vector sqrtD = ldlt.vectorD().cwiseSqrt(); + Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL if (debug) cout << "Dsqrt: " << sqrtD << endl; // U = sqrtD * L^ Matrix U = ldlt.matrixU(); // we store the permuted upper triangular matrix - ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; + ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; // toc(1, "ldl"); diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 155b36418..0c4689113 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -33,7 +33,7 @@ struct NegativeMatrixException : public std::exception { Matrix A; ///< The original matrix attempted to factor Matrix U; ///< The produced upper-triangular factor Matrix D; ///< The produced diagonal factor - Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_A), D(_D) {} + Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_U), D(_D) {} void print(const std::string& str = "") const { std::cout << str << "\n"; gtsam::print(A, " A: "); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 5b335f564..12f299637 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -42,18 +42,28 @@ public: Point2(double x, double y): x_(x), y_(y) {} Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @name Testable + /// @{ - /** print with optional string */ + /// print with optional string void print(const std::string& s = "") const; - /** equals with an tolerance, prints out message if unequal*/ + /// equals with an tolerance, prints out message if unequal bool equals(const Point2& q, double tol = 1e-9) const; - // Group requirements + /// @} + /// @name Group + /// @{ - /** "Compose", just adds the coordinates of two points. With optional derivatives */ + /// identity + inline static Point2 identity() { + return Point2(); + } + + /// "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() + inline Point2 inverse() const { return Point2(-x_, -y_); } + + /// "Compose", just adds the coordinates of two points. With optional derivatives inline Point2 compose(const Point2& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -62,33 +72,60 @@ public: return *this + p2; } - /** identity */ - inline static Point2 identity() { - return Point2(); - } + /** operators */ + inline Point2 operator- () const {return Point2(-x_,-y_);} + inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} + inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} + inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ - inline Point2 inverse() const { return Point2(-x_, -y_); } + /// @} + /// @name Manifold + /// @{ - // Manifold requirements + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { return dimension; } - /** Size of the tangent space */ + /// Dimensionality of tangent space = 2 DOF inline size_t dim() const { return dimension; } - /** Updates a with tangent space delta */ + /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - /** Lie requirements */ + /// @} + /// @name Lie Group + /// @{ - /** Exponential map around identity - just create a Point2 from a vector */ + /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } - /** Log map around identity - just return the Point2 as a vector */ + /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + /// @} + /// @name Vector Operators + /// @{ + + /** norm of point */ + double norm() const; + + /** creates a unit vector */ + Point2 unit() const { return *this/norm(); } + + /** distance between two points */ + inline double dist(const Point2& p2) const { + return (p2 - *this).norm(); + } + + /** operators */ + inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} + inline void operator *= (double s) {x_*=s;y_*=s;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + + /// @} /** "Between", subtracts point coordinates */ inline Point2 between(const Point2& p2, @@ -106,27 +143,6 @@ public: /** return vectorized form (column-wise) */ Vector vector() const { return Vector_(2, x_, y_); } - /** operators */ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} - inline Point2 operator- () const {return Point2(-x_,-y_);} - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - - /** norm of point */ - double norm() const; - - /** creates a unit vector */ - Point2 unit() const { return *this/norm(); } - - /** distance between two points */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 2f8c9f8cc..a01f5009b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -46,29 +46,28 @@ namespace gtsam { Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {} + /// @name Testable + /// @{ + /** print with optional string */ void print(const std::string& s = "") const; /** equals with an tolerance */ bool equals(const Point3& p, double tol = 1e-9) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Group + /// @{ - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return dimension; } - - /** identity */ + /// identity for group operation inline static Point3 identity() { return Point3(); } - /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ + /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } - /** "Compose" - just adds coordinates of two points */ + /// "Compose" - just adds coordinates of two points inline Point3 compose(const Point3& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -77,20 +76,58 @@ namespace gtsam { return *this + p2; } + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + inline size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } + + /// @} + /// @name Lie Group + /// @{ + /** Exponential map at identity - just create a Point3 from x,y,z */ static inline Point3 Expmap(const Vector& v) { return Point3(v); } /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - // Manifold requirements + /// @} + /// @name Vector Operators + /// @{ - inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + Point3 operator - () const { return Point3(-x_,-y_,-z_);} + bool operator ==(const Point3& q) const; + Point3 operator + (const Point3& q) const; + Point3 operator - (const Point3& q) const; + Point3 operator * (double s) const; + Point3 operator / (double s) const; - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } + /** distance between two points */ + double dist(const Point3& p2) const { + return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); + } + + /** dot product */ + double norm() const; + + /** cross product @return this x q */ + Point3 cross(const Point3 &q) const; + + /** dot product @return this * q*/ + double dot(const Point3 &q) const; + + /// @} /** Between using the default implementation */ inline Point3 between(const Point3& p2, @@ -112,19 +149,6 @@ namespace gtsam { inline double y() const {return y_;} inline double z() const {return z_;} - /** operators */ - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - bool operator ==(const Point3& q) const; - Point3 operator + (const Point3& q) const; - Point3 operator - (const Point3& q) const; - Point3 operator * (double s) const; - Point3 operator / (double s) const; - - /** distance between two points */ - double dist(const Point3& p2) const { - return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); - } - /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; @@ -133,15 +157,6 @@ namespace gtsam { Point3 sub (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** cross product @return this x q */ - Point3 cross(const Point3 &q) const; - - /** dot product @return this * q*/ - double dot(const Point3 &q) const; - - /** dot product */ - double norm() const; - private: /** Serialization function */ friend class boost::serialization::access; @@ -154,7 +169,7 @@ namespace gtsam { } }; - /** Syntactic sugar for multiplying coordinates by a scalar s*p */ + /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0e4bf4824..de311249a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -79,7 +79,6 @@ public: *this = Expmap(v); } - /// @name Testable /// @{ @@ -94,9 +93,7 @@ public: /// @{ /// identity for group operation - inline static Pose2 identity() { - return Pose2(); - } + inline static Pose2 identity() { return Pose2(); } /// inverse transformation with derivatives Pose2 inverse(boost::optional H1=boost::none) const; @@ -123,7 +120,6 @@ public: /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes inline static size_t Dim() { return dimension; } - /// Dimensionality of tangent space = 3 DOF inline size_t dim() const { return dimension; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index a7e687af1..fa6f02828 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -188,7 +188,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { if (H1) { #ifdef CORRECT_POSE3_EXMAP - *H1 = AdjointMap(inverse(p2)); + *H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); @@ -216,7 +216,7 @@ namespace gtsam { Pose3 Pose3::inverse(boost::optional H1) const { if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } + { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? #else { Matrix Rt = R_.transpose(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2fd5d7091..f03dcde06 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -65,66 +65,77 @@ namespace gtsam { /** convert to 4*4 matrix */ Matrix matrix() const; - /** print with optional string */ + /// @name Testable + /// @{ + + /// print with optional string void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ + /// assert equality up to a tolerance bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Compose two poses */ + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Pose3 identity() { return Pose3(); } + + /// inverse transformation with derivatives + Pose3 inverse(boost::optional H1=boost::none) const; + + ///compose this transformation onto another (first *this and then p2) + Pose3 compose(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); } - Pose3 transform_to(const Pose3& pose) const; + /// @} + /// @name Manifold + /// @{ - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } + /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes + static size_t Dim() { return dimension; } - /** Lie requirements */ - - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space = 6 DOF size_t dim() const { return dimension; } - /** identity */ - static Pose3 identity() { - return Pose3(); - } - /** - * Derivative of inverse - */ - Pose3 inverse(boost::optional H1=boost::none) const; + /** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose + Pose3 retract(const Vector& d) const; - /** - * composes two poses (first (*this) then p2) - * with optional derivatives - */ - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose + Vector localCoordinates(const Pose3& T2) const; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map from Lie algebra se(3) to SE(3) + static Pose3 Expmap(const Vector& xi); + + /// Exponential map from SE(3) to Lie algebra se(3) + static Vector Logmap(const Pose3& p); + + /// @} + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } + + Pose3 transform_to(const Pose3& pose) const; /** receives the point in Pose coordinates and transforms it to world coordinates */ Point3 transform_from(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** syntactic sugar for transform */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** receives the point in world coordinates and transforms it to Pose coordinates */ Point3 transform_to(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Exponential map around another pose */ - Pose3 retract(const Vector& d) const; - - /** Logarithm map around another pose T1 */ - Vector localCoordinates(const Pose3& T2) const; - - /** non-approximated versions of Expmap/Logmap */ - static Pose3 Expmap(const Vector& xi); - static Vector Logmap(const Pose3& p); - /** * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 3a8fb86f0..1a6d91b20 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -109,27 +109,25 @@ namespace gtsam { return s_; } + /// @name Testable + /// @{ + /** print */ void print(const std::string& s = "theta") const; /** equals with an tolerance */ bool equals(const Rot2& R, double tol = 1e-9) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { - return dimension; - } - - /** Lie requirements */ - - /** Dimensionality of the tangent space */ - inline size_t dim() const { - return dimension; - } + /// @} + /// @name Group + /// @{ /** identity */ - inline static Rot2 identity() { - return Rot2(); + inline static Rot2 identity() { return Rot2(); } + + /** The inverse rotation - negative angle */ + Rot2 inverse() const { + return Rot2(c_, -s_); } /** Compose - make a new rotation by adding angles */ @@ -140,7 +138,41 @@ namespace gtsam { return *this * R1; } - /** Expmap around identity - create a rotation from an angle */ + /** Compose - make a new rotation by adding angles */ + Rot2 operator*(const Rot2& R) const { + return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); + } + + /** syntactic sugar for rotate */ + inline Point2 operator*(const Point2& p) const { + return rotate(p); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { + return dimension; + } + + /// Dimensionality of the tangent space, DOF = 1 + inline size_t dim() const { + return dimension; + } + + /// Updates a with tangent space delta + inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + + /// Returns inverse retraction + inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /// Expmap around identity - create a rotation from an angle static Rot2 Expmap(const Vector& v) { if (zero(v)) return (Rot2()); @@ -148,19 +180,23 @@ namespace gtsam { return Rot2::fromAngle(v(0)); } - /** Logmap around identity - return the angle of the rotation */ + /// Logmap around identity - return the angle of the rotation static inline Vector Logmap(const Rot2& r) { return Vector_(1, r.theta()); } - // Manifold requirements + /// @} + /// @name Vector Operators + /// @{ - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + /** + * Creates a unit vector as a Point2 + */ + inline Point2 unit() const { + return Point2(c_, s_); + } - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + /// @} /** Between using the default implementation */ inline Rot2 between(const Rot2& p2, boost::optional H1 = @@ -176,16 +212,6 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - Rot2 operator*(const Rot2& R) const { - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } - /** * rotate point from rotated coordinate frame to * world = R*p @@ -193,11 +219,6 @@ namespace gtsam { Point2 rotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** syntactic sugar for rotate */ - inline Point2 operator*(const Point2& p) const { - return rotate(p); - } - /** * rotate point from world to rotated * frame = R'*p @@ -205,12 +226,6 @@ namespace gtsam { Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** - * Creates a unit vector as a Point2 - */ - inline Point2 unit() const { - return Point2(c_, s_); - } private: /** Serialization function */ diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 0c17060b3..e41e01ef3 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -130,12 +130,76 @@ namespace gtsam { static Rot3M rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3M& p, double tol = 1e-9) const; + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Rot3M identity() { + return Rot3M(); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3M compose(const Rot3M& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + inline Point3 operator*(const Point3& p) const { return rotate(p);} + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() + Rot3M inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3M( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3M Expmap(const Vector& v) { + if(zero(v)) return Rot3M(); + else return rodriguez(v); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3M& R); + + /// @} + /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -176,54 +240,6 @@ namespace gtsam { r1_.z(), r2_.z(), r3_.z()).finished()); } - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3M compose(const Rot3M& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3M Expmap(const Vector& v) { - if(zero(v)) return Rot3M(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3M identity() { - return Rot3M(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3M& R); - - // Manifold requirements - - Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() - Rot3M inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3M( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); - } - /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ @@ -236,12 +252,6 @@ namespace gtsam { return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { return rotate(p);} - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h index d21f7e2fb..5986a8103 100644 --- a/gtsam/geometry/Rot3Q.h +++ b/gtsam/geometry/Rot3Q.h @@ -127,12 +127,76 @@ namespace gtsam { static Rot3Q rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3Q& p, double tol = 1e-9) const; + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Rot3Q identity() { + return Rot3Q(); + } + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() + Rot3Q inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3Q(quaternion_.inverse()); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3Q compose(const Rot3Q& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + inline Point3 operator*(const Point3& p) const { + Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); + return Point3(r(0), r(1), r(2)); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose + Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3Q Expmap(const Vector& v) { + if(zero(v)) return Rot3Q(); + else return rodriguez(v); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3Q& R); + + /// @} + /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -167,51 +231,6 @@ namespace gtsam { */ Quaternion toQuaternion() const { return quaternion_; } - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3Q compose(const Rot3Q& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3Q Expmap(const Vector& v) { - if(zero(v)) return Rot3Q(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3Q identity() { - return Rot3Q(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3Q& R); - - // Manifold requirements - - Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() - Rot3Q inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3Q(quaternion_.inverse()); - } - /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ @@ -222,15 +241,6 @@ namespace gtsam { /** compose two rotations */ Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { - Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); - return Point3(r(0), r(1), r(2)); - } - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index e493b0047..b0f2d738c 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -44,6 +44,9 @@ namespace gtsam { uL_(uL), uR_(uR), v_(v) { } + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="") const; @@ -53,49 +56,53 @@ namespace gtsam { - q.v_) < tol); } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Group + /// @{ - /** Lie requirements */ - inline size_t dim() const { return dimension; } + /// identity + inline static StereoPoint2 identity() { return StereoPoint2(); } - /** convert to vector */ - Vector vector() const { - return Vector_(3, uL_, uR_, v_); + /// inverse + inline StereoPoint2 inverse() const { + return StereoPoint2()- (*this); } - /** add two stereo points */ - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } - - /** subtract two stereo points */ - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /* - * convenient function to get a Point2 from the left image - */ - inline Point2 point2(){ - return Point2(uL_, v_); - } - - /** "Compose", just adds the coordinates of two points. */ + /// "Compose", just adds the coordinates of two points. inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1; } - /** identity */ - inline static StereoPoint2 identity() { - return StereoPoint2(); + /// add two stereo points + StereoPoint2 operator+(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); } - /** inverse */ - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); + /// subtract two stereo points + StereoPoint2 operator-(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + inline size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + /** Exponential map around identity - just create a Point2 from a vector */ static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); @@ -106,14 +113,17 @@ namespace gtsam { return p.vector(); } - // Manifold requirements + /// @}} - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /** convert to vector */ + Vector vector() const { + return Vector_(3, uL_, uR_, v_); + } - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /** convenient function to get a Point2 from the left image */ + inline Point2 point2(){ + return Point2(uL_, v_); + } inline StereoPoint2 between(const StereoPoint2& p2) const { return gtsam::between_default(*this, p2); diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index eefa76df1..bfb1c1878 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -183,7 +183,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( const double dx_d_norm = result.dx_d.vector().norm(); const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta - if(mode == ONE_STEP_PER_ITERATION) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { @@ -217,8 +216,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( Delta *= 0.5; if(Delta > 1e-5) stay = true; - else + else { + if(verbose) cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << endl; stay = false; + } } } diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index e4476b260..7df21bf2c 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -5,7 +5,6 @@ */ #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 943e53aa1..d4d177815 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -9,10 +9,7 @@ #pragma once #include -#include -#include -#include -#include +#include namespace gtsam { @@ -26,15 +23,16 @@ namespace gtsam { * * @tparam VALUES The Values or TupleValues\Emph{N} that contains the * variables. + * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph */ -template -class GaussianISAM2 : public ISAM2 { +template > +class GaussianISAM2 : public ISAM2 { public: /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} + GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} + GaussianISAM2() : ISAM2() {} }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 95a7ca346..d414e13e9 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -15,18 +15,14 @@ * @author Michael Kaess, Richard Roberts */ -#include - -#include - -#include - namespace gtsam { -template -struct ISAM2::Impl { +using namespace std; - typedef ISAM2 ISAM2Type; +template +struct ISAM2::Impl { + + typedef ISAM2 ISAM2Type; struct PartialSolveResult { typename ISAM2Type::sharedClique bayesTree; @@ -58,7 +54,7 @@ struct ISAM2::Impl { * @param factors The factors from which to extract the variables * @return The set of variables indices from the factors */ - static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); + static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -100,7 +96,7 @@ struct ISAM2::Impl { * recalculate its delta. */ static void ExpmapMasked(VALUES& values, const Permuted& delta, - const Ordering& ordering, const vector& mask, + const Ordering& ordering, const std::vector& mask, boost::optional&> invalidateIfDebug = boost::optional&>()); /** @@ -138,15 +134,15 @@ struct _VariableAdder { }; /* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( +template +void ISAM2::Impl::AddVariables( const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); if(debug) newTheta.print("The new variables are: "); // Add the new keys onto the ordering, add zeros to the delta for the new variables - vector dims(newTheta.dims(*newTheta.orderingArbitrary())); + std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; const size_t newDim = accumulate(dims.begin(), dims.end(), 0); const size_t originalDim = delta->dim(); @@ -166,8 +162,8 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { +template +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const Symbol& key, factor->keys()) { @@ -178,8 +174,8 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Orderin } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { +template +FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { FastSet relinKeys; for(Index var=0; var(); @@ -191,7 +187,7 @@ FastSet ISAM2::Impl::CheckRelinearization(Permuted +template void ISAM2::Impl::FindAll(typename BayesTreeClique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? @@ -245,8 +241,8 @@ struct _SelectiveExpmapAndClear { }; /* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, +template +void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -260,9 +256,9 @@ void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permut } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +template +typename ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 461964359..4ed103615 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -21,22 +21,12 @@ #include // for operator += using namespace boost::assign; -#include -#include -#include - #include #include -#include -#include -#include #include - #include +#include -#include - -#include #include @@ -50,24 +40,24 @@ static const bool latestLast = true; static const bool structuralLast = false; /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): +template +ISAM2::ISAM2(const ISAM2Params& params): delta_(Permutation(), deltaUnpermuted_), params_(params) {} /* ************************************************************************* */ -template -ISAM2::ISAM2(): +template +ISAM2::ISAM2(): delta_(Permutation(), deltaUnpermuted_) {} /* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +template +FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph > allAffected; + FactorGraph > allAffected; FastList indices; BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); @@ -89,15 +79,15 @@ FastList ISAM2::getAffectedFactors(const FastList +template FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); toc(1,"getAffectedFactors"); - NonlinearFactorGraph nonlinearAffectedFactors; + GRAPH nonlinearAffectedFactors; tic(2,"affectedKeysSet"); // for fast lookup below @@ -128,9 +118,9 @@ ISAM2::relinearizeAffectedFactors(const FastList& af /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -template -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +template +FactorGraph::CacheFactor> +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -140,9 +130,9 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { // find the last variable that was eliminated Index key = (*orphan)->frontals().back(); #ifndef NDEBUG -// typename BayesNet::const_iterator it = orphan->end(); -// const Conditional& lastConditional = **(--it); -// typename Conditional::const_iterator keyit = lastConditional.endParents(); +// typename BayesNet::const_iterator it = orphan->end(); +// const CONDITIONAL& lastCONDITIONAL = **(--it); +// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); // const Index lastKey = *(--keyit); // assert(key == lastKey); #endif @@ -154,8 +144,8 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -template -boost::shared_ptr > ISAM2::recalculate( +template +boost::shared_ptr > ISAM2::recalculate( const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -174,8 +164,8 @@ boost::shared_ptr > ISAM2::recalculate( if (counter>0) { // cannot call on empty tree GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxConditionalSize; - avgClique = cstats.avgConditionalSize; + maxClique = cstats.maxCONDITIONALSize; + avgClique = cstats.avgCONDITIONALSize; numCliques = cdata.conditionalSizes.size(); nnzR = calculate_nnz(this->root()); } @@ -296,7 +286,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(5,"eliminate"); tic(6,"insert"); - Base::clear(); + BayesTree::clear(); this->insert(newRoot); toc(6,"insert"); @@ -422,9 +412,9 @@ boost::shared_ptr > ISAM2::recalculate( } /* ************************************************************************* */ -template -ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, bool force_relinearize) { +template +ISAM2Result ISAM2::update( + const GRAPH& newFactors, const Values& newTheta, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); static const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -494,14 +484,14 @@ ISAM2Result ISAM2::update( Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); toc(6,"expmap"); - result.variablesRelinearized = markedRelinMask.size(); + result.variablesRelinearized = markedKeys.size(); #ifndef NDEBUG lastRelinVariables_ = markedRelinMask; #endif } else { -#ifndef NDEBUG result.variablesRelinearized = 0; +#ifndef NDEBUG lastRelinVariables_ = vector(ordering_.nVars(), false); #endif } @@ -560,19 +550,28 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -template -Values ISAM2::calculateEstimate() const { +template +VALUES ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted - Values ret(theta_); + VALUES ret(theta_); vector mask(ordering_.nVars(), true); Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; } /* ************************************************************************* */ -template -Values ISAM2::calculateBestEstimate() const { +template +template +typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return getLinearizationPoint()[key].retract(delta); +} + +/* ************************************************************************* */ +template +VALUES ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); return theta_.retract(delta, ordering_); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index e76a33105..5e84d44d8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -19,21 +19,8 @@ #pragma once -#include -#include -#include -#include - -#include -#include -#include -#include #include -#include -#include #include -#include -#include namespace gtsam { @@ -204,8 +191,9 @@ private: * to the constructor, then add measurements and variables as they arrive using the update() * method. At any time, calculateEstimate() may be called to obtain the current * estimate of all variables. + * */ -template +template > class ISAM2: public BayesTree > { protected: @@ -229,7 +217,7 @@ protected: Permuted delta_; /** All original nonlinear factors are stored here to use during relinearization */ - NonlinearFactorGraph nonlinearFactors_; + GRAPH nonlinearFactors_; /** @brief The current elimination ordering Symbols to Index (integer) keys. * @@ -251,6 +239,8 @@ public: typedef BayesTree > Base; ///< The BayesTree base class typedef ISAM2 This; ///< This class + typedef VALUES Values; + typedef GRAPH Graph; /** Create an empty ISAM2 instance */ ISAM2(const ISAM2Params& params); @@ -280,17 +270,27 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, + ISAM2Result update(const GRAPH& newFactors, const VALUES& newTheta, bool force_relinearize = false); /** Access the current linearization point */ const VALUES& getLinearizationPoint() const {return theta_;} /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. + * This delta is incomplete because it was not updated below wildfire_threshold. If only + * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ VALUES calculateEstimate() const; + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. + * @param key + * @return + */ + template + typename KEY::Value calculateEstimate(const KEY& key) const; + /// @name Public members for non-typical usage //@{ @@ -305,7 +305,7 @@ public: const Permuted& getDelta() const { return delta_; } /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the current ordering */ const Ordering& getOrdering() const { return ordering_; } @@ -332,3 +332,5 @@ private: }; // ISAM2 } /// namespace gtsam + +#include diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index c7f10475a..2cea72e7e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -58,7 +58,7 @@ namespace gtsam { template void add(const F& factor) { - push_back(boost::shared_ptr(new F(factor))); + this->push_back(boost::shared_ptr(new F(factor))); } /** diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 761f2e032..16ec47247 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -24,14 +24,13 @@ #include #include #include -#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, +template +void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -63,8 +62,8 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +template +void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; @@ -90,8 +89,8 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -Values NonlinearISAM::estimate() const { +template +VALUES NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else @@ -99,14 +98,14 @@ Values NonlinearISAM::estimate() const { } /* ************************************************************************* */ -template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { +template +Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { return isam_.marginalCovariance(ordering_[key]); } /* ************************************************************************* */ -template -void NonlinearISAM::print(const std::string& s) const { +template +void NonlinearISAM::print(const std::string& s) const { cout << "ISAM - " << s << ":" << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; isam_.print("GaussianISAM"); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 55bff857f..8c6d2499d 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include @@ -25,11 +24,12 @@ namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ -template +template > class NonlinearISAM { public: - typedef gtsam::NonlinearFactorGraph Factors; + typedef VALUES Values; + typedef GRAPH Factors; protected: @@ -101,3 +101,5 @@ public: }; } // \namespace gtsam + +#include diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 15a7d341d..d0d5009eb 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -200,6 +200,8 @@ namespace gtsam { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + if(verbosity >= Parameters::ERROR) + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { lambda *= factor; @@ -212,6 +214,8 @@ namespace gtsam { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + if(verbosity >= Parameters::ERROR) + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { lambda *= factor; @@ -306,7 +310,7 @@ namespace gtsam { S solver(*graph_->linearize(*values_, *ordering_)); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), - *graph_, *values_, *ordering_, error_); + *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR); shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); cout << "newValues: " << newValues.get() << endl; return newValuesErrorLambda_(newValues, result.f_error, result.Delta); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 5e0a6003e..4d7d76bfc 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -69,8 +69,12 @@ bool check_convergence( } bool converged = (relativeErrorTreshold && (relativeDecrease < relativeErrorTreshold)) || (absoluteDecrease < absoluteErrorTreshold); - if (verbosity >= 1 && converged) - cout << "converged" << endl; + if (verbosity >= 1 && converged) { + if(absoluteDecrease >= 0.0) + cout << "converged" << endl; + else + cout << "Warning: stopping nonlinear iterations because error increased" << endl; + } return converged; } diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h index 09e510093..a9d6300c1 100644 --- a/gtsam/nonlinear/TupleValues-inl.h +++ b/gtsam/nonlinear/TupleValues-inl.h @@ -18,29 +18,6 @@ #pragma once -#include -#include - -// TupleValues instantiations for N = 1-6 -#define INSTANTIATE_TUPLE_VALUES1(Values1) \ - template class TupleValues1; - -#define INSTANTIATE_TUPLE_VALUES2(Values1, Values2) \ - template class TupleValues2; - -#define INSTANTIATE_TUPLE_VALUES3(Values1, Values2, Values3) \ - template class TupleValues3; - -#define INSTANTIATE_TUPLE_VALUES4(Values1, Values2, Values3, Values4) \ - template class TupleValues4; - -#define INSTANTIATE_TUPLE_VALUES5(Values1, Values2, Values3, Values4, Values5) \ - template class TupleValues5; - -#define INSTANTIATE_TUPLE_VALUES6(Values1, Values2, Values3, Values4, Values5, Values6) \ - template class TupleValues6; - - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index a809c360f..9f1cbebae 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -428,9 +428,12 @@ namespace gtsam { typedef C2 Values2; typedef C3 Values3; + typedef TupleValues > > Base; + typedef TupleValues3 This; + TupleValues3() {} - TupleValues3(const TupleValues > >& values); - TupleValues3(const TupleValues3& values); + TupleValues3(const Base& values); + TupleValues3(const This& values); TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3); // access functions @@ -466,16 +469,18 @@ namespace gtsam { template class TupleValues5 : public TupleValues > > > > { public: - // typedefs typedef C1 Values1; typedef C2 Values2; typedef C3 Values3; typedef C4 Values4; typedef C5 Values5; + typedef TupleValues > > > > Base; + typedef TupleValues5 This; + TupleValues5() {} - TupleValues5(const TupleValues5& values); - TupleValues5(const TupleValues > > > >& values); + TupleValues5(const This& values); + TupleValues5(const Base& values); TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5); @@ -490,7 +495,6 @@ namespace gtsam { template class TupleValues6 : public TupleValues > > > > > { public: - // typedefs typedef C1 Values1; typedef C2 Values2; typedef C3 Values3; @@ -498,9 +502,12 @@ namespace gtsam { typedef C5 Values5; typedef C6 Values6; + typedef TupleValues > > > > > Base; + typedef TupleValues6 This; + TupleValues6() {} - TupleValues6(const TupleValues6& values); - TupleValues6(const TupleValues > > > > >& values); + TupleValues6(const This& values); + TupleValues6(const Base& values); TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5, const Values6& cfg6); // access functions @@ -513,3 +520,5 @@ namespace gtsam { }; } + +#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4a4e1898f..e53c14944 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -28,10 +28,6 @@ #include #include -#include - -#define INSTANTIATE_VALUES(J) template class Values; - using namespace std; namespace gtsam { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bb45435ce..95f2409be 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -229,5 +229,7 @@ namespace gtsam { } }; -} +} // \namespace gtsam + +#include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 0cabc5f6d..60c64b164 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -22,7 +22,7 @@ using namespace boost::assign; #include #include -#include +#include using namespace gtsam; using namespace std; diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 9735e61d7..eb1c4614c 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -49,8 +49,9 @@ namespace gtsam { typedef NonlinearFactor1 Base; typedef PartialPriorFactor This; - Vector prior_; /// measurement on logmap parameters, in compressed form - std::vector mask_; /// flags to mask all parameters not measured + Vector prior_; ///< measurement on logmap parameters, in compressed form + std::vector mask_; ///< indices of values to constrain in compressed prior vector + Matrix H_; ///< Constant jacobian - computed at creation /** default constructor - only use for serialization */ PartialPriorFactor() {} @@ -58,10 +59,9 @@ namespace gtsam { /** * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses - * Sets the size of the mask with all values off */ PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) - : Base(model, key), mask_(T::Dim(), false) {} + : Base(model, key) {} public: @@ -70,31 +70,20 @@ namespace gtsam { virtual ~PartialPriorFactor() {} - /** Full Constructor: requires mask and vector - not for typical use */ - PartialPriorFactor(const KEY& key, const std::vector& mask, - const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask) { - assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable - assert(nrTrue() == model->dim()); - assert(nrTrue() == prior_.size()); - } - /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) { + Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); - mask_[idx] = true; - assert(nrTrue() == 1); + this->fillH(); } /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(const KEY& key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(T::Dim(), false) { + Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); - setMask(mask); - assert(nrTrue() == this->dim()); + this->fillH(); } /** implement functions needed for Testable */ @@ -117,40 +106,26 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = zeros(this->dim(), p.dim()); + if (H) (*H) = H_; // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); Vector masked_logmap = zero(this->dim()); - size_t masked_idx=0; - for (size_t i=0;i& mask() const { return mask_; } + const Matrix& H() const { return H_; } protected: - /** counts true elements in the mask */ - size_t nrTrue() const { - size_t result=0; + /** Constructs the jacobian matrix in place */ + void fillH() { for (size_t i=0; i& mask) { - for (size_t i=0; i(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(mask_); + ar & BOOST_SERIALIZATION_NVP(H_); } }; // \class PartialPriorFactor diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h index c15f2a701..941eda285 100644 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ b/gtsam/slam/PlanarSLAMOdometry.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 2d25d70a9..ee5649f78 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -87,6 +87,11 @@ namespace gtsam { return reprojectionError.vector(); } + /** return the measurement */ + inline const Point2 measured() const { + return z_; + } + private: /// Serialization function diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp index 3c73b5ba9..76e65a8cf 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/Simulated3D.cpp @@ -16,17 +16,9 @@ **/ #include -#include -#include namespace gtsam { -INSTANTIATE_VALUES(simulated3D::PointKey) -INSTANTIATE_VALUES(simulated3D::PoseKey) - -using namespace simulated3D; -INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - namespace simulated3D { Point3 prior (const Point3& x, boost::optional H) { diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 955d190c2..ae47a6e7d 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -19,13 +19,10 @@ #include #include #include -#include // Use planarSLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(planarSLAM::PointKey) - INSTANTIATE_TUPLE_VALUES2(planarSLAM::PoseValues, planarSLAM::PointValues) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(planarSLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(planarSLAM::Graph, planarSLAM::Values) diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 9219c242c..0eaaeaca1 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -16,14 +16,12 @@ **/ #include -#include #include #include // Use pose2SLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(pose2SLAM::Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose2SLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose2SLAM::Graph, pose2SLAM::Values) template class NonlinearOptimizer; diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 7c7bb0d2e..4f4618896 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -16,14 +16,12 @@ **/ #include -#include #include #include // Use pose3SLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(pose3SLAM::Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose3SLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose3SLAM::Graph, pose3SLAM::Values) diff --git a/gtsam/slam/simulated2D.cpp b/gtsam/slam/simulated2D.cpp index 17a11dc06..ae30c6800 100644 --- a/gtsam/slam/simulated2D.cpp +++ b/gtsam/slam/simulated2D.cpp @@ -16,17 +16,9 @@ */ #include -#include -#include namespace gtsam { - INSTANTIATE_VALUES(simulated2D::PoseKey) - - using namespace simulated2D; - - INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - namespace simulated2D { static Matrix I = gtsam::eye(2); diff --git a/gtsam/slam/simulated2DOriented.cpp b/gtsam/slam/simulated2DOriented.cpp index 5dc1b2b22..0b68f0e46 100644 --- a/gtsam/slam/simulated2DOriented.cpp +++ b/gtsam/slam/simulated2DOriented.cpp @@ -16,13 +16,9 @@ */ #include -#include namespace gtsam { - using namespace simulated2DOriented; - - namespace simulated2DOriented { static Matrix I = gtsam::eye(3); diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 78ffeb6f8..8f09c60e7 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -34,7 +34,6 @@ using namespace std; // template definitions #include #include -#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 053f7a971..4316aadc6 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,7 +20,7 @@ using namespace boost; #include #include #include -#include +#include #include #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 36b80bd8e..f42cb6e9c 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -20,7 +20,7 @@ using namespace boost; #include #include #include -#include +#include #include #include diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 54f3efa38..1eb62ec93 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -107,8 +107,7 @@ TEST(Pose3Graph, partial_prior_height) { EXPECT(assert_equal(expA, actA)); pose3SLAM::Graph graph; -// graph.add(height); // FAIL - on compile, can't initialize a reference? - graph.push_back(boost::shared_ptr(new Partial(height))); + graph.add(height); pose3SLAM::Values values; values.insert(key, init); @@ -163,7 +162,7 @@ TEST(Pose3Graph, partial_prior_xy) { EXPECT(assert_equal(expA, actA)); pose3SLAM::Graph graph; - graph.push_back(Partial::shared_ptr(new Partial(priorXY))); + graph.add(priorXY); pose3SLAM::Values values; values.insert(key, init); diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 23244f5f9..5a8200dd0 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -16,12 +16,11 @@ */ #include -#include #include #include namespace gtsam { - INSTANTIATE_TUPLE_VALUES2(visualSLAM::PoseValues, visualSLAM::PointValues) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Values) diff --git a/myconfigure.ardrone b/myconfigure.ardrone new file mode 100755 index 000000000..117bcdadf --- /dev/null +++ b/myconfigure.ardrone @@ -0,0 +1 @@ +../configure --build=i686-pc-linux-gnu --host=arm-none-linux-gnueabi -prefix=/usr CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 3027ab02f..3a946858a 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include using namespace gtsam; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 1bd46f12f..b4e04603f 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -27,7 +27,6 @@ using namespace boost::assign; #define GTSAM_MAGIC_GAUSSIAN 3 #include -#include #include #include diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 31e870f00..501bb9db8 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -24,8 +24,6 @@ #include #include -#include - using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 01d2920ad..9917273b9 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -35,7 +35,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index a7f3bb285..b90e98625 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include using namespace gtsam; diff --git a/tests/testRot3QOptimization.cpp b/tests/testRot3QOptimization.cpp index b94cad3a5..d46c30ea4 100644 --- a/tests/testRot3QOptimization.cpp +++ b/tests/testRot3QOptimization.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index b75e98102..4ba2e3a5f 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include using namespace gtsam; using namespace std;