12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

(in branch) Merged from trunk r7760-r7959

release/4.3a0
Richard Roberts 2011-11-29 17:02:02 +00:00
parent 7a95ccbd86
commit a0abe68b64
58 changed files with 741 additions and 701 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -34,7 +34,7 @@
#include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>

2
examples/SimpleRotation.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>

2
examples/easyPoint2KalmanFilter.cpp
View File

@ -24,7 +24,7 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
using namespace std;

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>

4
examples/vSLAMexample/vISAMexample.cpp
View File

@ -27,9 +27,7 @@ using namespace boost;
#include <gtsam/inference/graph-inl.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/nonlinear/NonlinearISAM-inl.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include "vSLAMutils.h"
#include "Feature2D.h"

6
gtsam.h
View File

@ -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();
};

5
gtsam/base/Group.h
View File

@ -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 T>
class GroupConcept {

202
gtsam/base/Lie.h
View File

@ -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<class T>
inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);}
/** Compute l0 s.t. l2=l1*l0 */
template<class T>
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<class T>
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<class T>
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<class T>
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<class T>
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 T>
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 T>
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<EFBFBD>Campbell<EFBFBD>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<65>Campbell<6C>Hausdorff_formula
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T>
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 <class T> 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 <class T>
T expm(const Vector& x, int K=7) {
Matrix xhat = wedge<T>(x);
return expm(xhat,K);
}
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>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<65>Campbell<6C>Hausdorff_formula
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T>
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 <class T> 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 <class T>
T expm(const Vector& x, int K=7) {
Matrix xhat = wedge<T>(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<T>; \
template class gtsam::GroupConcept<T>; \
template class gtsam::LieConcept<T>;
template class gtsam::ManifoldConcept<T>; \
template class gtsam::GroupConcept<T>; \
template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) \
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;

106
gtsam/base/Manifold.h
View File

@ -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 T>
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 T>
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

4
gtsam/base/cholesky.cpp
View File

@ -179,14 +179,14 @@ Eigen::LDLT<Matrix>::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");

2
gtsam/base/cholesky.h
View File

@ -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: ");

94
gtsam/geometry/Point2.h
View File

@ -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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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;

93
gtsam/geometry/Point3.h
View File

@ -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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
@ -133,15 +157,6 @@ namespace gtsam {
Point3 sub (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> 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;}
}

6
gtsam/geometry/Pose2.h
View File

@ -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<Matrix&> 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; }

4
gtsam/geometry/Pose3.cpp
View File

@ -188,7 +188,7 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> 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();

85
gtsam/geometry/Pose3.h
View File

@ -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<Matrix&> H1=boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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

99
gtsam/geometry/Rot2.h
View File

@ -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<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/**
* Creates a unit vector as a Point2
*/
inline Point2 unit() const {
return Point2(c_, s_);
}
private:
/** Serialization function */

118
gtsam/geometry/Rot3M.h
View File

@ -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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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

118
gtsam/geometry/Rot3Q.h
View File

@ -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<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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<Matrix&> 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

84
gtsam/geometry/StereoPoint2.h
View File

@ -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);

5
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -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;
}
}
}

1
gtsam/nonlinear/GaussianISAM2.cpp
View File

@ -5,7 +5,6 @@
*/
#include <gtsam/nonlinear/GaussianISAM2.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
using namespace std;
using namespace gtsam;

14
gtsam/nonlinear/GaussianISAM2.h
View File

@ -9,10 +9,7 @@
#pragma once
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/slam/simulated2D.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/ISAM2-inl.h>
#include <gtsam/nonlinear/ISAM2.h>
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<VALUES>
*/
template <class VALUES>
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES> {
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
public:
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES>(params) {}
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES>() {}
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
};
// optimize the BayesTree, starting from the root

44
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -15,18 +15,14 @@
* @author Michael Kaess, Richard Roberts
*/
#include <gtsam/base/FastSet.h>
#include <boost/foreach.hpp>
#include <vector>
namespace gtsam {
template<class CONDITIONAL, class VALUES>
struct ISAM2<CONDITIONAL, VALUES>::Impl {
using namespace std;
typedef ISAM2<CONDITIONAL, VALUES> ISAM2Type;
template<class CONDITIONAL, class VALUES, class GRAPH>
struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
struct PartialSolveResult {
typename ISAM2Type::sharedClique bayesTree;
@ -58,7 +54,7 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
* @param factors The factors from which to extract the variables
* @return The set of variables indices from the factors
*/
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors);
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const GRAPH& factors);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
@ -100,7 +96,7 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
* recalculate its delta.
*/
static void ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const vector<bool>& mask,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
/**
@ -138,15 +134,15 @@ struct _VariableAdder {
};
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& 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<Index> dims(newTheta.dims(*newTheta.orderingArbitrary()));
std::vector<Index> 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<CONDITIONAL,VALUES>::Impl::AddVariables(
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors) {
template<class CONDITIONAL, class VALUES, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
@ -178,8 +174,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Orderin
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold) {
template<class CONDITIONAL, class VALUES, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold) {
FastSet<Index> relinKeys;
for(Index var=0; var<delta.size(); ++var) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
@ -191,7 +187,7 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<Ve
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL, VALUES>::Impl::FindAll(typename BayesTreeClique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
@ -245,8 +241,8 @@ struct _SelectiveExpmapAndClear {
};
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
void ISAM2<CONDITIONAL, VALUES>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> 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<CONDITIONAL, VALUES>::Impl::ExpmapMasked(VALUES& values, const Permut
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
typename ISAM2<CONDITIONAL, VALUES>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, VALUES>::Impl::PartialSolve(GaussianFactorGraph& factors,
template<class CONDITIONAL, class VALUES, class GRAPH>
typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate");

83
gtsam/nonlinear/ISAM2-inl.h
View File

@ -21,22 +21,12 @@
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <set>
#include <limits>
#include <numeric>
#include <gtsam/base/timing.h>
#include <gtsam/base/debug.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2-impl-inl.h>
@ -50,24 +40,24 @@ static const bool latestLast = true;
static const bool structuralLast = false;
/* ************************************************************************* */
template<class Conditional, class Values>
ISAM2<Conditional, Values>::ISAM2(const ISAM2Params& params):
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), params_(params) {}
/* ************************************************************************* */
template<class Conditional, class Values>
ISAM2<Conditional, Values>::ISAM2():
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
delta_(Permutation(), deltaUnpermuted_) {}
/* ************************************************************************* */
template<class Conditional, class Values>
FastList<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const FastList<Index>& keys) const {
template<class CONDITIONAL, class VALUES, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& 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<NonlinearFactor<Values> > allAffected;
FactorGraph<NonlinearFactor<VALUES> > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
@ -89,15 +79,15 @@ FastList<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const FastList<I
/* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Values>
template<class CONDITIONAL, class VALUES, class GRAPH>
FactorGraph<GaussianFactor>::shared_ptr
ISAM2<Conditional, Values>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
toc(1,"getAffectedFactors");
NonlinearFactorGraph<Values> nonlinearAffectedFactors;
GRAPH nonlinearAffectedFactors;
tic(2,"affectedKeysSet");
// for fast lookup below
@ -128,9 +118,9 @@ ISAM2<Conditional, Values>::relinearizeAffectedFactors(const FastList<Index>& af
/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
template<class Conditional, class Values>
FactorGraph<typename ISAM2<Conditional, Values>::CacheFactor>
ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
template<class CONDITIONAL, class VALUES, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false;
@ -140,9 +130,9 @@ ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
// find the last variable that was eliminated
Index key = (*orphan)->frontals().back();
#ifndef NDEBUG
// typename BayesNet<Conditional>::const_iterator it = orphan->end();
// const Conditional& lastConditional = **(--it);
// typename Conditional::const_iterator keyit = lastConditional.endParents();
// typename BayesNet<CONDITIONAL>::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<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
return cachedBoundary;
}
template<class Conditional, class Values>
boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
template<class CONDITIONAL, class VALUES, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::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<FastSet<Index> > ISAM2<Conditional, Values>::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<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
toc(5,"eliminate");
tic(6,"insert");
Base::clear();
BayesTree<CONDITIONAL>::clear();
this->insert(newRoot);
toc(6,"insert");
@ -422,9 +412,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
}
/* ************************************************************************* */
template<class Conditional, class Values>
ISAM2Result ISAM2<Conditional, Values>::update(
const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta, bool force_relinearize) {
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::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<Conditional, Values>::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<bool>(ordering_.nVars(), false);
#endif
}
@ -560,19 +550,28 @@ ISAM2Result ISAM2<Conditional, Values>::update(
}
/* ************************************************************************* */
template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateEstimate() const {
template<class CONDITIONAL, class VALUES, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
Values ret(theta_);
VALUES ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
return ret;
}
/* ************************************************************************* */
template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
template<class CONDITIONAL, class VALUES, class GRAPH>
template<class KEY>
typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
return getLinearizationPoint()[key].retract(delta);
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.retract(delta, ordering_);

38
gtsam/nonlinear/ISAM2.h
View File

@ -19,21 +19,8 @@
#pragma once
#include <map>
#include <list>
#include <vector>
#include <stdexcept>
#include <gtsam/base/types.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/HessianFactor.h>
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<class CONDITIONAL, class VALUES>
template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
protected:
@ -229,7 +217,7 @@ protected:
Permuted<VectorValues> delta_;
/** All original nonlinear factors are stored here to use during relinearization */
NonlinearFactorGraph<VALUES> nonlinearFactors_;
GRAPH nonlinearFactors_;
/** @brief The current elimination ordering Symbols to Index (integer) keys.
*
@ -251,6 +239,8 @@ public:
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL, VALUES> 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<VALUES>& 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<class KEY>
typename KEY::Value calculateEstimate(const KEY& key) const;
/// @name Public members for non-typical usage
//@{
@ -305,7 +305,7 @@ public:
const Permuted<VectorValues>& getDelta() const { return delta_; }
/** Access the set of nonlinear factors */
const NonlinearFactorGraph<VALUES>& 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 <gtsam/nonlinear/ISAM2-inl.h>

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -58,7 +58,7 @@ namespace gtsam {
template<class F>
void add(const F& factor) {
push_back(boost::shared_ptr<F>(new F(factor)));
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/**

21
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -24,14 +24,13 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
template<class Values>
void NonlinearISAM<Values>::update(const Factors& newFactors,
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
const Values& initialValues) {
if(newFactors.size() > 0) {
@ -63,8 +62,8 @@ void NonlinearISAM<Values>::update(const Factors& newFactors,
}
/* ************************************************************************* */
template<class Values>
void NonlinearISAM<Values>::reorder_relinearize() {
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl;
@ -90,8 +89,8 @@ void NonlinearISAM<Values>::reorder_relinearize() {
}
/* ************************************************************************* */
template<class Values>
Values NonlinearISAM<Values>::estimate() const {
template<class VALUES, class GRAPH>
VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
if(isam_.size() > 0)
return linPoint_.retract(optimize(isam_), ordering_);
else
@ -99,14 +98,14 @@ Values NonlinearISAM<Values>::estimate() const {
}
/* ************************************************************************* */
template<class Values>
Matrix NonlinearISAM<Values>::marginalCovariance(const Symbol& key) const {
template<class VALUES, class GRAPH>
Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
return isam_.marginalCovariance(ordering_[key]);
}
/* ************************************************************************* */
template<class Values>
void NonlinearISAM<Values>::print(const std::string& s) const {
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
cout << "ISAM - " << s << ":" << endl;
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
isam_.print("GaussianISAM");

8
gtsam/nonlinear/NonlinearISAM.h
View File

@ -17,7 +17,6 @@
#pragma once
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianISAM.h>
@ -25,11 +24,12 @@ namespace gtsam {
/**
* Wrapper class to manage ISAM in a nonlinear context
*/
template<class Values>
template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
class NonlinearISAM {
public:
typedef gtsam::NonlinearFactorGraph<Values> Factors;
typedef VALUES Values;
typedef GRAPH Factors;
protected:
@ -101,3 +101,5 @@ public:
};
} // \namespace gtsam
#include <gtsam/nonlinear/NonlinearISAM-inl.h>

6
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -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);

8
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -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;
}

23
gtsam/nonlinear/TupleValues-inl.h
View File

@ -18,29 +18,6 @@
#pragma once
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
// TupleValues instantiations for N = 1-6
#define INSTANTIATE_TUPLE_VALUES1(Values1) \
template class TupleValues1<Values1>;
#define INSTANTIATE_TUPLE_VALUES2(Values1, Values2) \
template class TupleValues2<Values1, Values2>;
#define INSTANTIATE_TUPLE_VALUES3(Values1, Values2, Values3) \
template class TupleValues3<Values1, Values2, Values3>;
#define INSTANTIATE_TUPLE_VALUES4(Values1, Values2, Values3, Values4) \
template class TupleValues4<Values1, Values2, Values3, Values4>;
#define INSTANTIATE_TUPLE_VALUES5(Values1, Values2, Values3, Values4, Values5) \
template class TupleValues5<Values1, Values2, Values3, Values4, Values5>;
#define INSTANTIATE_TUPLE_VALUES6(Values1, Values2, Values3, Values4, Values5, Values6) \
template class TupleValues6<Values1, Values2, Values3, Values4, Values5, Values6>;
namespace gtsam {
/* ************************************************************************* */

25
gtsam/nonlinear/TupleValues.h
View File

@ -428,9 +428,12 @@ namespace gtsam {
typedef C2 Values2;
typedef C3 Values3;
typedef TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > > Base;
typedef TupleValues3<C1, C2, C3> This;
TupleValues3() {}
TupleValues3(const TupleValues<C1, TupleValues<C2, TupleValuesEnd<C3> > >& values);
TupleValues3(const TupleValues3<C1, C2, C3>& 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 C1, class C2, class C3, class C4, class C5>
class TupleValues5 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > {
public:
// typedefs
typedef C1 Values1;
typedef C2 Values2;
typedef C3 Values3;
typedef C4 Values4;
typedef C5 Values5;
typedef TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > > Base;
typedef TupleValues5<C1, C2, C3, C4, C5> This;
TupleValues5() {}
TupleValues5(const TupleValues5<C1, C2, C3, C4, C5>& values);
TupleValues5(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValuesEnd<C5> > > > >& 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 C1, class C2, class C3, class C4, class C5, class C6>
class TupleValues6 : public TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > {
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<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > > Base;
typedef TupleValues6<C1, C2, C3, C4, C5, C6> This;
TupleValues6() {}
TupleValues6(const TupleValues6<C1, C2, C3, C4, C5, C6>& values);
TupleValues6(const TupleValues<C1, TupleValues<C2, TupleValues<C3, TupleValues<C4, TupleValues<C5, TupleValuesEnd<C6> > > > > >& 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 <gtsam/nonlinear/TupleValues-inl.h>

4
gtsam/nonlinear/Values-inl.h
View File

@ -28,10 +28,6 @@
#include <gtsam/base/Lie-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#define INSTANTIATE_VALUES(J) template class Values<J>;
using namespace std;
namespace gtsam {

4
gtsam/nonlinear/Values.h
View File

@ -229,5 +229,7 @@ namespace gtsam {
}
};
}
} // \namespace gtsam
#include <gtsam/nonlinear/Values-inl.h>

2
gtsam/nonlinear/tests/testValues.cpp
View File

@ -22,7 +22,7 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Values.h>
using namespace gtsam;
using namespace std;

56
gtsam/slam/PartialPriorFactor.h
View File

@ -49,8 +49,9 @@ namespace gtsam {
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef PartialPriorFactor<VALUES, KEY> This;
Vector prior_; /// measurement on logmap parameters, in compressed form
std::vector<bool> mask_; /// flags to mask all parameters not measured
Vector prior_; ///< measurement on logmap parameters, in compressed form
std::vector<size_t> 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<bool>& 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<size_t>& 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<Matrix&> 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_.size();++i)
if (mask_[i]) {
masked_logmap(masked_idx) = full_logmap(i);
if (H) (*H)(masked_idx, i) = 1.0;
++masked_idx;
}
for (size_t i=0; i<mask_.size(); ++i)
masked_logmap(i) = full_logmap(mask_[i]);
return masked_logmap - prior_;
}
// access
const Vector& prior() const { return prior_; }
const std::vector<bool>& 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_.size(); ++i)
if (mask_[i]) ++result;
return result;
}
/** sets the mask using a set of indices */
void setMask(const std::vector<size_t>& mask) {
for (size_t i=0; i<mask.size(); ++i) {
assert(mask[i] < mask_.size());
mask_[mask[i]] = true;
}
H_(i, mask_[i]) = 1.0;
}
private:
@ -162,6 +137,7 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
ar & BOOST_SERIALIZATION_NVP(mask_);
ar & BOOST_SERIALIZATION_NVP(H_);
}
}; // \class PartialPriorFactor

2
gtsam/slam/PlanarSLAMOdometry.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/planarSLAMValues.h>
#include <gtsam/slam/PlanarSLAMValues.h>
namespace gtsam {

5
gtsam/slam/ProjectionFactor.h
View File

@ -87,6 +87,11 @@ namespace gtsam {
return reprojectionError.vector();
}
/** return the measurement */
inline const Point2 measured() const {
return z_;
}
private:
/// Serialization function

8
gtsam/slam/Simulated3D.cpp
View File

@ -16,17 +16,9 @@
**/
#include <gtsam/slam/Simulated3D.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
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<Matrix&> H) {

3
gtsam/slam/planarSLAM.cpp
View File

@ -19,13 +19,10 @@
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
// 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)

2
gtsam/slam/pose2SLAM.cpp
View File

@ -16,14 +16,12 @@
**/
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// 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<pose2SLAM::Graph, pose2SLAM::Values, GaussianFactorGraph, GaussianSequentialSolver>;

2
gtsam/slam/pose3SLAM.cpp
View File

@ -16,14 +16,12 @@
**/
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
// 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)

8
gtsam/slam/simulated2D.cpp
View File

@ -16,17 +16,9 @@
*/
#include <gtsam/slam/simulated2D.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {
INSTANTIATE_VALUES(simulated2D::PoseKey)
using namespace simulated2D;
INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues)
namespace simulated2D {
static Matrix I = gtsam::eye(2);

4
gtsam/slam/simulated2DOriented.cpp
View File

@ -16,13 +16,9 @@
*/
#include <gtsam/slam/simulated2DOriented.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
namespace gtsam {
using namespace simulated2DOriented;
namespace simulated2DOriented {
static Matrix I = gtsam::eye(3);

1
gtsam/slam/smallExample.cpp
View File

@ -34,7 +34,6 @@ using namespace std;
// template definitions
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
namespace gtsam {

2
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -20,7 +20,7 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/GeneralCameraT.h>

2
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -20,7 +20,7 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/GeneralCameraT.h>

5
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -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<Partial>(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);

3
gtsam/slam/visualSLAM.cpp
View File

@ -16,12 +16,11 @@
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
namespace gtsam {
INSTANTIATE_TUPLE_VALUES2(visualSLAM::PoseValues, visualSLAM::PointValues)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Values)
INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Values)

1
myconfigure.ardrone Executable file
View File

@ -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

2
tests/testExtendedKalmanFilter.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
using namespace gtsam;

1
tests/testGraph.cpp
View File

@ -27,7 +27,6 @@ using namespace boost::assign;
#define GTSAM_MAGIC_GAUSSIAN 3
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>

2
tests/testNonlinearEquality.cpp
View File

@ -24,8 +24,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
using namespace std;
using namespace gtsam;

1
tests/testNonlinearFactor.cpp
View File

@ -35,7 +35,6 @@
#include <gtsam/slam/simulated2D.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
using namespace std;
using namespace gtsam;

2
tests/testNonlinearISAM.cpp
View File

@ -6,7 +6,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/nonlinear/NonlinearISAM-inl.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/slam/planarSLAM.h>
using namespace gtsam;

2
tests/testRot3QOptimization.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3Q.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/Values-inl.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>

2
tests/testTupleValues.cpp
View File

@ -30,7 +30,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/TupleValues.h>
using namespace gtsam;
using namespace std;