diff --git a/.gitignore b/.gitignore index e0d1d4e65..030d51b09 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /build* *.pyc +*.DS_Store \ No newline at end of file diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index 57ffb6b9d..46e3a960a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -30,9 +30,9 @@ struct CommaInitializer : { typedef typename internal::dense_xpr_base >::type Base; EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer) - typedef typename internal::conditional::ret, - XprType, const XprType&>::type ExpressionTypeNested; - typedef typename XprType::InnerIterator InnerIterator; + typedef typename internal::conditional::ret, + XprType, const XprType&>::type ExpressionTypeNested; + typedef typename XprType::InnerIterator InnerIterator; inline CommaInitializer(XprType& xpr, const Scalar& s) : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) @@ -108,67 +108,67 @@ struct CommaInitializer : */ inline XprType& finished() { return m_xpr; } - // The following implement the DenseBase interface - - inline Index rows() const { return m_xpr.rows(); } - inline Index cols() const { return m_xpr.cols(); } - inline Index outerStride() const { return m_xpr.outerStride(); } - inline Index innerStride() const { return m_xpr.innerStride(); } - - inline CoeffReturnType coeff(Index row, Index col) const - { - return m_xpr.coeff(row, col); - } - - inline CoeffReturnType coeff(Index index) const - { - return m_xpr.coeff(index); - } - - inline const Scalar& coeffRef(Index row, Index col) const - { - return m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline const Scalar& coeffRef(Index index) const - { - return m_xpr.const_cast_derived().coeffRef(index); - } - - inline Scalar& coeffRef(Index row, Index col) - { - return m_xpr.const_cast_derived().coeffRef(row, col); - } - - inline Scalar& coeffRef(Index index) - { - return m_xpr.const_cast_derived().coeffRef(index); - } - - template - inline const PacketScalar packet(Index row, Index col) const - { - return m_xpr.template packet(row, col); - } - - template - inline void writePacket(Index row, Index col, const PacketScalar& x) - { - m_xpr.const_cast_derived().template writePacket(row, col, x); - } - - template - inline const PacketScalar packet(Index index) const - { - return m_xpr.template packet(index); - } - - template - inline void writePacket(Index index, const PacketScalar& x) - { - m_xpr.const_cast_derived().template writePacket(index, x); - } - + // The following implement the DenseBase interface + + inline Index rows() const { return m_xpr.rows(); } + inline Index cols() const { return m_xpr.cols(); } + inline Index outerStride() const { return m_xpr.outerStride(); } + inline Index innerStride() const { return m_xpr.innerStride(); } + + inline CoeffReturnType coeff(Index row, Index col) const + { + return m_xpr.coeff(row, col); + } + + inline CoeffReturnType coeff(Index index) const + { + return m_xpr.coeff(index); + } + + inline const Scalar& coeffRef(Index row, Index col) const + { + return m_xpr.const_cast_derived().coeffRef(row, col); + } + + inline const Scalar& coeffRef(Index index) const + { + return m_xpr.const_cast_derived().coeffRef(index); + } + + inline Scalar& coeffRef(Index row, Index col) + { + return m_xpr.const_cast_derived().coeffRef(row, col); + } + + inline Scalar& coeffRef(Index index) + { + return m_xpr.const_cast_derived().coeffRef(index); + } + + template + inline const PacketScalar packet(Index row, Index col) const + { + return m_xpr.template packet(row, col); + } + + template + inline void writePacket(Index row, Index col, const PacketScalar& x) + { + m_xpr.const_cast_derived().template writePacket(row, col, x); + } + + template + inline const PacketScalar packet(Index index) const + { + return m_xpr.template packet(index); + } + + template + inline void writePacket(Index index, const PacketScalar& x) + { + m_xpr.const_cast_derived().template writePacket(index, x); + } + const XprType& _expression() const { return m_xpr; } XprType& m_xpr; // target expression @@ -176,12 +176,12 @@ struct CommaInitializer : Index m_col; // current col id Index m_currentBlockRows; // current block height }; - -namespace internal { - template - struct traits > : traits - { - }; + +namespace internal { + template + struct traits > : traits + { + }; } /** \anchor MatrixBaseCommaInitRef diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 9e88607cc..69054fc1c 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -19,20 +19,6 @@ namespace gtsam { -/* ************************************************************************* */ -LieMatrix::LieMatrix(size_t m, size_t n, ...) -: Matrix(m,n) { - va_list ap; - va_start(ap, n); - for(size_t i = 0; i < m; ++i) { - for(size_t j = 0; j < n; ++j) { - double value = va_arg(ap, double); - (*this)(i,j) = value; - } - } - va_end(ap); -} - /* ************************************************************************* */ void LieMatrix::print(const std::string& name) const { gtsam::print(matrix(), name); diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 0a27eff69..8d43dd6ea 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -48,9 +48,6 @@ struct LieMatrix : public Matrix, public DerivedValue { LieMatrix(size_t m, size_t n, const double* const data) : Matrix(Eigen::Map(data, m, n)) {} - /** Specify arguments directly, as in Matrix_() - always force these to be doubles */ - GTSAM_EXPORT LieMatrix(size_t m, size_t n, ...); - /// @} /// @name Testable interface /// @{ diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 17aeff73a..f6cae28e2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -24,21 +24,10 @@ namespace gtsam { /* ************************************************************************* */ LieVector::LieVector(size_t m, const double* const data) -: Vector(Vector_(m,data)) -{ -} - -/* ************************************************************************* */ -LieVector::LieVector(size_t m, ...) : Vector(m) { - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); + for(size_t i = 0; i < m; i++) + (*this)(i) = data[i]; } /* ************************************************************************* */ diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index f1b05b34b..6989bbfd2 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -44,9 +44,6 @@ struct LieVector : public Vector, public DerivedValue { /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); - /** Specify arguments directly, as in Vector_() - always force these to be doubles */ - GTSAM_EXPORT LieVector(size_t m, ...); - /** get the underlying vector */ Vector vector() const { return static_cast(*this); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index adf98cf9a..3b16beb51 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -36,38 +36,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -Matrix Matrix_( size_t m, size_t n, const double* const data) { - MatrixRowMajor A(m,n); - copy(data, data+m*n, A.data()); - return Matrix(A); -} - -/* ************************************************************************* */ -Matrix Matrix_( size_t m, size_t n, const Vector& v) -{ - Matrix A(m,n); - // column-wise copy - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - A(i,j) = v(k); - return A; -} - -/* ************************************************************************* */ -Matrix Matrix_(size_t m, size_t n, ...) { - Matrix A(m,n); - va_list ap; - va_start(ap, n); - for( size_t i = 0 ; i < m ; i++) - for( size_t j = 0 ; j < n ; j++) { - double value = va_arg(ap, double); - A(i,j) = value; - } - va_end(ap); - return A; -} - /* ************************************************************************* */ Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); @@ -211,17 +179,6 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec x += alpha * A.transpose() * e; } -/* ************************************************************************* */ -Vector Vector_(const Matrix& A) -{ - size_t m = A.rows(), n = A.cols(); - Vector v(m*n); - for( size_t j = 0, k=0 ; j < n ; j++) - for( size_t i = 0; i < m ; i++,k++) - v(k) = A(i,j); - return v; -} - /* ************************************************************************* */ void print(const Matrix& A, const string &s, ostream& stream) { size_t m = A.rows(), n = A.cols(); diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 05f3bc7c9..caee66851 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -45,27 +45,6 @@ typedef Eigen::Matrix Matrix6; typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -/** - * constructor with size and initial data, row order ! - */ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const double* const data); - -/** - * constructor with size and vector data, column order !!! - */ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const Vector& v); - -/** - * constructor from Vector yielding v.size()*1 vector - */ -inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);} - -/** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 -*/ -GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...); - // Matlab-like syntax /** @@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) { return result; } -/** - * convert to column vector, column order !!! - */ -GTSAM_EXPORT Vector Vector_(const Matrix& A); - /** * print a matrix */ diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 2a0192930..48ada018f 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -79,33 +79,6 @@ void odprintf(const char *format, ...) { //#endif } -/* ************************************************************************* */ -Vector Vector_( size_t m, const double* const data) { - Vector A(m); - copy(data, data+m, A.data()); - return A; -} - -/* ************************************************************************* */ -Vector Vector_(size_t m, ...) { - Vector v(m); - va_list ap; - va_start(ap, m); - for( size_t i = 0 ; i < m ; i++) { - double value = va_arg(ap, double); - v(i) = value; - } - va_end(ap); - return v; -} - -/* ************************************************************************* */ -Vector Vector_(const std::vector& d) { - Vector v(d.size()); - copy(d.begin(), d.end(), v.data()); - return v; -} - /* ************************************************************************* */ bool zero(const Vector& v) { bool result = true; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 2c6066042..b35afccdb 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -46,22 +46,6 @@ typedef Eigen::VectorBlock ConstSubVector; */ GTSAM_EXPORT void odprintf(const char *format, ...); -/** - * constructor with size and initial data, row order ! - */ -GTSAM_EXPORT Vector Vector_( size_t m, const double* const data); - -/** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - */ -GTSAM_EXPORT Vector Vector_(size_t m, ...); - -/** - * Create a numeric vector from an STL vector of doubles - */ -GTSAM_EXPORT Vector Vector_(const std::vector& data); - /** * Create vector initialized to a constant value * @param n is the size of the vector diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 053a89da8..036f23f68 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -59,7 +59,7 @@ namespace gtsam { /** global functions for converting to a LieVector for use with numericalDerivative */ inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } + inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } /** * Numerically compute gradient of scalar function diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index b21756dc4..ee8fe14d9 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -39,20 +39,17 @@ TEST( LieMatrix, construction ) { TEST( LieMatrix, other_constructors ) { Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0); LieMatrix exp(init); - LieMatrix a(2,2,10.0,20.0,30.0,40.0); double data[] = {10,30,20,40}; LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); } /* ************************************************************************* */ TEST(LieMatrix, retract) { - LieMatrix init(2,2, 1.0,2.0,3.0,4.0); - Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0); + LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0)); + Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0); - LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0); + LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0)); LieMatrix actual = init.retract(update); EXPECT(assert_equal(expected, actual)); @@ -63,7 +60,7 @@ TEST(LieMatrix, retract) { EXPECT(assert_equal(expectedUpdate, actualUpdate)); Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4); - Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0)); + Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0))); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 11157a701..68655cc71 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2))); + EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2))); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 67eed6656..f66678c25 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -39,14 +39,9 @@ TEST( testLieVector, construction ) { TEST( testLieVector, other_constructors ) { Vector init = (Vector(2) << 10.0, 20.0); LieVector exp(init); - LieVector a(2,10.0,20.0); double data[] = {10,20}; LieVector b(2,data); - LieVector c(2.3), c_exp(LieVector(1, 2.3)); - EXPECT(assert_equal(exp, a)); EXPECT(assert_equal(exp, b)); - EXPECT(assert_equal(b, a)); - EXPECT(assert_equal(c_exp, c)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 34502ba99..828208361 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -43,18 +43,6 @@ TEST( matrix, constructor_data ) EQUALITY(A,B); } -/* ************************************************************************* */ -/* -TEST( matrix, constructor_vector ) -{ - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, -5, 3, 0, -5); - Vector v(4); - copy(data, data + 4, v.data()); - Matrix B = Matrix_(2, 2, v); // this one is column order ! - EQUALITY(A,trans(B)); -} -*/ /* ************************************************************************* */ TEST( matrix, Matrix_ ) { diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index bd3c2c217..f7e4d3baa 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -48,7 +48,8 @@ double f2(const LieVector& x) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian2) { - LieVector center(2, 0.5, 1.0); + Vector v_center = (Vector(2) << 0.5, 1.0); + LieVector center(v_center); Matrix expected = (Matrix(2,2) << -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), @@ -67,7 +68,9 @@ double f3(const LieVector& x1, const LieVector& x2) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian211) { - LieVector center1(1, 1.0), center2(1, 5.0); + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 5.0); + LieVector center1(v_center1), center2(v_center2); Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix actual11 = numericalHessian211(f3, center1, center2); @@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) { /* ************************************************************************* */ TEST(testNumericalDerivative, numericalHessian311) { - LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); + Vector v_center1 = (Vector(1) << 1.0); + Vector v_center2 = (Vector(1) << 2.0); + Vector v_center3 = (Vector(1) << 3.0); + LieVector center1(v_center1), center2(v_center2), center3(v_center3); + double x = center1(0), y = center2(0), z = center3(0); Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix actual11 = numericalHessian311(f4, center1, center2, center3); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index fc2b7a266..ee0d94366 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -23,15 +23,6 @@ using namespace std; using namespace gtsam; -/* ************************************************************************* */ -TEST( TestVector, Vector_variants ) -{ - Vector a = (Vector(2) << 10.0,20.0); - double data[] = {10,20}; - Vector b = Vector_(2, data); - EXPECT(assert_equal(a, b)); -} - namespace { /* ************************************************************************* */ template diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 05f55e203..7a43ef884 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -1,270 +1,270 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file timing.h - * @brief Timing utilities - * @author Richard Roberts, Michael Kaess - * @date Oct 5, 2010 - */ -#pragma once - -#include -#include -#include -#include -#include -#include - -// This file contains the GTSAM timing instrumentation library, a low-overhead method for -// learning at a medium-fine level how much time various components of an algorithm take -// in CPU and wall time. -// -// The output of this instrumentation is a call-tree-like printout containing statistics -// about each instrumented code block. To print this output at any time, call -// tictoc_print() or tictoc_print_(). -// -// An overall point to be aware of is that there are two versions of each function - one -// ending in an underscore '_' and one without the trailing underscore. The underscore -// versions always are active, but the versions without an underscore are active only when -// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing build type). -// GTSAM algorithms are all instrumented with the non-underscore versions, so generally -// you should use the underscore versions in your own code to leave out the GTSAM detail. -// -// gttic and gttoc start and stop a timed section, respectively. gttic creates a *scoped* -// object - when it goes out of scope gttoc is called automatically. Thus, you do not -// need to call gttoc if you are timing an entire function (see basic use examples below). -// However, you must be *aware* of this scoped nature - putting gttic inside of an if(...) -// block, for example, will only time code until the closing brace '}'. See advanced -// usage below if you need to avoid this. -// -// Multiple calls nest automatically - each gttic nests under the previous gttic called -// for which gttoc has not been called (or the previous gttic did not go out of scope). -// -// Basic usage examples are as follows: -// -// - Timing an entire function: -// void myFunction() { -// gttic_(myFunction); -// ........ -// } -// -// - Timing an entire function as well as its component parts: -// void myLongFunction() { -// gttic_(myLongFunction); -// gttic_(step1); // Will nest under the 'myLongFunction' label -// ........ -// gttoc_(step1); -// gttic_(step2); // Will nest under the 'myLongFunction' label -// ........ -// gttoc_(step2); -// ........ -// } -// -// - Timing functions calling/called by other functions: -// void oneStep() { -// gttic_(oneStep); // Will automatically nest under the gttic label of the calling function -// ....... -// } -// void algorithm() { -// gttic_(algorithm); -// oneStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label -// twoStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label -// } -// -// -// Advanced usage: -// -// - "Finishing iterations" - to get correct min/max times for each call, you must define -// in your code what constitutes an iteration. A single sum for the min/max times is -// accumulated within each iteration. If you don't care about min/max times, you don't -// need to worry about this. For example: -// void myOuterLoop() { -// while(true) { -// iterateMyAlgorithm(); -// tictoc_finishedIteration_(); -// tictoc_print_(); // Optional -// } -// } -// -// - Stopping timing a section in a different scope than it is started. Normally, a gttoc -// statement goes out of scope at end of C++ scope. However, you can use longtic and -// longtoc to start and stop timing with the specified label at any point, without regard -// too scope. Note that if you use these, it may become difficult to ensure that you -// have matching gttic/gttoc statments. You may want to consider reorganizing your timing -// outline to match the scope of your code. - -// Automatically use the new Boost timers if version is recent enough. -#if BOOST_VERSION >= 104800 -# ifndef GTSAM_DISABLE_NEW_TIMERS -# define GTSAM_USING_NEW_BOOST_TIMERS -# endif -#endif - -#ifdef GTSAM_USING_NEW_BOOST_TIMERS -# include -#else -# include -#endif - -#ifdef GTSAM_USE_TBB -# include -# undef min -# undef max -# undef ERROR -#endif - -namespace gtsam { - - namespace internal { - GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); - - /** - * Timing Entry, arranged in a tree - */ - class GTSAM_EXPORT TimingOutline { - protected: - size_t myId_; - size_t t_; - size_t tWall_; - double t2_ ; ///< cache the \sum t_i^2 - size_t tIt_; - size_t tMax_; - size_t tMin_; - size_t n_; - size_t myOrder_; - size_t lastChildOrder_; - std::string label_; - - // Tree structure - boost::weak_ptr parent_; ///< parent pointer - typedef FastMap > ChildMap; - ChildMap children_; ///< subtrees - -#ifdef GTSAM_USING_NEW_BOOST_TIMERS - boost::timer::cpu_timer timer_; -#else - boost::timer timer_; - gtsam::ValueWithDefault timerActive_; -#endif -#ifdef GTSAM_USE_TBB - tbb::tick_count tbbTimer_; -#endif - void add(size_t usecs, size_t usecsWall); - - public: - /// Constructor - TimingOutline(const std::string& label, size_t myId); - size_t time() const; ///< time taken, including children - double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children - double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds - double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds - double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds - double max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds - double mean() const { return self() / double(n_); } ///< mean self time, in seconds - void print(const std::string& outline = "") const; - void print2(const std::string& outline = "", const double parentTotal = -1.0) const; - const boost::shared_ptr& - child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); - void finishedIteration(); - - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); - }; // \TimingOutline - - /** - * No documentation - */ - class AutoTicToc { - private: - size_t id_; - const char *label_; - bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } - }; - - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; - } - -// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) -// There is a trick being used here to achieve near-zero runtime overhead, in that a -// static variable is created for each tic/toc statement storing an integer ID, but the -// integer ID is only looked up by string once when the static variable is initialized -// as the program starts. - -// tic -#define gttic_(label) \ - static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) - -// toc -#define gttoc_(label) \ - label##_obj.stop() - -// tic -#define longtic_(label) \ - static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::ticInternal(label##_id_tic, #label) - -// toc -#define longtoc_(label) \ - static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::tocInternal(label##_id_toc, #label) - -// indicate iteration is finished -inline void tictoc_finishedIteration_() { - ::gtsam::internal::timingRoot->finishedIteration(); } - -// print -inline void tictoc_print_() { - ::gtsam::internal::timingRoot->print(); } - -// print mean and standard deviation -inline void tictoc_print2_() { - ::gtsam::internal::timingRoot->print2(); } - -// get a node by label and assign it to variable -#define tictoc_getNode(variable, label) \ - static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ - const boost::shared_ptr variable = \ - ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); - -// reset -inline void tictoc_reset_() { +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timing.h + * @brief Timing utilities + * @author Richard Roberts, Michael Kaess + * @date Oct 5, 2010 + */ +#pragma once + +#include +#include +#include +#include +#include +#include + +// This file contains the GTSAM timing instrumentation library, a low-overhead method for +// learning at a medium-fine level how much time various components of an algorithm take +// in CPU and wall time. +// +// The output of this instrumentation is a call-tree-like printout containing statistics +// about each instrumented code block. To print this output at any time, call +// tictoc_print() or tictoc_print_(). +// +// An overall point to be aware of is that there are two versions of each function - one +// ending in an underscore '_' and one without the trailing underscore. The underscore +// versions always are active, but the versions without an underscore are active only when +// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing build type). +// GTSAM algorithms are all instrumented with the non-underscore versions, so generally +// you should use the underscore versions in your own code to leave out the GTSAM detail. +// +// gttic and gttoc start and stop a timed section, respectively. gttic creates a *scoped* +// object - when it goes out of scope gttoc is called automatically. Thus, you do not +// need to call gttoc if you are timing an entire function (see basic use examples below). +// However, you must be *aware* of this scoped nature - putting gttic inside of an if(...) +// block, for example, will only time code until the closing brace '}'. See advanced +// usage below if you need to avoid this. +// +// Multiple calls nest automatically - each gttic nests under the previous gttic called +// for which gttoc has not been called (or the previous gttic did not go out of scope). +// +// Basic usage examples are as follows: +// +// - Timing an entire function: +// void myFunction() { +// gttic_(myFunction); +// ........ +// } +// +// - Timing an entire function as well as its component parts: +// void myLongFunction() { +// gttic_(myLongFunction); +// gttic_(step1); // Will nest under the 'myLongFunction' label +// ........ +// gttoc_(step1); +// gttic_(step2); // Will nest under the 'myLongFunction' label +// ........ +// gttoc_(step2); +// ........ +// } +// +// - Timing functions calling/called by other functions: +// void oneStep() { +// gttic_(oneStep); // Will automatically nest under the gttic label of the calling function +// ....... +// } +// void algorithm() { +// gttic_(algorithm); +// oneStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label +// twoStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label +// } +// +// +// Advanced usage: +// +// - "Finishing iterations" - to get correct min/max times for each call, you must define +// in your code what constitutes an iteration. A single sum for the min/max times is +// accumulated within each iteration. If you don't care about min/max times, you don't +// need to worry about this. For example: +// void myOuterLoop() { +// while(true) { +// iterateMyAlgorithm(); +// tictoc_finishedIteration_(); +// tictoc_print_(); // Optional +// } +// } +// +// - Stopping timing a section in a different scope than it is started. Normally, a gttoc +// statement goes out of scope at end of C++ scope. However, you can use longtic and +// longtoc to start and stop timing with the specified label at any point, without regard +// too scope. Note that if you use these, it may become difficult to ensure that you +// have matching gttic/gttoc statments. You may want to consider reorganizing your timing +// outline to match the scope of your code. + +// Automatically use the new Boost timers if version is recent enough. +#if BOOST_VERSION >= 104800 +# ifndef GTSAM_DISABLE_NEW_TIMERS +# define GTSAM_USING_NEW_BOOST_TIMERS +# endif +#endif + +#ifdef GTSAM_USING_NEW_BOOST_TIMERS +# include +#else +# include +#endif + +#ifdef GTSAM_USE_TBB +# include +# undef min +# undef max +# undef ERROR +#endif + +namespace gtsam { + + namespace internal { + GTSAM_EXPORT size_t getTicTocID(const char *description); + GTSAM_EXPORT void ticInternal(size_t id, const char *label); + GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + /** + * Timing Entry, arranged in a tree + */ + class GTSAM_EXPORT TimingOutline { + protected: + size_t myId_; + size_t t_; + size_t tWall_; + double t2_ ; ///< cache the \sum t_i^2 + size_t tIt_; + size_t tMax_; + size_t tMin_; + size_t n_; + size_t myOrder_; + size_t lastChildOrder_; + std::string label_; + + // Tree structure + boost::weak_ptr parent_; ///< parent pointer + typedef FastMap > ChildMap; + ChildMap children_; ///< subtrees + +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer timer_; +#else + boost::timer timer_; + gtsam::ValueWithDefault timerActive_; +#endif +#ifdef GTSAM_USE_TBB + tbb::tick_count tbbTimer_; +#endif + void add(size_t usecs, size_t usecsWall); + + public: + /// Constructor + TimingOutline(const std::string& label, size_t myId); + size_t time() const; ///< time taken, including children + double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children + double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds + double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds + double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds + double max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds + double mean() const { return self() / double(n_); } ///< mean self time, in seconds + void print(const std::string& outline = "") const; + void print2(const std::string& outline = "", const double parentTotal = -1.0) const; + const boost::shared_ptr& + child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); + void ticInternal(); + void tocInternal(); + void finishedIteration(); + + GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + }; // \TimingOutline + + /** + * No documentation + */ + class AutoTicToc { + private: + size_t id_; + const char *label_; + bool isSet_; + public: + AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } + void stop() { tocInternal(id_, label_); isSet_ = false; } + ~AutoTicToc() { if(isSet_) stop(); } + }; + + GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + } + +// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) +// There is a trick being used here to achieve near-zero runtime overhead, in that a +// static variable is created for each tic/toc statement storing an integer ID, but the +// integer ID is only looked up by string once when the static variable is initialized +// as the program starts. + +// tic +#define gttic_(label) \ + static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ + ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + +// toc +#define gttoc_(label) \ + label##_obj.stop() + +// tic +#define longtic_(label) \ + static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ + ::gtsam::internal::ticInternal(label##_id_tic, #label) + +// toc +#define longtoc_(label) \ + static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ + ::gtsam::internal::tocInternal(label##_id_toc, #label) + +// indicate iteration is finished +inline void tictoc_finishedIteration_() { + ::gtsam::internal::timingRoot->finishedIteration(); } + +// print +inline void tictoc_print_() { + ::gtsam::internal::timingRoot->print(); } + +// print mean and standard deviation +inline void tictoc_print2_() { + ::gtsam::internal::timingRoot->print2(); } + +// get a node by label and assign it to variable +#define tictoc_getNode(variable, label) \ + static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ + const boost::shared_ptr variable = \ + ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + +// reset +inline void tictoc_reset_() { ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); - ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } - -#ifdef ENABLE_TIMING -#define gttic(label) gttic_(label) -#define gttoc(label) gttoc_(label) -#define longtic(label) longtic_(label) -#define longtoc(label) longtoc_(label) -#define tictoc_finishedIteration tictoc_finishedIteration_ -#define tictoc_print tictoc_print_ -#define tictoc_reset tictoc_reset_ -#else -#define gttic(label) ((void)0) -#define gttoc(label) ((void)0) -#define longtic(label) ((void)0) -#define longtoc(label) ((void)0) -#define tictoc_finishedIteration() ((void)0) -#define tictoc_print() ((void)0) -#define tictoc_reset() ((void)0) -#endif - -} + ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + +#ifdef ENABLE_TIMING +#define gttic(label) gttic_(label) +#define gttoc(label) gttoc_(label) +#define longtic(label) longtic_(label) +#define longtoc(label) longtoc_(label) +#define tictoc_finishedIteration tictoc_finishedIteration_ +#define tictoc_print tictoc_print_ +#define tictoc_reset tictoc_reset_ +#else +#define gttic(label) ((void)0) +#define gttoc(label) ((void)0) +#define longtic(label) ((void)0) +#define longtoc(label) ((void)0) +#define tictoc_finishedIteration() ((void)0) +#define tictoc_print() ((void)0) +#define tictoc_reset() ((void)0) +#endif + +} diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 66fe88407..f50f21d43 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -1,335 +1,335 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file types.h - * @brief Typedefs for easier changing of types - * @author Richard Roberts - * @date Aug 21, 2010 - * @addtogroup base - */ - -#pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include - -#ifdef GTSAM_USE_TBB -#include -#include -#include -#endif - -#ifdef GTSAM_USE_EIGEN_MKL_OPENMP -#include -#endif - -#ifdef __clang__ -# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ - _Pragma("clang diagnostic push") \ - _Pragma("clang diagnostic ignored \"" diag "\"") -#else -# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) -#endif - -#ifdef __clang__ -# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") -#else -# define CLANG_DIAGNOSTIC_POP() -#endif - -namespace gtsam { - - /// Integer nonlinear key type - typedef size_t Key; - - /// Typedef for a function to format a key, i.e. to convert it to a string - typedef boost::function KeyFormatter; - - // Helper function for DefaultKeyFormatter - GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - - /// The default KeyFormatter, which is used if no KeyFormatter is passed to - /// a nonlinear 'print' function. Automatically detects plain integer keys - /// and Symbol keys. - static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - - - /// The index type for Eigen objects - typedef ptrdiff_t DenseIndex; - - - /* ************************************************************************* */ - /** - * Helper class that uses templates to select between two types based on - * whether TEST_TYPE is const or not. - */ - template - struct const_selector { - }; - - /** Specialization for the non-const version */ - template - struct const_selector { - typedef AS_NON_CONST type; - }; - - /** Specialization for the const version */ - template - struct const_selector { - typedef AS_CONST type; - }; - - /* ************************************************************************* */ - /** - * Helper struct that encapsulates a value with a default, this is just used - * as a member object so you don't have to specify defaults in the class - * constructor. - */ - template - struct ValueWithDefault { - T value; - - /** Default constructor, initialize to default value supplied in template argument */ - ValueWithDefault() : value(defaultValue) {} - - /** Initialize to the given value */ - ValueWithDefault(const T& _value) : value(_value) {} - - /** Operator to access the value */ - T& operator*() { return value; } - - /** Operator to access the value */ - const T& operator*() const { return value; } - - /** Implicit conversion allows use in if statements for bool type, etc. */ - operator T() const { return value; } - }; - - /* ************************************************************************* */ - /** A helper class that behaves as a container with one element, and works with - * boost::range */ - template - class ListOfOneContainer { - T element_; - public: - typedef T value_type; - typedef const T* const_iterator; - typedef T* iterator; - ListOfOneContainer(const T& element) : element_(element) {} - const T* begin() const { return &element_; } - const T* end() const { return &element_ + 1; } - T* begin() { return &element_; } - T* end() { return &element_ + 1; } - size_t size() const { return 1; } - }; - - BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept >)); - - /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ - template - ListOfOneContainer ListOfOne(const T& element) { - return ListOfOneContainer(element); - } - - /* ************************************************************************* */ - /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. - template - class ThreadsafeException : -#ifdef GTSAM_USE_TBB - public tbb::tbb_exception -#else - public std::exception -#endif - { -#ifdef GTSAM_USE_TBB - private: - typedef tbb::tbb_exception Base; - protected: - typedef std::basic_string, tbb::tbb_allocator > String; -#else - private: - typedef std::exception Base; - protected: - typedef std::string String; -#endif - - protected: - bool dynamic_; ///< Whether this object was moved - mutable boost::optional description_; ///< Optional description - - /// Default constructor is protected - may only be created from derived classes - ThreadsafeException() : dynamic_(false) {} - - /// Copy constructor is protected - may only be created from derived classes - ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} - - /// Construct with description string - ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} - - /// Default destructor doesn't have the throw() - virtual ~ThreadsafeException() throw () {} - - public: - // Implement functions for tbb_exception -#ifdef GTSAM_USE_TBB - virtual tbb::tbb_exception* move() throw () { - void* cloneMemory = scalable_malloc(sizeof(DERIVED)); - if (!cloneMemory) { - std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; - exit(-1); - } - DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); - clone->dynamic_ = true; - return clone; - } - - virtual void destroy() throw() { - if (dynamic_) { - DERIVED* derivedPtr = static_cast(this); - derivedPtr->~DERIVED(); - scalable_free(derivedPtr); - } - } - - virtual void throw_self() { - throw *static_cast(this); } - - virtual const char* name() const throw() { - return typeid(DERIVED).name(); } -#endif - - virtual const char* what() const throw() { - return description_ ? description_->c_str() : ""; } - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class RuntimeErrorThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class OutOfRangeThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe invalid argument exception - class InvalidArgumentThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ -#ifdef __clang__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below -#endif - - /// An object whose scope defines a block where TBB and OpenMP parallelism are mixed. In such a - /// block, we use default threads for TBB, and p/2 threads for OpenMP. If GTSAM is not compiled to - /// use both TBB and OpenMP, this has no effect. - class TbbOpenMPMixedScope - { - int previousOpenMPThreads; - - public: -#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP - TbbOpenMPMixedScope() : - previousOpenMPThreads(omp_get_num_threads()) - { - omp_set_num_threads(omp_get_num_procs() / 4); - } - - ~TbbOpenMPMixedScope() - { - omp_set_num_threads(previousOpenMPThreads); - } -#else - TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {} - ~TbbOpenMPMixedScope() {} -#endif - }; - -#ifdef __clang__ -# pragma clang diagnostic pop -#endif - -} - -/* ************************************************************************* */ -/** An assertion that throws an exception if NDEBUG is not defined and -* evaluates to an empty statement otherwise. */ -#ifdef NDEBUG -#define assert_throw(CONDITION, EXCEPTION) ((void)0) -#else -#define assert_throw(CONDITION, EXCEPTION) \ - if (!(CONDITION)) { \ - throw (EXCEPTION); \ - } -#endif - -#ifdef _MSC_VER - -// Define some common g++ functions and macros we use that MSVC does not have - -#include -namespace std { - template inline int isfinite(T a) { - return (int)boost::math::isfinite(a); } - template inline int isnan(T a) { - return (int)boost::math::isnan(a); } - template inline int isinf(T a) { - return (int)boost::math::isinf(a); } -} - -#include -#ifndef M_PI -#define M_PI (boost::math::constants::pi()) -#endif -#ifndef M_PI_2 -#define M_PI_2 (boost::math::constants::pi() / 2.0) -#endif -#ifndef M_PI_4 -#define M_PI_4 (boost::math::constants::pi() / 4.0) -#endif - -#endif - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif - -#ifdef ERROR -#undef ERROR -#endif +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file types.h + * @brief Typedefs for easier changing of types + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#include +#include +#endif + +#ifdef GTSAM_USE_EIGEN_MKL_OPENMP +#include +#endif + +#ifdef __clang__ +# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ + _Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"" diag "\"") +#else +# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) +#endif + +#ifdef __clang__ +# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") +#else +# define CLANG_DIAGNOSTIC_POP() +#endif + +namespace gtsam { + + /// Integer nonlinear key type + typedef size_t Key; + + /// Typedef for a function to format a key, i.e. to convert it to a string + typedef boost::function KeyFormatter; + + // Helper function for DefaultKeyFormatter + GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); + + /// The default KeyFormatter, which is used if no KeyFormatter is passed to + /// a nonlinear 'print' function. Automatically detects plain integer keys + /// and Symbol keys. + static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + + + /// The index type for Eigen objects + typedef ptrdiff_t DenseIndex; + + + /* ************************************************************************* */ + /** + * Helper class that uses templates to select between two types based on + * whether TEST_TYPE is const or not. + */ + template + struct const_selector { + }; + + /** Specialization for the non-const version */ + template + struct const_selector { + typedef AS_NON_CONST type; + }; + + /** Specialization for the const version */ + template + struct const_selector { + typedef AS_CONST type; + }; + + /* ************************************************************************* */ + /** + * Helper struct that encapsulates a value with a default, this is just used + * as a member object so you don't have to specify defaults in the class + * constructor. + */ + template + struct ValueWithDefault { + T value; + + /** Default constructor, initialize to default value supplied in template argument */ + ValueWithDefault() : value(defaultValue) {} + + /** Initialize to the given value */ + ValueWithDefault(const T& _value) : value(_value) {} + + /** Operator to access the value */ + T& operator*() { return value; } + + /** Operator to access the value */ + const T& operator*() const { return value; } + + /** Implicit conversion allows use in if statements for bool type, etc. */ + operator T() const { return value; } + }; + + /* ************************************************************************* */ + /** A helper class that behaves as a container with one element, and works with + * boost::range */ + template + class ListOfOneContainer { + T element_; + public: + typedef T value_type; + typedef const T* const_iterator; + typedef T* iterator; + ListOfOneContainer(const T& element) : element_(element) {} + const T* begin() const { return &element_; } + const T* end() const { return &element_ + 1; } + T* begin() { return &element_; } + T* end() { return &element_ + 1; } + size_t size() const { return 1; } + }; + + BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept >)); + + /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ + template + ListOfOneContainer ListOfOne(const T& element) { + return ListOfOneContainer(element); + } + + /* ************************************************************************* */ + /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. + template + class ThreadsafeException : +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else + public std::exception +#endif + { +#ifdef GTSAM_USE_TBB + private: + typedef tbb::tbb_exception Base; + protected: + typedef std::basic_string, tbb::tbb_allocator > String; +#else + private: + typedef std::exception Base; + protected: + typedef std::string String; +#endif + + protected: + bool dynamic_; ///< Whether this object was moved + mutable boost::optional description_; ///< Optional description + + /// Default constructor is protected - may only be created from derived classes + ThreadsafeException() : dynamic_(false) {} + + /// Copy constructor is protected - may only be created from derived classes + ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} + + /// Construct with description string + ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} + + /// Default destructor doesn't have the throw() + virtual ~ThreadsafeException() throw () {} + + public: + // Implement functions for tbb_exception +#ifdef GTSAM_USE_TBB + virtual tbb::tbb_exception* move() throw () { + void* cloneMemory = scalable_malloc(sizeof(DERIVED)); + if (!cloneMemory) { + std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; + exit(-1); + } + DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); + clone->dynamic_ = true; + return clone; + } + + virtual void destroy() throw() { + if (dynamic_) { + DERIVED* derivedPtr = static_cast(this); + derivedPtr->~DERIVED(); + scalable_free(derivedPtr); + } + } + + virtual void throw_self() { + throw *static_cast(this); } + + virtual const char* name() const throw() { + return typeid(DERIVED).name(); } +#endif + + virtual const char* what() const throw() { + return description_ ? description_->c_str() : ""; } + }; + + /* ************************************************************************* */ + /// Threadsafe runtime error exception + class RuntimeErrorThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ + /// Threadsafe runtime error exception + class OutOfRangeThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ + /// Threadsafe invalid argument exception + class InvalidArgumentThreadsafe : public ThreadsafeException + { + public: + /// Construct with a string describing the exception + InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} + }; + + /* ************************************************************************* */ +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below +#endif + + /// An object whose scope defines a block where TBB and OpenMP parallelism are mixed. In such a + /// block, we use default threads for TBB, and p/2 threads for OpenMP. If GTSAM is not compiled to + /// use both TBB and OpenMP, this has no effect. + class TbbOpenMPMixedScope + { + int previousOpenMPThreads; + + public: +#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP + TbbOpenMPMixedScope() : + previousOpenMPThreads(omp_get_num_threads()) + { + omp_set_num_threads(omp_get_num_procs() / 4); + } + + ~TbbOpenMPMixedScope() + { + omp_set_num_threads(previousOpenMPThreads); + } +#else + TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {} + ~TbbOpenMPMixedScope() {} +#endif + }; + +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + +} + +/* ************************************************************************* */ +/** An assertion that throws an exception if NDEBUG is not defined and +* evaluates to an empty statement otherwise. */ +#ifdef NDEBUG +#define assert_throw(CONDITION, EXCEPTION) ((void)0) +#else +#define assert_throw(CONDITION, EXCEPTION) \ + if (!(CONDITION)) { \ + throw (EXCEPTION); \ + } +#endif + +#ifdef _MSC_VER + +// Define some common g++ functions and macros we use that MSVC does not have + +#include +namespace std { + template inline int isfinite(T a) { + return (int)boost::math::isfinite(a); } + template inline int isnan(T a) { + return (int)boost::math::isnan(a); } + template inline int isinf(T a) { + return (int)boost::math::isinf(a); } +} + +#include +#ifndef M_PI +#define M_PI (boost::math::constants::pi()) +#endif +#ifndef M_PI_2 +#define M_PI_2 (boost::math::constants::pi() / 2.0) +#endif +#ifndef M_PI_4 +#define M_PI_4 (boost::math::constants::pi() / 4.0) +#endif + +#endif + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +#ifdef ERROR +#undef ERROR +#endif diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 33d14e6dd..dc7466199 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -158,10 +158,10 @@ namespace gtsam { // /** // * Apply a reduction, which is a remapping of variable indices. // */ -// virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { -// DiscreteFactor::reduceWithInverse(inverseReduction); -// Potentials::reduceWithInverse(inverseReduction); -// } +// virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { +// DiscreteFactor::reduceWithInverse(inverseReduction); +// Potentials::reduceWithInverse(inverseReduction); +// } /// @} }; diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index d2ccce055..77be1d277 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -1,134 +1,134 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file DiscreteFactorGraph.cpp - * @date Feb 14, 2011 - * @author Duy-Nguyen Ta - * @author Frank Dellaert - */ - -//#define ENABLE_TIMING -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - // Instantiate base classes - template class FactorGraph; - template class EliminateableFactorGraph; - - /* ************************************************************************* */ - bool DiscreteFactorGraph::equals(const This& fg, double tol) const - { - return Base::equals(fg, tol); - } - - /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) keys.insert(factor->begin(), factor->end()); - return keys; - } - - /* ************************************************************************* */ - DecisionTreeFactor DiscreteFactorGraph::product() const { - DecisionTreeFactor result; - BOOST_FOREACH(const sharedFactor& factor, *this) - if (factor) result = (*factor) * result; - return result; - } - - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteFactor::Values &values) const { - double product = 1.0; - BOOST_FOREACH( const sharedFactor& factor, factors_ ) - product *= (*factor)(values); - return product; - } - - /* ************************************************************************* */ - void DiscreteFactorGraph::print(const std::string& s, - const KeyFormatter& formatter) const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); - } - } - -// /* ************************************************************************* */ -// void DiscreteFactorGraph::permuteWithInverse( -// const Permutation& inversePermutation) { -// BOOST_FOREACH(const sharedFactor& factor, factors_) { -// if(factor) -// factor->permuteWithInverse(inversePermutation); -// } -// } -// -// /* ************************************************************************* */ -// void DiscreteFactorGraph::reduceWithInverse( -// const internal::Reduction& inverseReduction) { -// BOOST_FOREACH(const sharedFactor& factor, factors_) { -// if(factor) -// factor->reduceWithInverse(inverseReduction); -// } -// } - - /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const - { - gttic(DiscreteFactorGraph_optimize); - return BaseEliminateable::eliminateSequential()->optimize(); - } - - /* ************************************************************************* */ - std::pair // - EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - - // PRODUCT: multiply all factors - gttic(product); - DecisionTreeFactor product; - BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors) - product = (*factor) * product; - gttoc(product); - - // sum out frontals, this is the factor on the separator - gttic(sum); - DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); - gttoc(sum); - - // Ordering keys for the conditional so that frontalKeys are really in front - Ordering orderedKeys; - orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end()); - orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end()); - - // now divide product/sum to get conditional - gttic(divide); - DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys)); - gttoc(divide); - - return std::make_pair(cond, sum); - } - -/* ************************************************************************* */ -} // namespace - +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file DiscreteFactorGraph.cpp + * @date Feb 14, 2011 + * @author Duy-Nguyen Ta + * @author Frank Dellaert + */ + +//#define ENABLE_TIMING +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + + // Instantiate base classes + template class FactorGraph; + template class EliminateableFactorGraph; + + /* ************************************************************************* */ + bool DiscreteFactorGraph::equals(const This& fg, double tol) const + { + return Base::equals(fg, tol); + } + + /* ************************************************************************* */ + FastSet DiscreteFactorGraph::keys() const { + FastSet keys; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) keys.insert(factor->begin(), factor->end()); + return keys; + } + + /* ************************************************************************* */ + DecisionTreeFactor DiscreteFactorGraph::product() const { + DecisionTreeFactor result; + BOOST_FOREACH(const sharedFactor& factor, *this) + if (factor) result = (*factor) * result; + return result; + } + + /* ************************************************************************* */ + double DiscreteFactorGraph::operator()( + const DiscreteFactor::Values &values) const { + double product = 1.0; + BOOST_FOREACH( const sharedFactor& factor, factors_ ) + product *= (*factor)(values); + return product; + } + + /* ************************************************************************* */ + void DiscreteFactorGraph::print(const std::string& s, + const KeyFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + } + } + +// /* ************************************************************************* */ +// void DiscreteFactorGraph::permuteWithInverse( +// const Permutation& inversePermutation) { +// BOOST_FOREACH(const sharedFactor& factor, factors_) { +// if(factor) +// factor->permuteWithInverse(inversePermutation); +// } +// } +// +// /* ************************************************************************* */ +// void DiscreteFactorGraph::reduceWithInverse( +// const internal::Reduction& inverseReduction) { +// BOOST_FOREACH(const sharedFactor& factor, factors_) { +// if(factor) +// factor->reduceWithInverse(inverseReduction); +// } +// } + + /* ************************************************************************* */ + DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const + { + gttic(DiscreteFactorGraph_optimize); + return BaseEliminateable::eliminateSequential()->optimize(); + } + + /* ************************************************************************* */ + std::pair // + EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { + + // PRODUCT: multiply all factors + gttic(product); + DecisionTreeFactor product; + BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors) + product = (*factor) * product; + gttoc(product); + + // sum out frontals, this is the factor on the separator + gttic(sum); + DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); + gttoc(sum); + + // Ordering keys for the conditional so that frontalKeys are really in front + Ordering orderedKeys; + orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end()); + orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end()); + + // now divide product/sum to get conditional + gttic(divide); + DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys)); + gttoc(divide); + + return std::make_pair(cond, sum); + } + +/* ************************************************************************* */ +} // namespace + diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index cd186beaf..27eb722d9 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -138,10 +138,10 @@ public: * to calling graph.eliminateSequential()->optimize(). */ DiscreteFactor::sharedValues optimize() const; - -// /** Permute the variables in the factors */ -// GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); -// + +// /** Permute the variables in the factors */ +// GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); +// // /** Apply a reduction, which is a remapping of variable indices. */ // GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); }; diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp new file mode 100644 index 000000000..cccedc5bb --- /dev/null +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -0,0 +1,152 @@ +/* + * @file EssentialMatrix.cpp + * @brief EssentialMatrix class + * @author Frank Dellaert + * @date December 5, 2014 + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, + boost::optional H) { + const Rot3& _1R2_ = _1P2_.rotation(); + const Point3& _1T2_ = _1P2_.translation(); + if (!H) { + // just make a direction out of translation and create E + Sphere2 direction(_1T2_); + return EssentialMatrix(_1R2_, direction); + } else { + // Calculate the 5*6 Jacobian H = D_E_1P2 + // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation + // First get 2*3 derivative from Sphere2::FromPoint3 + Matrix D_direction_1T2; + Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2); + H->resize(5, 6); + H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left + H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left + H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right + H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + return EssentialMatrix(_1R2_, direction); + } +} + +/* ************************************************************************* */ +void EssentialMatrix::print(const string& s) const { + cout << s; + aRb_.print("R:\n"); + aTb_.print("d: "); +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { + assert(xi.size() == 5); + Vector3 omega(sub(xi, 0, 3)); + Vector2 z(sub(xi, 3, 5)); + Rot3 R = aRb_.retract(omega); + Sphere2 t = aTb_.retract(z); + return EssentialMatrix(R, t); +} + +/* ************************************************************************* */ +Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + return Vector(5) << // + aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); +} + +/* ************************************************************************* */ +Point3 EssentialMatrix::transform_to(const Point3& p, + boost::optional DE, boost::optional Dpoint) const { + Pose3 pose(aRb_, aTb_.point3()); + Point3 q = pose.transform_to(p, DE, Dpoint); + if (DE) { + // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 + // The last 3 columns are derivative with respect to change in translation + // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() + // Duy made an educated guess that this needs to be rotated to the local frame + Matrix H(3, 5); + H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + *DE = H; + } + return q; +} + +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, + boost::optional HE, boost::optional HR) const { + + // The rotation must be conjugated to act in the camera frame + Rot3 c1Rc2 = aRb_.conjugate(cRb); + + if (!HE && !HR) { + // Rotate translation direction and return + Sphere2 c1Tc2 = cRb * aTb_; + return EssentialMatrix(c1Rc2, c1Tc2); + } else { + // Calculate derivatives + Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); + if (HE) { + *HE = zeros(5, 5); + HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 + HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) + } + if (HR) { + throw runtime_error( + "EssentialMatrix::rotate: derivative HR not implemented yet"); + /* + HR->resize(5, 3); + HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? + HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? + */ + } + return EssentialMatrix(c1Rc2, c1Tc2); + } +} + +/* ************************************************************************* */ +double EssentialMatrix::error(const Vector& vA, const Vector& vB, // + boost::optional H) const { + if (H) { + H->resize(1, 5); + // See math.lyx + Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) + * aTb_.basis(); + *H << HR, HD; + } + return dot(vA, E_ * vB); +} + +/* ************************************************************************* */ +ostream& operator <<(ostream& os, const EssentialMatrix& E) { + Rot3 R = E.rotation(); + Sphere2 d = E.direction(); + os.precision(10); + os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + return os; +} + +/* ************************************************************************* */ +istream& operator >>(istream& is, EssentialMatrix& E) { + double rx, ry, rz, dx, dy, dz; + is >> rx >> ry >> rz; // Read the rotation rxyz + is >> dx >> dy >> dz; // Read the translation dxyz + + // Create EssentialMatrix from rotation and translation + Rot3 rot = Rot3::RzRyRx(rx, ry, rz); + Sphere2 dt = Sphere2(dx, dy, dz); + E = EssentialMatrix(rot, dt); + + return is; +} + +/* ************************************************************************* */ + +} // gtsam + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 54fd94bbc..a7270fff0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -26,7 +26,7 @@ private: Rot3 aRb_; ///< Rotation between a and b Sphere2 aTb_; ///< translation direction from a to b - Matrix E_; ///< Essential matrix + Matrix3 E_; ///< Essential matrix public: @@ -48,6 +48,10 @@ public: aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { } + /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) + static EssentialMatrix FromPose3(const Pose3& _1P2_, + boost::optional H = boost::none); + /// Random, using Rot3::Random and Sphere2::Random template static EssentialMatrix Random(Engine & rng) { @@ -60,11 +64,7 @@ public: /// @{ /// print with optional string - void print(const std::string& s = "") const { - std::cout << s; - aRb_.print("R:\n"); - aTb_.print("d: "); - } + void print(const std::string& s = "") const; /// assert equality up to a tolerance bool equals(const EssentialMatrix& other, double tol = 1e-8) const { @@ -87,20 +87,10 @@ public: } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const { - assert(xi.size() == 5); - Vector3 omega(sub(xi, 0, 3)); - Vector2 z(sub(xi, 3, 5)); - Rot3 R = aRb_.retract(omega); - Sphere2 t = aTb_.retract(z); - return EssentialMatrix(R, t); - } + virtual EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const { - return Vector(5) << // - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_); - } + virtual Vector localCoordinates(const EssentialMatrix& other) const; /// @} @@ -108,17 +98,17 @@ public: /// @{ /// Rotation - const Rot3& rotation() const { + inline const Rot3& rotation() const { return aRb_; } /// Direction - const Sphere2& direction() const { + inline const Sphere2& direction() const { return aTb_; } /// Return 3*3 matrix representation - const Matrix& matrix() const { + inline const Matrix3& matrix() const { return E_; } @@ -131,20 +121,7 @@ public: */ Point3 transform_to(const Point3& p, boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const { - Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); - if (DE) { - // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 - // The last 3 columns are derivative with respect to change in translation - // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() - // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); - *DE = H; - } - return q; - } + boost::optional Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -152,36 +129,7 @@ public: * @param E essential matrix E in camera frame C */ EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const { - - // The rotation must be conjugated to act in the camera frame - Rot3 c1Rc2 = aRb_.conjugate(cRb); - - if (!HE && !HR) { - // Rotate translation direction and return - Sphere2 c1Tc2 = cRb * aTb_; - return EssentialMatrix(c1Rc2, c1Tc2); - } else { - // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 - Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) - } - if (HR) { - throw std::runtime_error( - "EssentialMatrix::rotate: derivative HR not implemented yet"); - /* - HR->resize(5, 3); - HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? - HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? - */ - } - return EssentialMatrix(c1Rc2, c1Tc2); - } - } + boost::none, boost::optional HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -194,17 +142,18 @@ public: /// epipolar error, algebraic double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const { - if (H) { - H->resize(1, 5); - // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) - * aTb_.basis(); - *H << HR, HD; - } - return dot(vA, E_ * vB); - } + boost::optional H = boost::none) const; + + /// @} + + /// @name Streaming operators + /// @{ + + /// stream to stream + friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E); + + /// stream from stream + friend std::istream& operator >>(std::istream& is, EssentialMatrix& E); /// @} diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 0838650ea..dc9a1dac8 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -172,7 +172,7 @@ public: static inline Point2 Expmap(const Vector& v) { return Point2(v); } /// 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()); } + static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); } /// @} /// @name Vector Space diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 4f5f43665..b6244630e 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3); /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol + && fabs(z_ - q.z()) < tol); } /* ************************************************************************* */ @@ -37,18 +38,18 @@ void Point3::print(const string& s) const { /* ************************************************************************* */ -bool Point3::operator== (const Point3& q) const { +bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } /* ************************************************************************* */ Point3 Point3::operator+(const Point3& q) const { - return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); + return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); } /* ************************************************************************* */ -Point3 Point3::operator- (const Point3& q) const { - return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); +Point3 Point3::operator-(const Point3& q) const { + return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } /* ************************************************************************* */ @@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = eye(3,3); +Point3 Point3::add(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = eye(3, 3); return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = eye(3,3); - if (H2) *H2 = -eye(3,3); +Point3 Point3::sub(const Point3 &q, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = eye(3, 3); + if (H2) + *H2 = -eye(3, 3); return *this - q; } /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q) const { - return Point3( y_*q.z_ - z_*q.y_, - z_*q.x_ - x_*q.z_, - x_*q.y_ - y_*q.x_ ); + return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, + x_ * q.y_ - y_ * q.x_); } /* ************************************************************************* */ double Point3::dot(const Point3 &q) const { - return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); + return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); } /* ************************************************************************* */ double Point3::norm() const { - return sqrt( x_*x_ + y_*y_ + z_*z_ ); + return sqrt(x_ * x_ + y_ * y_ + z_ * z_); +} + +/* ************************************************************************* */ +Point3 Point3::normalize(boost::optional H) const { + Point3 normalized = *this / norm(); + if (H) { + // 3*3 Derivative + double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; + double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; + H->resize(3, 3); + *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + *H /= pow(x2 + y2 + z2, 1.5); + } + return normalized; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5e2e0ff42..66924ec07 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -176,6 +176,9 @@ namespace gtsam { /** Distance of the point from the origin */ double norm() const; + /** normalize, with optional Jacobian */ + Point3 normalize(boost::optional H = boost::none) const; + /** cross product @return this x q */ Point3 cross(const Point3 &q) const; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index adbe763b3..bfd2fcb9a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -98,7 +98,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, /* ************************************************************************* */ Matrix6 Pose3::dExpInv_exp(const Vector& xi) { // Bernoulli numbers, from Wikipedia - static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, + static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, 0.0, 1.0 / 42.0, 0.0, -1.0 / 30); static const int N = 5; // order of approximation Matrix res = I6; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 45012770f..d121beb12 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -170,7 +170,7 @@ namespace gtsam { ///Log map at identity - return the canonical coordinates of this rotation static inline Vector Logmap(const Rot2& r) { - return Vector_(1, r.theta()); + return (Vector(1) << r.theta()); } /// @} diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8dcf14d4b..ef82e9369 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,198 +1,209 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Rot3.cpp - * @brief Rotation, common code between Rotation matrix and Quaternion - * @author Alireza Fathi - * @author Christian Potthast - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - -static const Matrix3 I3 = Matrix3::Identity(); - -/* ************************************************************************* */ -void Rot3::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Point3& w, double theta) { - return rodriguez((Vector)w.vector(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { - return rodriguez(w.point3(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::Random(boost::random::mt19937 & rng) { - // TODO allow any engine without including all of boost :-( - Sphere2 w = Sphere2::Random(rng); - boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); - double angle = randomAngle(rng); - return rodriguez(w,angle); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3(); - return rodriguez(w/t, t); -} - -/* ************************************************************************* */ -bool Rot3::equals(const Rot3 & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); -} - -/* ************************************************************************* */ -Point3 Rot3::operator*(const Point3& p) const { - return rotate(p); -} - -/* ************************************************************************* */ -Sphere2 Rot3::rotate(const Sphere2& p, - boost::optional HR, boost::optional Hp) const { - Sphere2 q = rotate(p.point3(Hp)); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * (*Hp); - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); - return q; -} - -/* ************************************************************************* */ -Sphere2 Rot3::operator*(const Sphere2& p) const { - return rotate(p); -} - -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; - return res; -} - - -/* ************************************************************************* */ -Point3 Rot3::column(int index) const{ - if(index == 3) - return r3(); - else if(index == 2) - return r2(); - else if(index == 1) - return r1(); // default returns r1 - else - throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); -} - -/* ************************************************************************* */ -Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; - boost::tie(I,q)=RQ(matrix()); - return q; -} - -/* ************************************************************************* */ -Vector3 Rot3::ypr() const { - Vector3 q = xyz(); - return Vector3(q(2),q(1),q(0)); -} - -/* ************************************************************************* */ -Vector3 Rot3::rpy() const { - return xyz(); -} - -/* ************************************************************************* */ -Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); - Vector v(4); - v(0) = q.w(); - v(1) = q.x(); - v(2) = q.y(); - v(3) = q.z(); - return v; -} - -/* ************************************************************************* */ -pair RQ(const Matrix3& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3 Qx = Rot3::Rx(-x); - Matrix3 B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3 Qy = Rot3::Ry(-y); - Matrix3 C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3 Qz = Rot3::Rz(-z); - Matrix3 R = C * Qz.matrix(); - - Vector xyz = Vector3(x, y, z); - return make_pair(R, xyz); -} - -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Rot3& R) { - os << "\n"; - os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; - os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; - os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; - return os; -} - -/* ************************************************************************* */ - -} // namespace gtsam - +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3.cpp + * @brief Rotation, common code between Rotation matrix and Quaternion + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +static const Matrix3 I3 = Matrix3::Identity(); + +/* ************************************************************************* */ +void Rot3::print(const std::string& s) const { + gtsam::print((Matrix)matrix(), s); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Point3& w, double theta) { + return rodriguez((Vector)w.vector(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { + return rodriguez(w.point3(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::Random(boost::random::mt19937 & rng) { + // TODO allow any engine without including all of boost :-( + Sphere2 w = Sphere2::Random(rng); + boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); + double angle = randomAngle(rng); + return rodriguez(w,angle); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3(); + return rodriguez(w/t, t); +} + +/* ************************************************************************* */ +bool Rot3::equals(const Rot3 & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} + +/* ************************************************************************* */ +Point3 Rot3::operator*(const Point3& p) const { + return rotate(p); +} + +/* ************************************************************************* */ +Sphere2 Rot3::rotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q = rotate(p.point3(Hp)); + if (Hp) + (*Hp) = q.basis().transpose() * matrix() * (*Hp); + if (HR) + (*HR) = -q.basis().transpose() * matrix() * p.skew(); + return q; +} + +/* ************************************************************************* */ +Sphere2 Rot3::unrotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q = unrotate(p.point3(Hp)); + if (Hp) + (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); + if (HR) + (*HR) = q.basis().transpose() * q.skew(); + return q; +} + +/* ************************************************************************* */ +Sphere2 Rot3::operator*(const Sphere2& p) const { + return rotate(p); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; + return res; +} + +/* ************************************************************************* */ +/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) +Matrix3 Rot3::dexpInvL(const Vector3& v) { + if(zero(v)) return eye(3); + Matrix x = skewSymmetric(v); + Matrix x2 = x*x; + double theta = v.norm(), vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + Matrix res = eye(3) + 0.5*x - s2*x2; + return res; +} + + +/* ************************************************************************* */ +Point3 Rot3::column(int index) const{ + if(index == 3) + return r3(); + else if(index == 2) + return r2(); + else if(index == 1) + return r1(); // default returns r1 + else + throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3"); +} + +/* ************************************************************************* */ +Vector3 Rot3::xyz() const { + Matrix I;Vector3 q; + boost::tie(I,q)=RQ(matrix()); + return q; +} + +/* ************************************************************************* */ +Vector3 Rot3::ypr() const { + Vector3 q = xyz(); + return Vector3(q(2),q(1),q(0)); +} + +/* ************************************************************************* */ +Vector3 Rot3::rpy() const { + return xyz(); +} + +/* ************************************************************************* */ +Vector Rot3::quaternion() const { + Quaternion q = toQuaternion(); + Vector v(4); + v(0) = q.w(); + v(1) = q.x(); + v(2) = q.y(); + v(3) = q.z(); + return v; +} + +/* ************************************************************************* */ +pair RQ(const Matrix3& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3 Qx = Rot3::Rx(-x); + Matrix3 B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3 Qy = Rot3::Ry(-y); + Matrix3 C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3 Qz = Rot3::Rz(-z); + Matrix3 R = C * Qz.matrix(); + + Vector xyz = Vector3(x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Rot3& R) { + os << "\n"; + os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; + os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; + os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; + return os; +} + +/* ************************************************************************* */ + +} // namespace gtsam + diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ea59fab17..847d31d65 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -194,7 +194,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez(Vector_(3,wx,wy,wz));} + { return rodriguez((Vector(3) << wx, wy, wz));} /// @} /// @name Testable @@ -331,6 +331,10 @@ namespace gtsam { Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, boost::optional Hp = boost::none) const; + /// unrotate 3D direction from world frame to rotated coordinate frame + Sphere2 unrotate(const Sphere2& p, boost::optional HR = boost::none, + boost::optional Hp = boost::none) const; + /// rotate 3D direction from rotated coordinate frame to world frame Sphere2 operator*(const Sphere2& p) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 99c5c619e..6257d8400 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -1,308 +1,308 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Rot3M.cpp - * @brief Rotation (internal: 3*3 matrix representation*) - * @author Alireza Fathi - * @author Christian Potthast - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include // Get GTSAM_USE_QUATERNIONS macro - -#ifndef GTSAM_USE_QUATERNIONS - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -static const Matrix3 I3 = Matrix3::Identity(); - -/* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} - -/* ************************************************************************* */ -Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1.vector(); - rot_.col(1) = col2.vector(); - rot_.col(2) = col3.vector(); -} - -/* ************************************************************************* */ -Rot3::Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) { - rot_ << R11, R12, R13, - R21, R22, R23, - R31, R32, R33; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix3& R) { - rot_ = R; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix& R) { - if (R.rows()!=3 || R.cols()!=3) - throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - rot_ = R; -} - -///* ************************************************************************* */ -//Rot3::Rot3(const Matrix3& R) : rot_(R) {} - -/* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} - -/* ************************************************************************* */ -Rot3 Rot3::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); -} - -/* ************************************************************************* */ -Rot3 Rot3::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); -} - -/* ************************************************************************* */ -Rot3 Rot3::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); -} - -/* ************************************************************************* */ -// Considerably faster than composing matrices above ! -Rot3 Rot3::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; -#ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); -#endif - - double c = cos(theta), s = sin(theta), c_1 = 1 - c; - - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; - - return Rot3( - c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(Matrix3(rot_*R2.rot_)); -} - -/* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(rot_.transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; - return Rot3(Matrix3(rot_.transpose()*R2.rot_)); -} - -/* ************************************************************************* */ -Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1 || H2) { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; - } - return Point3(rot_ * p.vector()); -} - -/* ************************************************************************* */ -// Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { - - static const double PI = boost::math::constants::pi(); - - const Matrix3& rot = R.rot_; - // Get trace(R) - double tr = rot.trace(); - - // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. - // we do something special - if (std::abs(tr+1.0) < 1e-10) { - if(std::abs(rot(2,2)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(2,2) )) * - Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); - else if(std::abs(rot(1,1)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(1,1))) * - Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); - else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * - Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); - } else { - double magnitude; - double tr_3 = tr-3.0; // always negative - if (tr_3<-1e-7) { - double theta = acos((tr-1.0)/2.0); - magnitude = theta/(2.0*sin(theta)); - } else { - // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) - // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) - magnitude = 0.5 - tr_3*tr_3/12.0; - } - return magnitude*Vector3( - rot(2,1)-rot(1,2), - rot(0,2)-rot(2,0), - rot(1,0)-rot(0,1)); - } -} - -/* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { - const double x = omega(0), y = omega(1), z = omega(2); - const double x2 = x * x, y2 = y * y, z2 = z * z; - const double xy = x * y, xz = x * z, yz = y * z; - const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, - (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, - (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); -} - -/* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); - return (*this)*Cayley<3>(-Omega/2); - } else { - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = (a * f - cd + f) * K; - const double y = (b * f - ce - c) * K; - const double z = (fg - di - d) * K; - return -2 * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); - // using templated version of Cayley - Eigen::Matrix3d Omega = Cayley<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); - } else { - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -Matrix3 Rot3::matrix() const { - return rot_; -} - -/* ************************************************************************* */ -Matrix3 Rot3::transpose() const { - return rot_.transpose(); -} - -/* ************************************************************************* */ -Point3 Rot3::r1() const { return Point3(rot_.col(0)); } - -/* ************************************************************************* */ -Point3 Rot3::r2() const { return Point3(rot_.col(1)); } - -/* ************************************************************************* */ -Point3 Rot3::r3() const { return Point3(rot_.col(2)); } - -/* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); -} - -/* ************************************************************************* */ - -} // namespace gtsam - -#endif +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot3M.cpp + * @brief Rotation (internal: 3*3 matrix representation*) + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include // Get GTSAM_USE_QUATERNIONS macro + +#ifndef GTSAM_USE_QUATERNIONS + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +static const Matrix3 I3 = Matrix3::Identity(); + +/* ************************************************************************* */ +Rot3::Rot3() : rot_(Matrix3::Identity()) {} + +/* ************************************************************************* */ +Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { + rot_.col(0) = col1.vector(); + rot_.col(1) = col2.vector(); + rot_.col(2) = col3.vector(); +} + +/* ************************************************************************* */ +Rot3::Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) { + rot_ << R11, R12, R13, + R21, R22, R23, + R31, R32, R33; +} + +/* ************************************************************************* */ +Rot3::Rot3(const Matrix3& R) { + rot_ = R; +} + +/* ************************************************************************* */ +Rot3::Rot3(const Matrix& R) { + if (R.rows()!=3 || R.cols()!=3) + throw invalid_argument("Rot3 constructor expects 3*3 matrix"); + rot_ = R; +} + +///* ************************************************************************* */ +//Rot3::Rot3(const Matrix3& R) : rot_(R) {} + +/* ************************************************************************* */ +Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} + +/* ************************************************************************* */ +Rot3 Rot3::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} + +/* ************************************************************************* */ +Rot3 Rot3::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} + +/* ************************************************************************* */ +Rot3 Rot3::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} + +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3 Rot3::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +#ifndef NDEBUG + double l_n = wwTxx + wwTyy + wwTzz; + if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); +#endif + + double c = cos(theta), s = sin(theta), c_1 = 1 - c; + + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; + + return Rot3( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} + +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(Matrix3(rot_*R2.rot_)); +} + +/* ************************************************************************* */ +Rot3 Rot3::inverse(boost::optional H1) const { + if (H1) *H1 = -rot_; + return Rot3(Matrix3(rot_.transpose())); +} + +/* ************************************************************************* */ +Rot3 Rot3::between (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*rot_); + if (H2) *H2 = I3; + return Rot3(Matrix3(rot_.transpose()*R2.rot_)); +} + +/* ************************************************************************* */ +Point3 Rot3::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1 || H2) { + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; + } + return Point3(rot_ * p.vector()); +} + +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector3 Rot3::Logmap(const Rot3& R) { + + static const double PI = boost::math::constants::pi(); + + const Matrix3& rot = R.rot_; + // Get trace(R) + double tr = rot.trace(); + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr+1.0) < 1e-10) { + if(std::abs(rot(2,2)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(2,2) )) * + Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); + else if(std::abs(rot(1,1)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(1,1))) * + Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); + else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + return (PI / sqrt(2.0+2.0*rot(0,0))) * + Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); + } else { + double magnitude; + double tr_3 = tr-3.0; // always negative + if (tr_3<-1e-7) { + double theta = acos((tr-1.0)/2.0); + magnitude = theta/(2.0*sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3*tr_3/12.0; + } + return magnitude*Vector3( + rot(2,1)-rot(1,2), + rot(0,2)-rot(2,0), + rot(1,0)-rot(0,1)); + } +} + +/* ************************************************************************* */ +Rot3 Rot3::retractCayley(const Vector& omega) const { + const double x = omega(0), y = omega(1), z = omega(2); + const double x2 = x * x, y2 = y * y, z2 = z * z; + const double xy = x * y, xz = x * z, yz = y * z; + const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; + return (*this) + * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, + (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); +} + +/* ************************************************************************* */ +Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { + if(mode == Rot3::EXPMAP) { + return (*this)*Expmap(omega); + } else if(mode == Rot3::CAYLEY) { + return retractCayley(omega); + } else if(mode == Rot3::SLOW_CAYLEY) { + Matrix Omega = skewSymmetric(omega); + return (*this)*Cayley<3>(-Omega/2); + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { + if(mode == Rot3::EXPMAP) { + return Logmap(between(T)); + } else if(mode == Rot3::CAYLEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a=A(0,0),b=A(0,1),c=A(0,2); + const double d=A(1,0),e=A(1,1),f=A(1,2); + const double g=A(2,0),h=A(2,1),i=A(2,2); + const double di = d*i, ce = c*e, cd = c*d, fg=f*g; + const double M = 1 + e - f*h + i + e*i; + const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); + const double x = (a * f - cd + f) * K; + const double y = (b * f - ce - c) * K; + const double z = (fg - di - d) * K; + return -2 * Vector3(x, y, z); + } else if(mode == Rot3::SLOW_CAYLEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // using templated version of Cayley + Eigen::Matrix3d Omega = Cayley<3>(A); + return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Matrix3 Rot3::matrix() const { + return rot_; +} + +/* ************************************************************************* */ +Matrix3 Rot3::transpose() const { + return rot_.transpose(); +} + +/* ************************************************************************* */ +Point3 Rot3::r1() const { return Point3(rot_.col(0)); } + +/* ************************************************************************* */ +Point3 Rot3::r2() const { return Point3(rot_.col(1)); } + +/* ************************************************************************* */ +Point3 Rot3::r3() const { return Point3(rot_.col(2)); } + +/* ************************************************************************* */ +Quaternion Rot3::toQuaternion() const { + return Quaternion(rot_); +} + +/* ************************************************************************* */ + +} // namespace gtsam + +#endif diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index b6cae287c..ed2561e22 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -14,6 +14,7 @@ * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert + * @author Alex Trevor * @brief The Sphere2 class - basically a point on a unit sphere */ @@ -27,6 +28,21 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +Sphere2 Sphere2::FromPoint3(const Point3& point, + boost::optional H) { + Sphere2 direction(point); + if (H) { + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix D_p_point; + point.normalize(D_p_point); // TODO, this calculates norm a second time :-( + // Calculate the 2*3 Jacobian + H->resize(2, 3); + *H << direction.basis().transpose() * D_p_point; + } + return direction; +} + /* ************************************************************************* */ Sphere2 Sphere2::Random(boost::random::mt19937 & rng) { // TODO allow any engine without including all of boost :-( @@ -98,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional H) const { } /* ************************************************************************* */ -Sphere2 Sphere2::retract(const Vector& v) const { +Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { // Get the vector form of the point and the basis matrix Vector p = Point3::Logmap(p_); @@ -106,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const { // Compute the 3D xi_hat vector Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); - Vector newPoint = p + xi_hat; + + if (mode == Sphere2::EXPMAP) { + double xi_hat_norm = xi_hat.norm(); - // Project onto the manifold, i.e. the closest point on the circle to the new location; - // same as putting it onto the unit circle - Vector projected = newPoint / newPoint.norm(); + // Avoid nan + if (xi_hat_norm == 0.0) { + if (v.norm () == 0.0) + return Sphere2 (point3 ()); + else + return Sphere2 (-point3 ()); + } + + Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + return Sphere2(exp_p_xi_hat); + } else if (mode == Sphere2::RENORM) { + // Project onto the manifold, i.e. the closest point on the circle to the new location; + // same as putting it onto the unit circle + Vector newPoint = p + xi_hat; + Vector projected = newPoint / newPoint.norm(); - return Sphere2(Point3::Expmap(projected)); + return Sphere2(Point3::Expmap(projected)); + } else { + assert (false); + exit (1); + } } /* ************************************************************************* */ -Vector Sphere2::localCoordinates(const Sphere2& y) const { + Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const { - // Make sure that the angle different between x and y is less than 90. Otherwise, - // we can project x + xi_hat from the tangent space at x to y. - assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); + if (mode == Sphere2::EXPMAP) { + Matrix B = basis(); + + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + double theta = acos(p.transpose() * q); - // Get the basis matrix - Matrix B = basis(); - - // Create the vector forms of p and q (the Point3 of y). - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); - - // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). - double alpha = p.transpose() * q; - assert(alpha != 0.0); - Matrix coeffs = (B.transpose() * q) / alpha; - Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); - return result; + // the below will be nan if theta == 0.0 + if (p == q) + return (Vector (2) << 0, 0); + else if (p == (Vector)-q) + return (Vector (2) << M_PI, 0); + + Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); + Vector result = B.transpose() * result_hat; + + return result; + } else if (mode == Sphere2::RENORM) { + // Make sure that the angle different between x and y is less than 90. Otherwise, + // we can project x + xi_hat from the tangent space at x to y. + assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); + + // Get the basis matrix + Matrix B = basis(); + + // Create the vector forms of p and q (the Point3 of y). + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + + // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). + double alpha = p.transpose() * q; + assert(alpha != 0.0); + Matrix coeffs = (B.transpose() * q) / alpha; + Vector result = (Vector(2) << coeffs(0, 0), coeffs(1, 0)); + return result; + } else { + assert (false); + exit (1); + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 0c549ed0b..ac8124139 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -14,6 +14,7 @@ * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert + * @author Alex Trevor * @brief Develop a Sphere2 class - basically a point on a unit sphere */ @@ -22,6 +23,10 @@ #include #include +#ifndef SPHERE2_DEFAULT_COORDINATES_MODE + #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM +#endif + // (Cumbersome) forward declaration for random generator namespace boost { namespace random { @@ -65,6 +70,10 @@ public: p_ = p_ / p_.norm(); } + /// Named constructor from Point3 with optional Jacobian + static Sphere2 FromPoint3(const Point3& point, + boost::optional H = boost::none); + /// Random direction, using boost::uniform_on_sphere static Sphere2 Random(boost::random::mt19937 & rng); @@ -85,7 +94,11 @@ public: /// @name Other functionality /// @{ - /// Returns the local coordinate frame to tangent plane + /** + * Returns the local coordinate frame to tangent plane + * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions + * tangent to the sphere at the current direction. + */ Matrix basis() const; /// Return skew-symmetric associated with 3D point on unit sphere @@ -98,6 +111,13 @@ public: return p_; } + /// Return unit-norm Vector + Vector unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector ()); + } + /// Signed, vector-valued error between two directions Vector error(const Sphere2& q, boost::optional H = boost::none) const; @@ -121,11 +141,16 @@ public: return 2; } + enum CoordinatesMode { + EXPMAP, ///< Use the exponential map to retract + RENORM ///< Retract with vector addtion and renormalize. + }; + /// The retract function - Sphere2 retract(const Vector& v) const; + Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; /// The local coordinates function - Vector localCoordinates(const Sphere2& s) const; + Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; /// @} }; diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index c73ae1182..53a21a9fb 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate) double g = 1+k[0]*r+k[1]*r*r ; double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ; double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ; - Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ; + Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ; Vector v_i = K.K() * v_hat ; Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; Point2 q = K.uncalibrate(p); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index da5240b15..865f27f81 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -10,6 +10,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) { // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); } +//************************************************************************* +TEST (EssentialMatrix, FromPose3_a) { + Matrix actualH; + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//************************************************************************* +TEST (EssentialMatrix, FromPose3_b) { + Matrix actualH; + Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix trueE(c1Rc2, c1Tc2); + Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + +//************************************************************************* +TEST (EssentialMatrix, streaming) { + EssentialMatrix expected(c1Rc2, c1Tc2), actual; + stringstream ss; + ss << expected; + ss >> actual; + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index ef4a3894c..ab0d12b9e 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -12,75 +12,86 @@ /** * @file testPoint3.cpp * @brief Unit tests for Point3 class - */ + */ -#include -#include #include +#include +#include +#include using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point3) GTSAM_CONCEPT_LIE_INST(Point3) -static Point3 P(0.2,0.7,-2); +static Point3 P(0.2, 0.7, -2); /* ************************************************************************* */ TEST(Point3, Lie) { - Point3 p1(1,2,3); - Point3 p2(4,5,6); + Point3 p1(1, 2, 3); + Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); - EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.)))); + EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ -TEST( Point3, arithmetic) -{ - CHECK(P*3==3*P); - CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) )); - CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1))); - CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1))); - CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2)); - CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3))); - CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2)); +TEST( Point3, arithmetic) { + CHECK(P * 3 == 3 * P); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ -TEST( Point3, equals) -{ +TEST( Point3, equals) { CHECK(P.equals(P)); Point3 Q; CHECK(!P.equals(Q)); } /* ************************************************************************* */ -TEST( Point3, dot) -{ - Point3 origin, ones(1,1,1); - CHECK(origin.dot(Point3(1,1,0)) == 0); - CHECK(ones.dot(Point3(1,1,0)) == 2); +TEST( Point3, dot) { + Point3 origin, ones(1, 1, 1); + CHECK(origin.dot(Point3(1, 1, 0)) == 0); + CHECK(ones.dot(Point3(1, 1, 0)) == 2); } /* ************************************************************************* */ -TEST( Point3, stream) -{ - Point3 p(1,2, -3); +TEST( Point3, stream) { + Point3 p(1, 2, -3); std::ostringstream os; os << p; EXPECT(os.str() == "[1, 2, -3]';"); } +//************************************************************************* +TEST (Point3, normalize) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Point3 expected(point / sqrt(14)); + EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(&Point3::normalize, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 3de661224..c67031a13 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -96,7 +96,7 @@ TEST(Rot2, logmap) { Rot2 rot0(Rot2::fromAngle(M_PI/2.0)); Rot2 rot(Rot2::fromAngle(M_PI)); - Vector expected = Vector_(1, M_PI/2.0); + Vector expected = (Vector(1) << M_PI/2.0); Vector actual = rot0.localCoordinates(rot); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index c6e519a44..306d84b94 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -13,6 +13,8 @@ * @file testSphere2.cpp * @date Feb 03, 2012 * @author Can Erdogan + * @author Frank Dellaert + * @author Alex Trevor * @brief Tests the Sphere2 class */ @@ -25,6 +27,7 @@ #include #include #include +#include using namespace boost::assign; using namespace gtsam; @@ -75,10 +78,35 @@ TEST(Sphere2, rotate) { } } +//******************************************************************************* +static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { + return R.unrotate (p); +} + +TEST(Sphere2, unrotate) { + Rot3 R = Rot3::yaw(-M_PI/4.0); + Sphere2 p(1, 0, 0); + Sphere2 expected = Sphere2(1, 1, 0); + Sphere2 actual = R.unrotate (p); + EXPECT(assert_equal(expected, actual, 1e-8)); + Matrix actualH, expectedH; + // Use numerical derivatives to calculate the expected Jacobian + { + expectedH = numericalDerivative21(unrotate_, R, p); + R.unrotate(p, actualH, boost::none); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } + { + expectedH = numericalDerivative22(unrotate_, R, p); + R.unrotate(p, boost::none, actualH); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } +} + //******************************************************************************* TEST(Sphere2, error) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // + r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5)); EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5)); @@ -101,8 +129,8 @@ TEST(Sphere2, error) { //******************************************************************************* TEST(Sphere2, distance) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // + r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); @@ -146,9 +174,20 @@ TEST(Sphere2, retract) { Vector v(2); v << 0.5, 0; Sphere2 expected(Point3(1, 0, 0.5)); - Sphere2 actual = p.retract(v); + Sphere2 actual = p.retract(v, Sphere2::RENORM); EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8)); +} + +//******************************************************************************* +TEST(Sphere2, retract_expmap) { + Sphere2 p; + Vector v(2); + v << (M_PI/2.0), 0; + Sphere2 expected(Point3(0, 0, 1)); + Sphere2 actual = p.retract(v, Sphere2::EXPMAP); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8)); } //******************************************************************************* @@ -174,9 +213,9 @@ inline static Vector randomVector(const Vector& minLimits, TEST(Sphere2, localCoordinates_retract) { size_t numIterations = 10000; - Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = - Vector_(3, 1.0, 1.0, 1.0); - Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0); + Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = + (Vector(3) << 1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -198,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) { } } +//******************************************************************************* +// Let x and y be two Sphere2's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +TEST(Sphere2, localCoordinates_retract_expmap) { + + size_t numIterations = 10000; + Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit = + (Vector(3) << 1.0, 1.0, 1.0); + Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). + sleep(0); + + // Create the two Sphere2s. + // Unlike the above case, we can use any two sphers. + Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.norm () > M_PI) + v12 = v12 / M_PI; + Sphere2 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Sphere2 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + //******************************************************************************* //TEST( Pose2, between ) //{ @@ -216,7 +288,7 @@ TEST(Sphere2, localCoordinates_retract) { // EXPECT(assert_equal(expected,actual1)); // EXPECT(assert_equal(expected,actual2)); // -// Matrix expectedH1 = Matrix_(3,3, +// Matrix expectedH1 = (Matrix(3,3) << // 0.0,-1.0,-2.0, // 1.0, 0.0,-2.0, // 0.0, 0.0,-1.0 @@ -227,7 +299,7 @@ TEST(Sphere2, localCoordinates_retract) { // // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx // EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); // -// Matrix expectedH2 = Matrix_(3,3, +// Matrix expectedH2 = (Matrix(3,3) << // 1.0, 0.0, 0.0, // 0.0, 1.0, 0.0, // 0.0, 0.0, 1.0 @@ -253,6 +325,17 @@ TEST(Sphere2, Random) { EXPECT(assert_equal(expectedMean,actualMean,0.1)); } +//************************************************************************* +TEST (Sphere2, FromPoint3) { + Matrix actualH; + Point3 point(1, -2, 3); // arbitrary point + Sphere2 expected(point); + EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8)); + Matrix expectedH = numericalDerivative11( + boost::bind(Sphere2::FromPoint3, _1, boost::none), point); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/tests/timePose2.cpp b/gtsam/geometry/tests/timePose2.cpp index a8f2f7137..28044068c 100644 --- a/gtsam/geometry/tests/timePose2.cpp +++ b/gtsam/geometry/tests/timePose2.cpp @@ -138,7 +138,7 @@ int main() // create a random pose: double x = 4.0, y = 2.0, r = 0.3; - Vector v = Vector_(3,x,y,r); + Vector v = (Vector(3) << x, y, r); Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3); TEST(Expmap, Pose2::Expmap(v)); diff --git a/gtsam/geometry/tests/timePose3.cpp b/gtsam/geometry/tests/timePose3.cpp index 9538ad4b4..13e630706 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/gtsam/geometry/tests/timePose3.cpp @@ -17,23 +17,23 @@ #include -#include +#include #include using namespace std; using namespace gtsam; -/* ************************************************************************* */ +/* ************************************************************************* */ #define TEST(TITLE,STATEMENT) \ - gttic_(TITLE); \ - for(int i = 0; i < n; i++) \ - STATEMENT; \ - gttoc_(TITLE); + gttic_(TITLE); \ + for(int i = 0; i < n; i++) \ + STATEMENT; \ + gttoc_(TITLE); int main() { - int n = 5000000; - cout << "NOTE: Times are reported for " << n << " calls" << endl; + int n = 5000000; + cout << "NOTE: Times are reported for " << n << " calls" << endl; double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; @@ -47,9 +47,9 @@ int main() TEST(between, T.between(T2)) TEST(between_derivatives, T.between(T2,H1,H2)) TEST(Logmap, Pose3::Logmap(T.between(T2))) - - // Print timings - tictoc_print_(); + + // Print timings + tictoc_print_(); return 0; } diff --git a/gtsam/geometry/tests/timeRot2.cpp b/gtsam/geometry/tests/timeRot2.cpp index 6d828ef9c..86dda2b8c 100644 --- a/gtsam/geometry/tests/timeRot2.cpp +++ b/gtsam/geometry/tests/timeRot2.cpp @@ -95,7 +95,7 @@ int main() // create a random direction: double norm=sqrt(16.0+4.0); double x=4.0/norm, y=2.0/norm; - Vector v = Vector_(2,x,y); + Vector v = (Vector(2) << x, y); Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); TEST(Rot2_Expmap, Rot2::Expmap(v)); diff --git a/gtsam/geometry/tests/timeRot3.cpp b/gtsam/geometry/tests/timeRot3.cpp index 64feb1909..dd98465ed 100644 --- a/gtsam/geometry/tests/timeRot3.cpp +++ b/gtsam/geometry/tests/timeRot3.cpp @@ -39,7 +39,7 @@ int main() // create a random direction: double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = Vector_(3,x,y,z); + Vector v = (Vector(3) << x, y, z); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001)) diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 62072ee18..1934c58ed 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -1,72 +1,72 @@ -/* ---------------------------------------------------------------------------- - -* GTSAM Copyright 2010, Georgia Tech Research Corporation, -* Atlanta, Georgia 30332-0415 -* All Rights Reserved -* Authors: Frank Dellaert, et al. (see THANKS for the full author list) - -* See LICENSE for the license information - -* -------------------------------------------------------------------------- */ - -/** -* @file BayesNet.h -* @brief Bayes network -* @author Frank Dellaert -* @author Richard Roberts -*/ - -#pragma once - -#include - -#include - -namespace gtsam { - - /** - * A BayesNet is a tree of conditionals, stored in elimination order. - * - * todo: how to handle Bayes nets with an optimize function? Currently using global functions. - * \nosubgrouping - */ - template - class BayesNet : public FactorGraph { - - private: - - typedef FactorGraph Base; - - public: - typedef typename boost::shared_ptr sharedConditional; ///< A shared pointer to a conditional - - protected: - /// @name Standard Constructors - /// @{ - - /** Default constructor as an empty BayesNet */ - BayesNet() {}; - - /** Construct from iterator over conditionals */ - template - BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} - - /// @} - - public: - /// @name Testable - /// @{ - - /** print out graph */ - void print(const std::string& s = "BayesNet", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - /// @} - - /// @name Standard Interface - /// @{ - - void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - }; - +/* ---------------------------------------------------------------------------- + +* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* Atlanta, Georgia 30332-0415 +* All Rights Reserved +* Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +* See LICENSE for the license information + +* -------------------------------------------------------------------------- */ + +/** +* @file BayesNet.h +* @brief Bayes network +* @author Frank Dellaert +* @author Richard Roberts +*/ + +#pragma once + +#include + +#include + +namespace gtsam { + + /** + * A BayesNet is a tree of conditionals, stored in elimination order. + * + * todo: how to handle Bayes nets with an optimize function? Currently using global functions. + * \nosubgrouping + */ + template + class BayesNet : public FactorGraph { + + private: + + typedef FactorGraph Base; + + public: + typedef typename boost::shared_ptr sharedConditional; ///< A shared pointer to a conditional + + protected: + /// @name Standard Constructors + /// @{ + + /** Default constructor as an empty BayesNet */ + BayesNet() {}; + + /** Construct from iterator over conditionals */ + template + BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} + + /// @} + + public: + /// @name Testable + /// @{ + + /** print out graph */ + void print(const std::string& s = "BayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Standard Interface + /// @{ + + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + }; + } \ No newline at end of file diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 485f222a2..002bbc518 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -1,285 +1,285 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file BayesTree.h - * @brief Bayes Tree is a tree of cliques of a Bayes Chain - * @author Frank Dellaert - */ - -// \callgraph - -#pragma once - -#include - -#include -#include -#include -#include - -namespace gtsam { - - // Forward declarations - template class FactorGraph; - template class ClusterTree; - - /* ************************************************************************* */ - /** clique statistics */ - struct GTSAM_EXPORT BayesTreeCliqueStats { - double avgConditionalSize; - std::size_t maxConditionalSize; - double avgSeparatorSize; - std::size_t maxSeparatorSize; - void print(const std::string& s = "") const ; - }; - - /** store all the sizes */ - struct GTSAM_EXPORT BayesTreeCliqueData { - FastVector conditionalSizes; - FastVector separatorSizes; - BayesTreeCliqueStats getStats() const; - }; - - /* ************************************************************************* */ - /** - * Bayes tree - * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, - * which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional. - * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this - * as it is only used when developing special versions of BayesTree, e.g. for ISAM2. - * - * \addtogroup Multifrontal - * \nosubgrouping - */ - template - class BayesTree - { - protected: - typedef BayesTree This; - typedef boost::shared_ptr shared_ptr; - - public: - typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique - typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique - typedef Clique Node; ///< Synonym for Clique (TODO: remove) - typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove) - typedef typename CLIQUE::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; - typedef typename CLIQUE::BayesNetType BayesNetType; - typedef boost::shared_ptr sharedBayesNet; - typedef typename CLIQUE::FactorType FactorType; - typedef boost::shared_ptr sharedFactor; - typedef typename CLIQUE::FactorGraphType FactorGraphType; - typedef boost::shared_ptr sharedFactorGraph; - typedef typename FactorGraphType::Eliminate Eliminate; - typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; - - /** A convenience class for a list of shared cliques */ - typedef FastList Cliques; - - /** Map from keys to Clique */ - typedef ConcurrentMap Nodes; - - protected: - - /** Map from indices to Clique */ - Nodes nodes_; - - /** Root cliques */ - FastVector roots_; - - /// @name Standard Constructors - /// @{ - - /** Create an empty Bayes Tree */ - BayesTree() {} - - /** Copy constructor */ - BayesTree(const This& other); - - /// @} - - /** Assignment operator */ - This& operator=(const This& other); - - /// @name Testable - /// @{ - - /** check equality */ - bool equals(const This& other, double tol = 1e-9) const; - - public: - /** print */ - void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /// @} - - /// @name Standard Interface - /// @{ - - /** number of cliques */ - size_t size() const; - - /** Check if there are any cliques in the tree */ - inline bool empty() const { - return nodes_.empty(); - } - - /** return nodes */ - const Nodes& nodes() const { return nodes_; } - - /** Access node by variable */ - const sharedNode operator[](Key j) const { return nodes_.at(j); } - - /** return root cliques */ - const FastVector& roots() const { return roots_; } - - /** alternate syntax for matlab: find the clique that contains the variable with Key j */ - const sharedClique& clique(Key j) const { - typename Nodes::const_iterator c = nodes_.find(j); - if(c == nodes_.end()) - throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree"); - else - return c->second; - } - - /** Gather data on all cliques */ - BayesTreeCliqueData getCliqueData() const; - - /** Collect number of cliques with cached separator marginals */ - size_t numCachedSeparatorMarginals() const; - - /** Return marginal on any variable. Note that this actually returns a conditional, for which a - * solution may be directly obtained by calling .solve() on the returned object. - * Alternatively, it may be directly used as its factor base class. For example, for Gaussian - * systems, this returns a GaussianConditional, which inherits from JacobianFactor and - * GaussianFactor. */ - sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; - - /** - * return joint on two variables - * Limitation: can only calculate joint if cliques are disjoint or one of them is root - */ - sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; - - /** - * return joint on two variables as a BayesNet - * Limitation: can only calculate joint if cliques are disjoint or one of them is root - */ - sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; - - /** - * Read only with side effects - */ - - /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Find parent clique of a conditional. It will look at all parents and - * return the one with the lowest index in the ordering. - */ - template - Key findParentClique(const CONTAINER& parents) const; - - /** Remove all nodes */ - void clear(); - - /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ - void deleteCachedShortcuts(); - - /** - * Remove path from clique to root and return that path as factors - * plus a list of orphaned subtree roots. Used in removeTop below. - */ - void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); - - /** - * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. - * Factors and orphans are added to the in/out arguments. - */ - void removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans); - - /** - * Remove the requested subtree. */ - Cliques removeSubtree(const sharedClique& subtree); - - /** Insert a new subtree with known parent clique. This function does not check that the - * specified parent is the correct parent. This function updates all of the internal data - * structures associated with adding a subtree, such as populating the nodes index. */ - void insertRoot(const sharedClique& subtree); - - /** add a clique (top down) */ - void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); - - /** Add all cliques in this BayesTree to the specified factor graph */ - void addFactorsToGraph(FactorGraph& graph) const; - - protected: - - /** private helper method for saving the Tree to a text file in GraphViz format */ - void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter, - int parentnum = 0) const; - - /** Gather data on a single clique */ - void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const; - - /** remove a clique: warning, can result in a forest */ - void removeClique(sharedClique clique); - - /** Fill the nodes index for a subtree */ - void fillNodesIndex(const sharedClique& subtree); - - // Friend JunctionTree because it directly fills roots and nodes index. - template friend class ClusterTree; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(nodes_); - ar & BOOST_SERIALIZATION_NVP(roots_); - } - - /// @} - - }; // BayesTree - - /* ************************************************************************* */ - template - class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType - { - public: - typedef CLIQUE CliqueType; - typedef typename CLIQUE::ConditionalType Base; - - boost::shared_ptr clique; - - BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : - clique(clique) - { - // Store parent keys in our base type factor so that eliminating those parent keys will pull - // this subtree into the elimination. - this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); - } - - void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const { - clique->print(s + "stored clique", formatter); - } - }; - -} /// namespace gtsam +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BayesTree.h + * @brief Bayes Tree is a tree of cliques of a Bayes Chain + * @author Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include + +#include +#include +#include +#include + +namespace gtsam { + + // Forward declarations + template class FactorGraph; + template class ClusterTree; + + /* ************************************************************************* */ + /** clique statistics */ + struct GTSAM_EXPORT BayesTreeCliqueStats { + double avgConditionalSize; + std::size_t maxConditionalSize; + double avgSeparatorSize; + std::size_t maxSeparatorSize; + void print(const std::string& s = "") const ; + }; + + /** store all the sizes */ + struct GTSAM_EXPORT BayesTreeCliqueData { + FastVector conditionalSizes; + FastVector separatorSizes; + BayesTreeCliqueStats getStats() const; + }; + + /* ************************************************************************* */ + /** + * Bayes tree + * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, + * which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional. + * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this + * as it is only used when developing special versions of BayesTree, e.g. for ISAM2. + * + * \addtogroup Multifrontal + * \nosubgrouping + */ + template + class BayesTree + { + protected: + typedef BayesTree This; + typedef boost::shared_ptr shared_ptr; + + public: + typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique + typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique + typedef Clique Node; ///< Synonym for Clique (TODO: remove) + typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove) + typedef typename CLIQUE::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename CLIQUE::BayesNetType BayesNetType; + typedef boost::shared_ptr sharedBayesNet; + typedef typename CLIQUE::FactorType FactorType; + typedef boost::shared_ptr sharedFactor; + typedef typename CLIQUE::FactorGraphType FactorGraphType; + typedef boost::shared_ptr sharedFactorGraph; + typedef typename FactorGraphType::Eliminate Eliminate; + typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; + + /** A convenience class for a list of shared cliques */ + typedef FastList Cliques; + + /** Map from keys to Clique */ + typedef ConcurrentMap Nodes; + + protected: + + /** Map from indices to Clique */ + Nodes nodes_; + + /** Root cliques */ + FastVector roots_; + + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + BayesTree() {} + + /** Copy constructor */ + BayesTree(const This& other); + + /// @} + + /** Assignment operator */ + This& operator=(const This& other); + + /// @name Testable + /// @{ + + /** check equality */ + bool equals(const This& other, double tol = 1e-9) const; + + public: + /** print */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /// @} + + /// @name Standard Interface + /// @{ + + /** number of cliques */ + size_t size() const; + + /** Check if there are any cliques in the tree */ + inline bool empty() const { + return nodes_.empty(); + } + + /** return nodes */ + const Nodes& nodes() const { return nodes_; } + + /** Access node by variable */ + const sharedNode operator[](Key j) const { return nodes_.at(j); } + + /** return root cliques */ + const FastVector& roots() const { return roots_; } + + /** alternate syntax for matlab: find the clique that contains the variable with Key j */ + const sharedClique& clique(Key j) const { + typename Nodes::const_iterator c = nodes_.find(j); + if(c == nodes_.end()) + throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree"); + else + return c->second; + } + + /** Gather data on all cliques */ + BayesTreeCliqueData getCliqueData() const; + + /** Collect number of cliques with cached separator marginals */ + size_t numCachedSeparatorMarginals() const; + + /** Return marginal on any variable. Note that this actually returns a conditional, for which a + * solution may be directly obtained by calling .solve() on the returned object. + * Alternatively, it may be directly used as its factor base class. For example, for Gaussian + * systems, this returns a GaussianConditional, which inherits from JacobianFactor and + * GaussianFactor. */ + sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** + * return joint on two variables + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ + sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** + * return joint on two variables as a BayesNet + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ + sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; + + /** + * Read only with side effects + */ + + /** saves the Tree to a text file in GraphViz format */ + void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Find parent clique of a conditional. It will look at all parents and + * return the one with the lowest index in the ordering. + */ + template + Key findParentClique(const CONTAINER& parents) const; + + /** Remove all nodes */ + void clear(); + + /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ + void deleteCachedShortcuts(); + + /** + * Remove path from clique to root and return that path as factors + * plus a list of orphaned subtree roots. Used in removeTop below. + */ + void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); + + /** + * Given a list of indices, turn "contaminated" part of the tree back into a factor graph. + * Factors and orphans are added to the in/out arguments. + */ + void removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans); + + /** + * Remove the requested subtree. */ + Cliques removeSubtree(const sharedClique& subtree); + + /** Insert a new subtree with known parent clique. This function does not check that the + * specified parent is the correct parent. This function updates all of the internal data + * structures associated with adding a subtree, such as populating the nodes index. */ + void insertRoot(const sharedClique& subtree); + + /** add a clique (top down) */ + void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); + + /** Add all cliques in this BayesTree to the specified factor graph */ + void addFactorsToGraph(FactorGraph& graph) const; + + protected: + + /** private helper method for saving the Tree to a text file in GraphViz format */ + void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter, + int parentnum = 0) const; + + /** Gather data on a single clique */ + void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const; + + /** remove a clique: warning, can result in a forest */ + void removeClique(sharedClique clique); + + /** Fill the nodes index for a subtree */ + void fillNodesIndex(const sharedClique& subtree); + + // Friend JunctionTree because it directly fills roots and nodes index. + template friend class ClusterTree; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(nodes_); + ar & BOOST_SERIALIZATION_NVP(roots_); + } + + /// @} + + }; // BayesTree + + /* ************************************************************************* */ + template + class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType + { + public: + typedef CLIQUE CliqueType; + typedef typename CLIQUE::ConditionalType Base; + + boost::shared_ptr clique; + + BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : + clique(clique) + { + // Store parent keys in our base type factor so that eliminating those parent keys will pull + // this subtree into the elimination. + this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); + } + + void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const { + clique->print(s + "stored clique", formatter); + } + }; + +} /// namespace gtsam diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 63c52ad5f..beb222178 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -1,173 +1,173 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file BayesTreeCliqueBase.h - * @brief Base class for cliques of a BayesTree - * @author Richard Roberts and Frank Dellaert - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - // Forward declarations - template class BayesTree; - template struct EliminationTraits; - - /** - * This is the base class for BayesTree cliques. The default and standard derived type is - * BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store - * extra data along with the clique. - * - * This class is templated on the derived class (i.e. the curiously recursive template pattern). - * The advantage of this over using virtual classes is that it avoids the need for casting to get - * the derived type. This is possible because all cliques in a BayesTree are the same type - if - * they were not then we'd need a virtual class. - * - * @tparam DERIVED The derived clique type. - * @tparam CONDITIONAL The conditional type. - * \nosubgrouping */ - template - class BayesTreeCliqueBase - { - private: - typedef BayesTreeCliqueBase This; - typedef DERIVED DerivedType; - typedef EliminationTraits EliminationTraitsType; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef boost::shared_ptr derived_ptr; - typedef boost::weak_ptr derived_weak_ptr; - - public: - typedef FACTORGRAPH FactorGraphType; - typedef typename EliminationTraitsType::BayesNetType BayesNetType; - typedef typename BayesNetType::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; - typedef typename FactorGraphType::FactorType FactorType; - typedef typename FactorGraphType::Eliminate Eliminate; - - protected: - - /// @name Standard Constructors - /// @{ - - /** Default constructor */ - BayesTreeCliqueBase() : problemSize_(1) {} - - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} - - /// @} - - /// This stores the Cached separator margnal P(S) - mutable boost::optional cachedSeparatorMarginal_; - - public: - sharedConditional conditional_; - derived_weak_ptr parent_; - FastVector children; - int problemSize_; - - /// Fill the elimination result produced during elimination. Here this just stores the - /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique - /// to also cache the remaining factor. - void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); - - /// @name Testable - /// @{ - - /** check equality */ - bool equals(const DERIVED& other, double tol = 1e-9) const; - - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// @} - /// @name Standard Interface - /// @{ - - /** Access the conditional */ - const sharedConditional& conditional() const { return conditional_; } - - /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { return parent_.expired(); } - - /** The size of subtree rooted at this clique, i.e., nr of Cliques */ - size_t treeSize() const; - - /** Collect number of cliques with cached separator marginals */ - size_t numCachedSeparatorMarginals() const; - - /** return a shared_ptr to the parent clique */ - derived_ptr parent() const { return parent_.lock(); } - - /** Problem size (used for parallel traversal) */ - int problemSize() const { return problemSize_; } - - /// @} - /// @name Advanced Interface - /// @{ - - /** return the conditional P(S|Root) on the separator given the root */ - BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const; - - /** return the marginal P(S) on the separator */ - FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const; - - /** return the marginal P(C) of the clique, using marginal caching */ - FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const; - - /** - * This deletes the cached shortcuts of all cliques (subtree) below this clique. - * This is performed when the bayes tree is modified. - */ - void deleteCachedShortcuts(); - - const boost::optional& cachedSeparatorMarginal() const { - return cachedSeparatorMarginal_; } - - friend class BayesTree; - - protected: - - /// Calculate set \f$ S \setminus B \f$ for shortcut calculations - FastVector separator_setminus_B(const derived_ptr& B) const; - - /** Determine variable indices to keep in recursive separator shortcut calculation The factor - * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables - * not in S union B. */ - FastVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; - - /** Non-recursive delete cached shortcuts and marginals - internal only. */ - void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(conditional_); - ar & BOOST_SERIALIZATION_NVP(parent_); - ar & BOOST_SERIALIZATION_NVP(children); - } - - /// @} - - }; - -} +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BayesTreeCliqueBase.h + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + // Forward declarations + template class BayesTree; + template struct EliminationTraits; + + /** + * This is the base class for BayesTree cliques. The default and standard derived type is + * BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store + * extra data along with the clique. + * + * This class is templated on the derived class (i.e. the curiously recursive template pattern). + * The advantage of this over using virtual classes is that it avoids the need for casting to get + * the derived type. This is possible because all cliques in a BayesTree are the same type - if + * they were not then we'd need a virtual class. + * + * @tparam DERIVED The derived clique type. + * @tparam CONDITIONAL The conditional type. + * \nosubgrouping */ + template + class BayesTreeCliqueBase + { + private: + typedef BayesTreeCliqueBase This; + typedef DERIVED DerivedType; + typedef EliminationTraits EliminationTraitsType; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef boost::shared_ptr derived_ptr; + typedef boost::weak_ptr derived_weak_ptr; + + public: + typedef FACTORGRAPH FactorGraphType; + typedef typename EliminationTraitsType::BayesNetType BayesNetType; + typedef typename BayesNetType::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename FactorGraphType::FactorType FactorType; + typedef typename FactorGraphType::Eliminate Eliminate; + + protected: + + /// @name Standard Constructors + /// @{ + + /** Default constructor */ + BayesTreeCliqueBase() : problemSize_(1) {} + + /** Construct from a conditional, leaving parent and child pointers uninitialized */ + BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} + + /// @} + + /// This stores the Cached separator margnal P(S) + mutable boost::optional cachedSeparatorMarginal_; + + public: + sharedConditional conditional_; + derived_weak_ptr parent_; + FastVector children; + int problemSize_; + + /// Fill the elimination result produced during elimination. Here this just stores the + /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique + /// to also cache the remaining factor. + void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); + + /// @name Testable + /// @{ + + /** check equality */ + bool equals(const DERIVED& other, double tol = 1e-9) const; + + /** print this node */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** Access the conditional */ + const sharedConditional& conditional() const { return conditional_; } + + /** is this the root of a Bayes tree ? */ + inline bool isRoot() const { return parent_.expired(); } + + /** The size of subtree rooted at this clique, i.e., nr of Cliques */ + size_t treeSize() const; + + /** Collect number of cliques with cached separator marginals */ + size_t numCachedSeparatorMarginals() const; + + /** return a shared_ptr to the parent clique */ + derived_ptr parent() const { return parent_.lock(); } + + /** Problem size (used for parallel traversal) */ + int problemSize() const { return problemSize_; } + + /// @} + /// @name Advanced Interface + /// @{ + + /** return the conditional P(S|Root) on the separator given the root */ + BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const; + + /** return the marginal P(S) on the separator */ + FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const; + + /** return the marginal P(C) of the clique, using marginal caching */ + FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const; + + /** + * This deletes the cached shortcuts of all cliques (subtree) below this clique. + * This is performed when the bayes tree is modified. + */ + void deleteCachedShortcuts(); + + const boost::optional& cachedSeparatorMarginal() const { + return cachedSeparatorMarginal_; } + + friend class BayesTree; + + protected: + + /// Calculate set \f$ S \setminus B \f$ for shortcut calculations + FastVector separator_setminus_B(const derived_ptr& B) const; + + /** Determine variable indices to keep in recursive separator shortcut calculation The factor + * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables + * not in S union B. */ + FastVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; + + /** Non-recursive delete cached shortcuts and marginals - internal only. */ + void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(conditional_); + ar & BOOST_SERIALIZATION_NVP(parent_); + ar & BOOST_SERIALIZATION_NVP(children); + } + + /// @} + + }; + +} diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 551fdec0c..bdfec9cd5 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -1,152 +1,152 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Conditional.h - * @brief Base class for conditional densities - * @author Frank Dellaert - */ - -// \callgraph -#pragma once - -#include - -#include - -namespace gtsam { - - /** - * TODO: Update comments. The following comments are out of date!!! - * - * Base class for conditional densities, templated on KEY type. This class - * provides storage for the keys involved in a conditional, and iterators and - * access to the frontal and separator keys. - * - * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer - * to the associated factor type and shared_ptr type of the derived class. See - * IndexConditional and GaussianConditional for examples. - * \nosubgrouping - */ - template - class Conditional - { - protected: - /** The first nrFrontal variables are frontal and the rest are parents. */ - size_t nrFrontals_; - - private: - /// Typedef to this class - typedef Conditional This; - - public: - /** View of the frontal keys (call frontals()) */ - typedef boost::iterator_range Frontals; - - /** View of the separator keys (call parents()) */ - typedef boost::iterator_range Parents; - - protected: - /// @name Standard Constructors - /// @{ - - /** Empty Constructor to make serialization possible */ - Conditional() : nrFrontals_(0) {} - - /** Constructor */ - Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {} - - /// @} - /// @name Testable - /// @{ - - /** print with optional formatter */ - void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; - - /** check equality */ - bool equals(const This& c, double tol = 1e-9) const; - - /// @} - - public: - /// @name Standard Interface - /// @{ - - /** return the number of frontals */ - size_t nrFrontals() const { return nrFrontals_; } - - /** return the number of parents */ - size_t nrParents() const { return asFactor().size() - nrFrontals_; } - - /** Convenience function to get the first frontal key */ - Key firstFrontalKey() const { - if(nrFrontals_ > 0) - return asFactor().front(); - else - throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys"); - } - - /** return a view of the frontal keys */ - Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); } - - /** return a view of the parent keys */ - Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); } - - /** Iterator pointing to first frontal key. */ - typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); } - - /** Iterator pointing past the last frontal key. */ - typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; } - - /** Iterator pointing to the first parent key. */ - typename FACTOR::const_iterator beginParents() const { return endFrontals(); } - - /** Iterator pointing past the last parent key. */ - typename FACTOR::const_iterator endParents() const { return asFactor().end(); } - - /// @} - /// @name Advanced Interface - /// @{ - - /** Mutable version of nrFrontals */ - size_t& nrFrontals() { return nrFrontals_; } - - /** Mutable iterator pointing to first frontal key. */ - typename FACTOR::iterator beginFrontals() { return asFactor().begin(); } - - /** Mutable iterator pointing past the last frontal key. */ - typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; } - - /** Mutable iterator pointing to the first parent key. */ - typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; } - - /** Mutable iterator pointing past the last parent key. */ - typename FACTOR::iterator endParents() { return asFactor().end(); } - - private: - // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type) - FACTOR& asFactor() { return static_cast(static_cast(*this)); } - - // Cast to derived type (const) (casts down to derived conditional type, then up to factor type) - const FACTOR& asFactor() const { return static_cast(static_cast(*this)); } - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(nrFrontals_); - } - - /// @} - - }; - -} // gtsam +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Conditional.h + * @brief Base class for conditional densities + * @author Frank Dellaert + */ + +// \callgraph +#pragma once + +#include + +#include + +namespace gtsam { + + /** + * TODO: Update comments. The following comments are out of date!!! + * + * Base class for conditional densities, templated on KEY type. This class + * provides storage for the keys involved in a conditional, and iterators and + * access to the frontal and separator keys. + * + * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer + * to the associated factor type and shared_ptr type of the derived class. See + * IndexConditional and GaussianConditional for examples. + * \nosubgrouping + */ + template + class Conditional + { + protected: + /** The first nrFrontal variables are frontal and the rest are parents. */ + size_t nrFrontals_; + + private: + /// Typedef to this class + typedef Conditional This; + + public: + /** View of the frontal keys (call frontals()) */ + typedef boost::iterator_range Frontals; + + /** View of the separator keys (call parents()) */ + typedef boost::iterator_range Parents; + + protected: + /// @name Standard Constructors + /// @{ + + /** Empty Constructor to make serialization possible */ + Conditional() : nrFrontals_(0) {} + + /** Constructor */ + Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {} + + /// @} + /// @name Testable + /// @{ + + /** print with optional formatter */ + void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** check equality */ + bool equals(const This& c, double tol = 1e-9) const; + + /// @} + + public: + /// @name Standard Interface + /// @{ + + /** return the number of frontals */ + size_t nrFrontals() const { return nrFrontals_; } + + /** return the number of parents */ + size_t nrParents() const { return asFactor().size() - nrFrontals_; } + + /** Convenience function to get the first frontal key */ + Key firstFrontalKey() const { + if(nrFrontals_ > 0) + return asFactor().front(); + else + throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys"); + } + + /** return a view of the frontal keys */ + Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); } + + /** return a view of the parent keys */ + Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); } + + /** Iterator pointing to first frontal key. */ + typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); } + + /** Iterator pointing past the last frontal key. */ + typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; } + + /** Iterator pointing to the first parent key. */ + typename FACTOR::const_iterator beginParents() const { return endFrontals(); } + + /** Iterator pointing past the last parent key. */ + typename FACTOR::const_iterator endParents() const { return asFactor().end(); } + + /// @} + /// @name Advanced Interface + /// @{ + + /** Mutable version of nrFrontals */ + size_t& nrFrontals() { return nrFrontals_; } + + /** Mutable iterator pointing to first frontal key. */ + typename FACTOR::iterator beginFrontals() { return asFactor().begin(); } + + /** Mutable iterator pointing past the last frontal key. */ + typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; } + + /** Mutable iterator pointing to the first parent key. */ + typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; } + + /** Mutable iterator pointing past the last parent key. */ + typename FACTOR::iterator endParents() { return asFactor().end(); } + + private: + // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type) + FACTOR& asFactor() { return static_cast(static_cast(*this)); } + + // Cast to derived type (const) (casts down to derived conditional type, then up to factor type) + const FACTOR& asFactor() const { return static_cast(static_cast(*this)); } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(nrFrontals_); + } + + /// @} + + }; + +} // gtsam diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index b0f5acca3..fcea1284e 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -1,168 +1,168 @@ -/* ---------------------------------------------------------------------------- - -* GTSAM Copyright 2010, Georgia Tech Research Corporation, -* Atlanta, Georgia 30332-0415 -* All Rights Reserved -* Authors: Frank Dellaert, et al. (see THANKS for the full author list) - -* See LICENSE for the license information - -* -------------------------------------------------------------------------- */ - -/** -* @file EliminationTree.h -* @author Frank Dellaert -* @author Richard Roberts -* @date Oct 13, 2010 -*/ -#pragma once - -#include -#include - -#include -#include - -class EliminationTreeTester; // for unit tests, see testEliminationTree - -namespace gtsam { - - class VariableIndex; - class Ordering; - - /** - * An elimination tree is a data structure used intermediately during - * elimination. In future versions it will be used to save work between - * multiple eliminations. - * - * When a variable is eliminated, a new factor is created by combining that - * variable's neighboring factors. The new combined factor involves the combined - * factors' involved variables. When the lowest-ordered one of those variables - * is eliminated, it consumes that combined factor. In the elimination tree, - * that lowest-ordered variable is the parent of the variable that was eliminated to - * produce the combined factor. This yields a tree in general, and not a chain - * because of the implicit sparse structure of the resulting Bayes net. - * - * This structure is examined even more closely in a JunctionTree, which - * additionally identifies cliques in the chordal Bayes net. - * \nosubgrouping - */ - template - class EliminationTree - { - protected: - typedef EliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - - public: - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR - typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals - typedef typename boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef typename GRAPH::Eliminate Eliminate; - - struct Node { - typedef FastVector Factors; - typedef FastVector > Children; - - Key key; ///< key associated with root - Factors factors; ///< factors associated with root - Children children; ///< sub-trees - - sharedFactor eliminate(const boost::shared_ptr& output, - const Eliminate& function, const FastVector& childrenFactors) const; - - void print(const std::string& str, const KeyFormatter& keyFormatter) const; - }; - - typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node - - protected: - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - - FastVector roots_; - FastVector remainingFactors_; - - /// @name Standard Constructors - /// @{ - - /** - * Build the elimination tree of a factor graph using pre-computed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ - EliminationTree(const FactorGraphType& factorGraph, - const VariableIndex& structure, const Ordering& order); - - /** Build the elimination tree of a factor graph. Note that this has to compute the column - * structure as a VariableIndex, so if you already have this precomputed, use the other - * constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ - EliminationTree(const FactorGraphType& factorGraph, const Ordering& order); - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - EliminationTree(const This& other) { *this = other; } - - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors - * are copied, factors are not cloned. */ - This& operator=(const This& other); - - /// @} - - public: - /// @name Standard Interface - /// @{ - - /** Eliminate the factors to a Bayes net and remaining factor graph - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The Bayes net and factor graph resulting from elimination - */ - std::pair, boost::shared_ptr > - eliminate(Eliminate function) const; - - /// @} - /// @name Testable - /// @{ - - /** Print the tree to cout */ - void print(const std::string& name = "EliminationTree: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - protected: - /** Test whether the tree is equal to another */ - bool equals(const This& other, double tol = 1e-9) const; - - /// @} - - public: - /// @name Advanced Interface - /// @{ - - /** Return the set of roots (one for a tree, multiple for a forest) */ - const FastVector& roots() const { return roots_; } - - /** Return the remaining factors that are not pulled into elimination */ - const FastVector& remainingFactors() const { return remainingFactors_; } - - /** Swap the data of this tree with another one, this operation is very fast. */ - void swap(This& other); - - protected: - /// Protected default constructor - EliminationTree() {} - - private: - /// Allow access to constructor and add methods for testing purposes - friend class ::EliminationTreeTester; - }; - -} +/* ---------------------------------------------------------------------------- + +* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* Atlanta, Georgia 30332-0415 +* All Rights Reserved +* Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +* See LICENSE for the license information + +* -------------------------------------------------------------------------- */ + +/** +* @file EliminationTree.h +* @author Frank Dellaert +* @author Richard Roberts +* @date Oct 13, 2010 +*/ +#pragma once + +#include +#include + +#include +#include + +class EliminationTreeTester; // for unit tests, see testEliminationTree + +namespace gtsam { + + class VariableIndex; + class Ordering; + + /** + * An elimination tree is a data structure used intermediately during + * elimination. In future versions it will be used to save work between + * multiple eliminations. + * + * When a variable is eliminated, a new factor is created by combining that + * variable's neighboring factors. The new combined factor involves the combined + * factors' involved variables. When the lowest-ordered one of those variables + * is eliminated, it consumes that combined factor. In the elimination tree, + * that lowest-ordered variable is the parent of the variable that was eliminated to + * produce the combined factor. This yields a tree in general, and not a chain + * because of the implicit sparse structure of the resulting Bayes net. + * + * This structure is examined even more closely in a JunctionTree, which + * additionally identifies cliques in the chordal Bayes net. + * \nosubgrouping + */ + template + class EliminationTree + { + protected: + typedef EliminationTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + + public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR + typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals + typedef typename boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename GRAPH::Eliminate Eliminate; + + struct Node { + typedef FastVector Factors; + typedef FastVector > Children; + + Key key; ///< key associated with root + Factors factors; ///< factors associated with root + Children children; ///< sub-trees + + sharedFactor eliminate(const boost::shared_ptr& output, + const Eliminate& function, const FastVector& childrenFactors) const; + + void print(const std::string& str, const KeyFormatter& keyFormatter) const; + }; + + typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node + + protected: + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + + FastVector roots_; + FastVector remainingFactors_; + + /// @name Standard Constructors + /// @{ + + /** + * Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not + * precomputed, you can call the Create(const FactorGraph&) + * named constructor instead. + * @return The elimination tree + */ + EliminationTree(const FactorGraphType& factorGraph, + const VariableIndex& structure, const Ordering& order); + + /** Build the elimination tree of a factor graph. Note that this has to compute the column + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree + */ + EliminationTree(const FactorGraphType& factorGraph, const Ordering& order); + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + EliminationTree(const This& other) { *this = other; } + + /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + * are copied, factors are not cloned. */ + This& operator=(const This& other); + + /// @} + + public: + /// @name Standard Interface + /// @{ + + /** Eliminate the factors to a Bayes net and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes net and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(Eliminate function) const; + + /// @} + /// @name Testable + /// @{ + + /** Print the tree to cout */ + void print(const std::string& name = "EliminationTree: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + protected: + /** Test whether the tree is equal to another */ + bool equals(const This& other, double tol = 1e-9) const; + + /// @} + + public: + /// @name Advanced Interface + /// @{ + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const { return roots_; } + + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const { return remainingFactors_; } + + /** Swap the data of this tree with another one, this operation is very fast. */ + void swap(This& other); + + protected: + /// Protected default constructor + EliminationTree() {} + + private: + /// Allow access to constructor and add methods for testing purposes + friend class ::EliminationTreeTester; + }; + +} diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 8ce667664..e67c0e248 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -1,58 +1,58 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VariableIndex.cpp - * @author Richard Roberts - * @date March 26, 2013 - */ - -#include - -#include - -namespace gtsam { - -using namespace std; - -/* ************************************************************************* */ -bool VariableIndex::equals(const VariableIndex& other, double tol) const { - return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ - && this->index_ == other.index_; -} - -/* ************************************************************************* */ -void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const { - cout << str; - cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; - BOOST_FOREACH(KeyMap::value_type key_factors, index_) { - cout << "var " << keyFormatter(key_factors.first) << ":"; - BOOST_FOREACH(const size_t factor, key_factors.second) - cout << " " << factor; - cout << "\n"; - } - cout.flush(); -} - -/* ************************************************************************* */ -void VariableIndex::outputMetisFormat(ostream& os) const { - os << size() << " " << nFactors() << "\n"; - // run over variables, which will be hyper-edges. - BOOST_FOREACH(KeyMap::value_type key_factors, index_) { - // every variable is a hyper-edge covering its factors - BOOST_FOREACH(const size_t factor, key_factors.second) - os << (factor+1) << " "; // base 1 - os << "\n"; - } - os << flush; -} - -} +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VariableIndex.cpp + * @author Richard Roberts + * @date March 26, 2013 + */ + +#include + +#include + +namespace gtsam { + +using namespace std; + +/* ************************************************************************* */ +bool VariableIndex::equals(const VariableIndex& other, double tol) const { + return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ + && this->index_ == other.index_; +} + +/* ************************************************************************* */ +void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const { + cout << str; + cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; + BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + cout << "var " << keyFormatter(key_factors.first) << ":"; + BOOST_FOREACH(const size_t factor, key_factors.second) + cout << " " << factor; + cout << "\n"; + } + cout.flush(); +} + +/* ************************************************************************* */ +void VariableIndex::outputMetisFormat(ostream& os) const { + os << size() << " " << nFactors() << "\n"; + // run over variables, which will be hyper-edges. + BOOST_FOREACH(KeyMap::value_type key_factors, index_) { + // every variable is a hyper-edge covering its factors + BOOST_FOREACH(const size_t factor, key_factors.second) + os << (factor+1) << " "; // base 1 + os << "\n"; + } + os << flush; +} + +} diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 4c89e18eb..3985221d3 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -1,180 +1,180 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VariableIndex.h - * @author Richard Roberts - * @date March 26, 2013 - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace gtsam { - -/** - * The VariableIndex class computes and stores the block column structure of a - * factor graph. The factor graph stores a collection of factors, each of - * which involves a set of variables. In contrast, the VariableIndex is built - * from a factor graph prior to elimination, and stores the list of factors - * that involve each variable. This information is stored as a deque of - * lists of factor indices. - * \nosubgrouping - */ -class GTSAM_EXPORT VariableIndex { -public: - - typedef boost::shared_ptr shared_ptr; - typedef FastList Factors; - typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; - -protected: - typedef FastMap KeyMap; - KeyMap index_; - size_t nFactors_; // Number of factors in the original factor graph. - size_t nEntries_; // Sum of involved variable counts of each factor. - -public: - typedef KeyMap::const_iterator const_iterator; - typedef KeyMap::const_iterator iterator; - typedef KeyMap::value_type value_type; - -public: - - /// @name Standard Constructors - /// @{ - - /** Default constructor, creates an empty VariableIndex */ - VariableIndex() : nFactors_(0), nEntries_(0) {} - - /** - * Create a VariableIndex that computes and stores the block column structure - * of a factor graph. - */ - template - VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } - - /// @} - /// @name Standard Interface - /// @{ - - /** - * The number of variable entries. This is one greater than the variable - * with the highest index. - */ - Key size() const { return index_.size(); } - - /** The number of factors in the original factor graph */ - size_t nFactors() const { return nFactors_; } - - /** The number of nonzero blocks, i.e. the number of variable-factor entries */ - size_t nEntries() const { return nEntries_; } - - /** Access a list of factors by variable */ - const Factors& operator[](Key variable) const { - KeyMap::const_iterator item = index_.find(variable); - if(item == index_.end()) - throw std::invalid_argument("Requested non-existent variable from VariableIndex"); - else - return item->second; - } - - /// @} - /// @name Testable - /// @{ - - /** Test for equality (for unit tests and debug assertions). */ - bool equals(const VariableIndex& other, double tol=0.0) const; - - /** Print the variable index (for unit tests and debugging). */ - void print(const std::string& str = "VariableIndex: ", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /** - * Output dual hypergraph to Metis file format for use with hmetis - * In the dual graph, variables are hyperedges, factors are nodes. - */ - void outputMetisFormat(std::ostream& os) const; - - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Augment the variable index with new factors. This can be used when - * solving problems incrementally. - */ - template - void augment(const FG& factors, boost::optional&> newFactorIndices = boost::none); - - /** - * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement - * nFactors_ because the factor indices need to remain consistent. Removing factors from a factor - * graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than - * the highest-numbered factor referenced in a VariableIndex. - * - * @param indices The indices of the factors to remove, which must match \c factors - * @param factors The factors being removed, which must symbolically correspond exactly to the - * factors with the specified \c indices that were added. - */ - template - void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); - - /** Remove unused empty variables (in debug mode verifies they are empty). */ - template - void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); - - /** Iterator to the first variable entry */ - const_iterator begin() const { return index_.begin(); } - - /** Iterator to the first variable entry */ - const_iterator end() const { return index_.end(); } - - /** Find the iterator for the requested variable entry */ - const_iterator find(Key key) const { return index_.find(key); } - -protected: - Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); } - Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); } - - Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); } - Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } - - /// Internal version of 'at' that asserts existence - const Factors& internalAt(Key variable) const { - const KeyMap::const_iterator item = index_.find(variable); - assert(item != index_.end()); - return item->second; } - - /// Internal version of 'at' that asserts existence - Factors& internalAt(Key variable) { - const KeyMap::iterator item = index_.find(variable); - assert(item != index_.end()); - return item->second; } - - /// @} -}; - -} - -#include +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VariableIndex.h + * @author Richard Roberts + * @date March 26, 2013 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * The VariableIndex class computes and stores the block column structure of a + * factor graph. The factor graph stores a collection of factors, each of + * which involves a set of variables. In contrast, the VariableIndex is built + * from a factor graph prior to elimination, and stores the list of factors + * that involve each variable. This information is stored as a deque of + * lists of factor indices. + * \nosubgrouping + */ +class GTSAM_EXPORT VariableIndex { +public: + + typedef boost::shared_ptr shared_ptr; + typedef FastList Factors; + typedef Factors::iterator Factor_iterator; + typedef Factors::const_iterator Factor_const_iterator; + +protected: + typedef FastMap KeyMap; + KeyMap index_; + size_t nFactors_; // Number of factors in the original factor graph. + size_t nEntries_; // Sum of involved variable counts of each factor. + +public: + typedef KeyMap::const_iterator const_iterator; + typedef KeyMap::const_iterator iterator; + typedef KeyMap::value_type value_type; + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor, creates an empty VariableIndex */ + VariableIndex() : nFactors_(0), nEntries_(0) {} + + /** + * Create a VariableIndex that computes and stores the block column structure + * of a factor graph. + */ + template + VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } + + /// @} + /// @name Standard Interface + /// @{ + + /** + * The number of variable entries. This is one greater than the variable + * with the highest index. + */ + Key size() const { return index_.size(); } + + /** The number of factors in the original factor graph */ + size_t nFactors() const { return nFactors_; } + + /** The number of nonzero blocks, i.e. the number of variable-factor entries */ + size_t nEntries() const { return nEntries_; } + + /** Access a list of factors by variable */ + const Factors& operator[](Key variable) const { + KeyMap::const_iterator item = index_.find(variable); + if(item == index_.end()) + throw std::invalid_argument("Requested non-existent variable from VariableIndex"); + else + return item->second; + } + + /// @} + /// @name Testable + /// @{ + + /** Test for equality (for unit tests and debug assertions). */ + bool equals(const VariableIndex& other, double tol=0.0) const; + + /** Print the variable index (for unit tests and debugging). */ + void print(const std::string& str = "VariableIndex: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** + * Output dual hypergraph to Metis file format for use with hmetis + * In the dual graph, variables are hyperedges, factors are nodes. + */ + void outputMetisFormat(std::ostream& os) const; + + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FG& factors, boost::optional&> newFactorIndices = boost::none); + + /** + * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement + * nFactors_ because the factor indices need to remain consistent. Removing factors from a factor + * graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than + * the highest-numbered factor referenced in a VariableIndex. + * + * @param indices The indices of the factors to remove, which must match \c factors + * @param factors The factors being removed, which must symbolically correspond exactly to the + * factors with the specified \c indices that were added. + */ + template + void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); + + /** Remove unused empty variables (in debug mode verifies they are empty). */ + template + void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); + + /** Iterator to the first variable entry */ + const_iterator begin() const { return index_.begin(); } + + /** Iterator to the first variable entry */ + const_iterator end() const { return index_.end(); } + + /** Find the iterator for the requested variable entry */ + const_iterator find(Key key) const { return index_.find(key); } + +protected: + Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); } + Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); } + + Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); } + Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } + + /// Internal version of 'at' that asserts existence + const Factors& internalAt(Key variable) const { + const KeyMap::const_iterator item = index_.find(variable); + assert(item != index_.end()); + return item->second; } + + /// Internal version of 'at' that asserts existence + Factors& internalAt(Key variable) { + const KeyMap::iterator item = index_.find(variable); + assert(item != index_.end()); + return item->second; } + + /// @} +}; + +} + +#include diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index e05cbc60d..3e35d5e65 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma) { - return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma); + return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 79449204e..90e39867b 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -1,328 +1,328 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file JacobianFactor.h - * @author Richard Roberts - * @author Christian Potthast - * @author Frank Dellaert - * @date Dec 8, 2010 - */ -#pragma once - -#include -#include -#include -#include - -#include - -namespace gtsam { - - // Forward declarations - class HessianFactor; - class VariableSlots; - class GaussianFactorGraph; - class GaussianConditional; - class HessianFactor; - class VectorValues; - class Ordering; - class JacobianFactor; - - GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); - - /** - * A Gaussian factor in the squared-error form. - * - * JacobianFactor implements a - * Gaussian, which has quadratic negative log-likelihood - * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] - * where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The - * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model - * \f$ \Sigma \f$ are stored in this class. - * - * This factor represents the sum-of-squares error of a \a linear - * measurement function, and is created upon linearization of a NoiseModelFactor, - * which in turn is a sum-of-squares factor with a nonlinear measurement function. - * - * Here is an example of how this factor represents a sum-of-squares error: - * - * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be - * the actual observed measurement, the residual is - * \f[ f(x) = h(x) - z . \f] - * If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this - * measurement, then the negative log-likelihood of the Gaussian induced by this - * measurement model is - * \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] - * Because \f$ h(x) \f$ is linear, we can write it as - * \f[ h(x) = Ax + e \f] - * and thus we have - * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] - * where \f$ b = z - e \f$. - * - * This factor can involve an arbitrary number of variables, and in the - * above example \f$ x \f$ would almost always be only be a subset of the variables - * in the entire factor graph. There are special constructors for 1-, 2-, and 3- - * way JacobianFactors, and additional constructors for creating n-way JacobianFactors. - * The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, - * for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$, - * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$ - * and the negative log-likelihood represented by this factor would be - * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] - */ - class GTSAM_EXPORT JacobianFactor : public GaussianFactor - { - public: - typedef JacobianFactor This; ///< Typedef to this class - typedef GaussianFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - - protected: - VerticalBlockMatrix Ab_; // the block view of the full matrix - noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix - - public: - typedef VerticalBlockMatrix::Block ABlock; - typedef VerticalBlockMatrix::constBlock constABlock; - typedef ABlock::ColXpr BVector; - typedef constABlock::ConstColXpr constBVector; - - /** Convert from other GaussianFactor */ - explicit JacobianFactor(const GaussianFactor& gf); - - /** Copy constructor */ - JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {} - - /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ - explicit JacobianFactor(const HessianFactor& hf); - - /** default constructor for I/O */ - JacobianFactor(); - - /** Construct Null factor */ - explicit JacobianFactor(const Vector& b_in); - - /** Construct unary factor */ - JacobianFactor(Key i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct binary factor */ - JacobianFactor(Key i1, const Matrix& A1, - Key i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct ternary factor */ - JacobianFactor(Key i1, const Matrix& A1, Key i2, - const Matrix& A2, Key i3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Construct an n-ary factor - * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ - template - JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); - - /** Constructor with arbitrary number keys, and where the augmented matrix is given all together - * instead of in block terms. Note that only the active view of the provided augmented matrix - * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ - template - JacobianFactor( - const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); - - /** - * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots - * structure computed for \c graph is already available, providing it will reduce the amount of - * computation performed. */ - explicit JacobianFactor( - const GaussianFactorGraph& graph, - boost::optional ordering = boost::none, - boost::optional variableSlots = boost::none); - - /** Virtual destructor */ - virtual ~JacobianFactor() {} - - /** Clone this JacobianFactor */ - virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); - } - - // Implementing Testable interface - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - - Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ - Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ - virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ - - /** Return the augmented information matrix represented by this GaussianFactor. - * The augmented information matrix contains the information matrix with an - * additional column holding the information vector, and an additional row - * holding the transpose of the information vector. The lower-right entry - * contains the constant error term (when \f$ \delta x = 0 \f$). The - * augmented information matrix is described in more detail in HessianFactor, - * which in fact stores an augmented information matrix. - */ - virtual Matrix augmentedInformation() const; - - /** Return the non-augmented information matrix represented by this - * GaussianFactor. - */ - virtual Matrix information() const; - - /** - * @brief Returns (dense) A,b pair associated with factor, bakes in the weights - */ - virtual std::pair jacobian() const; - - /** - * @brief Returns (dense) A,b pair associated with factor, does not bake in weights - */ - std::pair jacobianUnweighted() const; - - /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: - * [A b] - * weights are baked in */ - virtual Matrix augmentedJacobian() const; - - /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: - * [A b] - * weights are not baked in */ - Matrix augmentedJacobianUnweighted() const; - - /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ - const VerticalBlockMatrix& matrixObject() const { return Ab_; } - - /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ - VerticalBlockMatrix& matrixObject() { return Ab_; } - - /** - * Construct the corresponding anti-factor to negate information - * stored stored in this factor. - * @return a HessianFactor with negated Hessian matrices - */ - virtual GaussianFactor::shared_ptr negate() const; - - /** Check if the factor is empty. TODO: How should this be defined? */ - virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } - - /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained(); } - - /** Return the dimension of the variable pointed to by the given key iterator - * todo: Remove this in favor of keeping track of dimensions with variables? - */ - virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } - - /** - * return the number of rows in the corresponding linear system - */ - size_t rows() const { return Ab_.rows(); } - - /** - * return the number of columns in the corresponding linear system - */ - size_t cols() const { return Ab_.cols(); } - - /** get a copy of model */ - const SharedDiagonal& get_model() const { return model_; } - - /** get a copy of model (non-const version) */ - SharedDiagonal& get_model() { return model_; } - - /** Get a view of the r.h.s. vector b, not weighted by noise */ - const constBVector getb() const { return Ab_(size()).col(0); } - - /** Get a view of the A matrix for the variable pointed to by the given key iterator */ - constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } - - /** Get a view of the A matrix, not weighted by noise */ - constABlock getA() const { return Ab_.range(0, size()); } - - /** Get a view of the r.h.s. vector b (non-const version) */ - BVector getb() { return Ab_(size()).col(0); } - - /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ - ABlock getA(iterator variable) { return Ab_(variable - begin()); } - - /** Get a view of the A matrix */ - ABlock getA() { return Ab_.range(0, size()); } - - /** Return A*x */ - Vector operator*(const VectorValues& x) const; - - /** x += A'*e. If x is initially missing any values, they are created and assumed to start as - * zero vectors. */ - void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; - - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - - /// A'*b for Jacobian - VectorValues gradientAtZero() const; - - /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ - JacobianFactor whiten() const; - - /** Eliminate the requested variables. */ - std::pair, boost::shared_ptr > - eliminate(const Ordering& keys); - - /** set noiseModel correctly */ - void setModel(bool anyConstrained, const Vector& sigmas); - - /** - * Densely partially eliminate with QR factorization, this is usually provided as an argument to - * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors - * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the - * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the - * order specified in \c keys. - * @param factors Factors to combine and eliminate - * @param keys The variables to eliminate in the order as specified here in \c keys - * @return The conditional and remaining factor - * - * \addtogroup LinearSolving */ - friend GTSAM_EXPORT std::pair, boost::shared_ptr > - EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); - - /** - * splits a pre-factorized factor into a conditional, and changes the current - * factor to be the remaining component. Performs same operation as eliminate(), - * but without running QR. - */ - boost::shared_ptr splitConditional(size_t nrFrontals); - - protected: - - /// Internal function to fill blocks and set dimensions - template - void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(Ab_); - ar & BOOST_SERIALIZATION_NVP(model_); - } - }; // JacobianFactor - -} // gtsam - -#include - - +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianFactor.h + * @author Richard Roberts + * @author Christian Potthast + * @author Frank Dellaert + * @date Dec 8, 2010 + */ +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + + // Forward declarations + class HessianFactor; + class VariableSlots; + class GaussianFactorGraph; + class GaussianConditional; + class HessianFactor; + class VectorValues; + class Ordering; + class JacobianFactor; + + GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); + + /** + * A Gaussian factor in the squared-error form. + * + * JacobianFactor implements a + * Gaussian, which has quadratic negative log-likelihood + * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] + * where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The + * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model + * \f$ \Sigma \f$ are stored in this class. + * + * This factor represents the sum-of-squares error of a \a linear + * measurement function, and is created upon linearization of a NoiseModelFactor, + * which in turn is a sum-of-squares factor with a nonlinear measurement function. + * + * Here is an example of how this factor represents a sum-of-squares error: + * + * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be + * the actual observed measurement, the residual is + * \f[ f(x) = h(x) - z . \f] + * If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this + * measurement, then the negative log-likelihood of the Gaussian induced by this + * measurement model is + * \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] + * Because \f$ h(x) \f$ is linear, we can write it as + * \f[ h(x) = Ax + e \f] + * and thus we have + * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] + * where \f$ b = z - e \f$. + * + * This factor can involve an arbitrary number of variables, and in the + * above example \f$ x \f$ would almost always be only be a subset of the variables + * in the entire factor graph. There are special constructors for 1-, 2-, and 3- + * way JacobianFactors, and additional constructors for creating n-way JacobianFactors. + * The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, + * for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$, + * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$ + * and the negative log-likelihood represented by this factor would be + * \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f] + */ + class GTSAM_EXPORT JacobianFactor : public GaussianFactor + { + public: + typedef JacobianFactor This; ///< Typedef to this class + typedef GaussianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + + protected: + VerticalBlockMatrix Ab_; // the block view of the full matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix + + public: + typedef VerticalBlockMatrix::Block ABlock; + typedef VerticalBlockMatrix::constBlock constABlock; + typedef ABlock::ColXpr BVector; + typedef constABlock::ConstColXpr constBVector; + + /** Convert from other GaussianFactor */ + explicit JacobianFactor(const GaussianFactor& gf); + + /** Copy constructor */ + JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {} + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit JacobianFactor(const HessianFactor& hf); + + /** default constructor for I/O */ + JacobianFactor(); + + /** Construct Null factor */ + explicit JacobianFactor(const Vector& b_in); + + /** Construct unary factor */ + JacobianFactor(Key i1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + + /** Construct binary factor */ + JacobianFactor(Key i1, const Matrix& A1, + Key i2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + + /** Construct ternary factor */ + JacobianFactor(Key i1, const Matrix& A1, Key i2, + const Matrix& A2, Key i3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); + + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + JacobianFactor( + const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); + + /** + * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots + * structure computed for \c graph is already available, providing it will reduce the amount of + * computation performed. */ + explicit JacobianFactor( + const GaussianFactorGraph& graph, + boost::optional ordering = boost::none, + boost::optional variableSlots = boost::none); + + /** Virtual destructor */ + virtual ~JacobianFactor() {} + + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + // Implementing Testable interface + virtual void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; + + Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ + Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ + virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ + + /** Return the augmented information matrix represented by this GaussianFactor. + * The augmented information matrix contains the information matrix with an + * additional column holding the information vector, and an additional row + * holding the transpose of the information vector. The lower-right entry + * contains the constant error term (when \f$ \delta x = 0 \f$). The + * augmented information matrix is described in more detail in HessianFactor, + * which in fact stores an augmented information matrix. + */ + virtual Matrix augmentedInformation() const; + + /** Return the non-augmented information matrix represented by this + * GaussianFactor. + */ + virtual Matrix information() const; + + /** + * @brief Returns (dense) A,b pair associated with factor, bakes in the weights + */ + virtual std::pair jacobian() const; + + /** + * @brief Returns (dense) A,b pair associated with factor, does not bake in weights + */ + std::pair jacobianUnweighted() const; + + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * weights are baked in */ + virtual Matrix augmentedJacobian() const; + + /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: + * [A b] + * weights are not baked in */ + Matrix augmentedJacobianUnweighted() const; + + /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + const VerticalBlockMatrix& matrixObject() const { return Ab_; } + + /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ + VerticalBlockMatrix& matrixObject() { return Ab_; } + + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + + /** Check if the factor is empty. TODO: How should this be defined? */ + virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } + + /** is noise model constrained ? */ + bool isConstrained() const { return model_->isConstrained(); } + + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + */ + virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } + + /** + * return the number of rows in the corresponding linear system + */ + size_t rows() const { return Ab_.rows(); } + + /** + * return the number of columns in the corresponding linear system + */ + size_t cols() const { return Ab_.cols(); } + + /** get a copy of model */ + const SharedDiagonal& get_model() const { return model_; } + + /** get a copy of model (non-const version) */ + SharedDiagonal& get_model() { return model_; } + + /** Get a view of the r.h.s. vector b, not weighted by noise */ + const constBVector getb() const { return Ab_(size()).col(0); } + + /** Get a view of the A matrix for the variable pointed to by the given key iterator */ + constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } + + /** Get a view of the A matrix, not weighted by noise */ + constABlock getA() const { return Ab_.range(0, size()); } + + /** Get a view of the r.h.s. vector b (non-const version) */ + BVector getb() { return Ab_(size()).col(0); } + + /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */ + ABlock getA(iterator variable) { return Ab_(variable - begin()); } + + /** Get a view of the A matrix */ + ABlock getA() { return Ab_.range(0, size()); } + + /** Return A*x */ + Vector operator*(const VectorValues& x) const; + + /** x += A'*e. If x is initially missing any values, they are created and assumed to start as + * zero vectors. */ + void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; + + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + + /// A'*b for Jacobian + VectorValues gradientAtZero() const; + + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ + JacobianFactor whiten() const; + + /** Eliminate the requested variables. */ + std::pair, boost::shared_ptr > + eliminate(const Ordering& keys); + + /** set noiseModel correctly */ + void setModel(bool anyConstrained, const Vector& sigmas); + + /** + * Densely partially eliminate with QR factorization, this is usually provided as an argument to + * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors + * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the + * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the + * order specified in \c keys. + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate in the order as specified here in \c keys + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ + friend GTSAM_EXPORT std::pair, boost::shared_ptr > + EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); + + /** + * splits a pre-factorized factor into a conditional, and changes the current + * factor to be the remaining component. Performs same operation as eliminate(), + * but without running QR. + */ + boost::shared_ptr splitConditional(size_t nrFrontals); + + protected: + + /// Internal function to fill blocks and set dimensions + template + void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(Ab_); + ar & BOOST_SERIALIZATION_NVP(model_); + } + }; // JacobianFactor + +} // gtsam + +#include + + diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index f72563d7a..04c26336f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -719,8 +719,8 @@ namespace gtsam { }; /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by: - /// Dipl.-Inform. Jan Oberl�nder (M.Sc.), FZI Research Center for - /// Information Technology, Karlsruhe, Germany. + /// Dipl.-Inform. Jan Oberl�nder (M.Sc.), FZI Research Center for + /// Information Technology, Karlsruhe, Germany. /// oberlaender@fzi.de /// Thanks Jan! class GTSAM_EXPORT Cauchy : public Base { diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 8c0de69a2..44cb8c146 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -23,18 +23,18 @@ using namespace std; namespace gtsam { - - /* ************************************************************************* */ - static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + + /* ************************************************************************* */ + static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); if( !jf ) { jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) } - result->push_back(jf); - } - return result; + result->push_back(jf); + } + return result; } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 9ca17e2fe..664fcf3b7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -1,347 +1,347 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VectorValues.cpp - * @brief Implementations for VectorValues - * @author Richard Roberts - * @author Alex Cunningham - */ - -#include - -#include -#include -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { - - using boost::combine; - using boost::adaptors::transformed; - using boost::adaptors::map_values; - using boost::accumulate; - - /* ************************************************************************* */ - VectorValues::VectorValues(const VectorValues& first, const VectorValues& second) - { - // Merge using predicate for comparing first of pair - merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), - boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2))); - if(size() != first.size() + second.size()) - throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); - } - - /* ************************************************************************* */ - VectorValues::VectorValues(const Vector& x, const Dims& dims) { - typedef pair Pair; - size_t j = 0; - BOOST_FOREACH(const Pair& v, dims) { - Key key; - size_t n; - boost::tie(key, n) = v; - values_.insert(make_pair(key, sub(x, j, j + n))); - j += n; - } - } - - /* ************************************************************************* */ - VectorValues VectorValues::Zero(const VectorValues& other) - { - VectorValues result; - BOOST_FOREACH(const KeyValuePair& v, other) - result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); - return result; - } - - /* ************************************************************************* */ - void VectorValues::update(const VectorValues& values) - { - iterator hint = begin(); - BOOST_FOREACH(const KeyValuePair& key_value, values) - { - // Use this trick to find the value using a hint, since we are inserting from another sorted map - size_t oldSize = values_.size(); - hint = values_.insert(hint, key_value); - if(values_.size() > oldSize) { - values_.unsafe_erase(hint); - throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); - } else { - hint->second = key_value.second; - } - } - } - - /* ************************************************************************* */ - void VectorValues::insert(const VectorValues& values) - { - size_t originalSize = size(); - values_.insert(values.begin(), values.end()); - if(size() != originalSize + values.size()) - throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys."); - } - - /* ************************************************************************* */ - void VectorValues::setZero() - { - BOOST_FOREACH(Vector& v, values_ | map_values) - v.setZero(); - } - - /* ************************************************************************* */ - void VectorValues::print(const string& str, const KeyFormatter& formatter) const { - cout << str << ": " << size() << " elements\n"; - BOOST_FOREACH(const value_type& key_value, *this) - cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; - cout.flush(); - } - - /* ************************************************************************* */ - bool VectorValues::equals(const VectorValues& x, double tol) const { - if(this->size() != x.size()) - return false; - typedef boost::tuple ValuePair; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { - if(values.get<0>().first != values.get<1>().first || - !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) - return false; - } - return true; - } - - /* ************************************************************************* */ - Vector VectorValues::vector() const - { - // Count dimensions - DenseIndex totalDim = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) - totalDim += v.size(); - - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) { - result.segment(pos, v.size()) = v; - pos += v.size(); - } - - return result; - } - - /* ************************************************************************* */ - Vector VectorValues::vector(const FastVector& keys) const - { - // Count dimensions and collect pointers to avoid double lookups - DenseIndex totalDim = 0; - FastVector items(keys.size()); - for(size_t i = 0; i < keys.size(); ++i) { - items[i] = &at(keys[i]); - totalDim += items[i]->size(); - } - - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - BOOST_FOREACH(const Vector *v, items) { - result.segment(pos, v->size()) = *v; - pos += v->size(); - } - - return result; - } - - /* ************************************************************************* */ - Vector VectorValues::vector(const Dims& keys) const - { - // Count dimensions - DenseIndex totalDim = 0; - BOOST_FOREACH(size_t dim, keys | map_values) - totalDim += dim; - Vector result(totalDim); - size_t j = 0; - BOOST_FOREACH(const Dims::value_type& it, keys) { - result.segment(j,it.second) = at(it.first); - j += it.second; - } - return result; - } - - /* ************************************************************************* */ - void VectorValues::swap(VectorValues& other) { - this->values_.swap(other.values_); - } - - /* ************************************************************************* */ - namespace internal - { - bool structureCompareOp(const boost::tuple& vv) - { - return vv.get<0>().first == vv.get<1>().first - && vv.get<0>().second.size() == vv.get<1>().second.size(); - } - } - - /* ************************************************************************* */ - bool VectorValues::hasSameStructure(const VectorValues other) const - { - return accumulate(combine(*this, other) - | transformed(internal::structureCompareOp), true, logical_and()); - } - - /* ************************************************************************* */ - double VectorValues::dot(const VectorValues& v) const - { - if(this->size() != v.size()) - throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); - double result = 0.0; - typedef boost::tuple ValuePair; - using boost::adaptors::map_values; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { - assert_throw(values.get<0>().first == values.get<1>().first, - invalid_argument("VectorValues::dot called with a VectorValues of different structure")); - assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), - invalid_argument("VectorValues::dot called with a VectorValues of different structure")); - result += values.get<0>().second.dot(values.get<1>().second); - } - return result; - } - - /* ************************************************************************* */ - double VectorValues::norm() const { - return std::sqrt(this->squaredNorm()); - } - - /* ************************************************************************* */ - double VectorValues::squaredNorm() const { - double sumSquares = 0.0; - using boost::adaptors::map_values; - BOOST_FOREACH(const Vector& v, *this | map_values) - sumSquares += v.squaredNorm(); - return sumSquares; - } - - /* ************************************************************************* */ - VectorValues VectorValues::operator+(const VectorValues& c) const - { - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+ called with different vector sizes"); - assert_throw(hasSameStructure(c), - invalid_argument("VectorValues::operator+ called with different vector sizes")); - - VectorValues result; - // The result.end() hint here should result in constant-time inserts - for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); - - return result; - } - - /* ************************************************************************* */ - VectorValues VectorValues::add(const VectorValues& c) const - { - return *this + c; - } - - /* ************************************************************************* */ - VectorValues& VectorValues::operator+=(const VectorValues& c) - { - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator+= called with different vector sizes"); - assert_throw(hasSameStructure(c), - invalid_argument("VectorValues::operator+= called with different vector sizes")); - - iterator j1 = begin(); - const_iterator j2 = c.begin(); - // The result.end() hint here should result in constant-time inserts - for(; j1 != end(); ++j1, ++j2) - j1->second += j2->second; - - return *this; - } - - /* ************************************************************************* */ - VectorValues& VectorValues::addInPlace(const VectorValues& c) - { - return *this += c; - } - - /* ************************************************************************* */ - VectorValues& VectorValues::addInPlace_(const VectorValues& c) - { - for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) { - pair xi = tryInsert(j2->first, Vector()); - if(xi.second) - xi.first->second = j2->second; - else - xi.first->second += j2->second; - } - return *this; - } - - /* ************************************************************************* */ - VectorValues VectorValues::operator-(const VectorValues& c) const - { - if(this->size() != c.size()) - throw invalid_argument("VectorValues::operator- called with different vector sizes"); - assert_throw(hasSameStructure(c), - invalid_argument("VectorValues::operator- called with different vector sizes")); - - VectorValues result; - // The result.end() hint here should result in constant-time inserts - for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); - - return result; - } - - /* ************************************************************************* */ - VectorValues VectorValues::subtract(const VectorValues& c) const - { - return *this - c; - } - - /* ************************************************************************* */ - VectorValues operator*(const double a, const VectorValues &v) - { - VectorValues result; - BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) - result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); - return result; - } - - /* ************************************************************************* */ - VectorValues VectorValues::scale(const double a) const - { - return a * *this; - } - - /* ************************************************************************* */ - VectorValues& VectorValues::operator*=(double alpha) - { - BOOST_FOREACH(Vector& v, *this | map_values) - v *= alpha; - return *this; - } - - /* ************************************************************************* */ - VectorValues& VectorValues::scaleInPlace(double alpha) - { - return *this *= alpha; - } - - /* ************************************************************************* */ - -} // \namespace gtsam +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VectorValues.cpp + * @brief Implementations for VectorValues + * @author Richard Roberts + * @author Alex Cunningham + */ + +#include + +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + + using boost::combine; + using boost::adaptors::transformed; + using boost::adaptors::map_values; + using boost::accumulate; + + /* ************************************************************************* */ + VectorValues::VectorValues(const VectorValues& first, const VectorValues& second) + { + // Merge using predicate for comparing first of pair + merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), + boost::bind(&less::operator(), less(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2))); + if(size() != first.size() + second.size()) + throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); + } + + /* ************************************************************************* */ + VectorValues::VectorValues(const Vector& x, const Dims& dims) { + typedef pair Pair; + size_t j = 0; + BOOST_FOREACH(const Pair& v, dims) { + Key key; + size_t n; + boost::tie(key, n) = v; + values_.insert(make_pair(key, sub(x, j, j + n))); + j += n; + } + } + + /* ************************************************************************* */ + VectorValues VectorValues::Zero(const VectorValues& other) + { + VectorValues result; + BOOST_FOREACH(const KeyValuePair& v, other) + result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); + return result; + } + + /* ************************************************************************* */ + void VectorValues::update(const VectorValues& values) + { + iterator hint = begin(); + BOOST_FOREACH(const KeyValuePair& key_value, values) + { + // Use this trick to find the value using a hint, since we are inserting from another sorted map + size_t oldSize = values_.size(); + hint = values_.insert(hint, key_value); + if(values_.size() > oldSize) { + values_.unsafe_erase(hint); + throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); + } else { + hint->second = key_value.second; + } + } + } + + /* ************************************************************************* */ + void VectorValues::insert(const VectorValues& values) + { + size_t originalSize = size(); + values_.insert(values.begin(), values.end()); + if(size() != originalSize + values.size()) + throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys."); + } + + /* ************************************************************************* */ + void VectorValues::setZero() + { + BOOST_FOREACH(Vector& v, values_ | map_values) + v.setZero(); + } + + /* ************************************************************************* */ + void VectorValues::print(const string& str, const KeyFormatter& formatter) const { + cout << str << ": " << size() << " elements\n"; + BOOST_FOREACH(const value_type& key_value, *this) + cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; + cout.flush(); + } + + /* ************************************************************************* */ + bool VectorValues::equals(const VectorValues& x, double tol) const { + if(this->size() != x.size()) + return false; + typedef boost::tuple ValuePair; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { + if(values.get<0>().first != values.get<1>().first || + !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) + return false; + } + return true; + } + + /* ************************************************************************* */ + Vector VectorValues::vector() const + { + // Count dimensions + DenseIndex totalDim = 0; + BOOST_FOREACH(const Vector& v, *this | map_values) + totalDim += v.size(); + + // Copy vectors + Vector result(totalDim); + DenseIndex pos = 0; + BOOST_FOREACH(const Vector& v, *this | map_values) { + result.segment(pos, v.size()) = v; + pos += v.size(); + } + + return result; + } + + /* ************************************************************************* */ + Vector VectorValues::vector(const FastVector& keys) const + { + // Count dimensions and collect pointers to avoid double lookups + DenseIndex totalDim = 0; + FastVector items(keys.size()); + for(size_t i = 0; i < keys.size(); ++i) { + items[i] = &at(keys[i]); + totalDim += items[i]->size(); + } + + // Copy vectors + Vector result(totalDim); + DenseIndex pos = 0; + BOOST_FOREACH(const Vector *v, items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; + } + + /* ************************************************************************* */ + Vector VectorValues::vector(const Dims& keys) const + { + // Count dimensions + DenseIndex totalDim = 0; + BOOST_FOREACH(size_t dim, keys | map_values) + totalDim += dim; + Vector result(totalDim); + size_t j = 0; + BOOST_FOREACH(const Dims::value_type& it, keys) { + result.segment(j,it.second) = at(it.first); + j += it.second; + } + return result; + } + + /* ************************************************************************* */ + void VectorValues::swap(VectorValues& other) { + this->values_.swap(other.values_); + } + + /* ************************************************************************* */ + namespace internal + { + bool structureCompareOp(const boost::tuple& vv) + { + return vv.get<0>().first == vv.get<1>().first + && vv.get<0>().second.size() == vv.get<1>().second.size(); + } + } + + /* ************************************************************************* */ + bool VectorValues::hasSameStructure(const VectorValues other) const + { + return accumulate(combine(*this, other) + | transformed(internal::structureCompareOp), true, logical_and()); + } + + /* ************************************************************************* */ + double VectorValues::dot(const VectorValues& v) const + { + if(this->size() != v.size()) + throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); + double result = 0.0; + typedef boost::tuple ValuePair; + using boost::adaptors::map_values; + BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { + assert_throw(values.get<0>().first == values.get<1>().first, + invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), + invalid_argument("VectorValues::dot called with a VectorValues of different structure")); + result += values.get<0>().second.dot(values.get<1>().second); + } + return result; + } + + /* ************************************************************************* */ + double VectorValues::norm() const { + return std::sqrt(this->squaredNorm()); + } + + /* ************************************************************************* */ + double VectorValues::squaredNorm() const { + double sumSquares = 0.0; + using boost::adaptors::map_values; + BOOST_FOREACH(const Vector& v, *this | map_values) + sumSquares += v.squaredNorm(); + return sumSquares; + } + + /* ************************************************************************* */ + VectorValues VectorValues::operator+(const VectorValues& c) const + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+ called with different vector sizes"); + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator+ called with different vector sizes")); + + VectorValues result; + // The result.end() hint here should result in constant-time inserts + for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) + result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + + return result; + } + + /* ************************************************************************* */ + VectorValues VectorValues::add(const VectorValues& c) const + { + return *this + c; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::operator+=(const VectorValues& c) + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator+= called with different vector sizes"); + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator+= called with different vector sizes")); + + iterator j1 = begin(); + const_iterator j2 = c.begin(); + // The result.end() hint here should result in constant-time inserts + for(; j1 != end(); ++j1, ++j2) + j1->second += j2->second; + + return *this; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::addInPlace(const VectorValues& c) + { + return *this += c; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::addInPlace_(const VectorValues& c) + { + for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) { + pair xi = tryInsert(j2->first, Vector()); + if(xi.second) + xi.first->second = j2->second; + else + xi.first->second += j2->second; + } + return *this; + } + + /* ************************************************************************* */ + VectorValues VectorValues::operator-(const VectorValues& c) const + { + if(this->size() != c.size()) + throw invalid_argument("VectorValues::operator- called with different vector sizes"); + assert_throw(hasSameStructure(c), + invalid_argument("VectorValues::operator- called with different vector sizes")); + + VectorValues result; + // The result.end() hint here should result in constant-time inserts + for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) + result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + + return result; + } + + /* ************************************************************************* */ + VectorValues VectorValues::subtract(const VectorValues& c) const + { + return *this - c; + } + + /* ************************************************************************* */ + VectorValues operator*(const double a, const VectorValues &v) + { + VectorValues result; + BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) + result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); + return result; + } + + /* ************************************************************************* */ + VectorValues VectorValues::scale(const double a) const + { + return a * *this; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::operator*=(double alpha) + { + BOOST_FOREACH(Vector& v, *this | map_values) + v *= alpha; + return *this; + } + + /* ************************************************************************* */ + VectorValues& VectorValues::scaleInPlace(double alpha) + { + return *this *= alpha; + } + + /* ************************************************************************* */ + +} // \namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 4ea01dfbe..502d9314b 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -1,384 +1,384 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VectorValues.h - * @brief Factor Graph Values - * @author Richard Roberts - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace gtsam { - - /** - * This class represents a collection of vector-valued variables associated - * each with a unique integer index. It is typically used to store the variables - * of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet - * returns this class. - * - * For basic usage, such as receiving a linear solution from gtsam solving functions, - * or creating this class in unit tests and examples where speed is not important, - * you can use a simple interface: - * - The default constructor VectorValues() to create this class - * - insert(Key, const Vector&) to add vector variables - * - operator[](Key) for read and write access to stored variables - * - \ref exists (Key) to check if a variable is present - * - Other facilities like iterators, size(), dim(), etc. - * - * Indices can be non-consecutive and inserted out-of-order, but you should not - * use indices that are larger than a reasonable array size because the indices - * correspond to positions in an internal array. - * - * Example: - * \code - VectorValues values; - values.insert(3, Vector_(3, 1.0, 2.0, 3.0)); - values.insert(4, Vector_(2, 4.0, 5.0)); - values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0)); - - // Prints [ 3.0 4.0 ] - gtsam::print(values[1]); - - // Prints [ 8.0 9.0 ] - values[1] = Vector_(2, 8.0, 9.0); - gtsam::print(values[1]); - \endcode - * - *

Advanced Interface and Performance Information

- * - * Internally, all vector values are stored as part of one large vector. In - * gtsam this vector is always pre-allocated for efficiency, using the - * advanced interface described below. Accessing and modifying already-allocated - * values is \f$ O(1) \f$. Using the insert() function of the standard interface - * is slow because it requires re-allocating the internal vector. - * - * For advanced usage, or where speed is important: - * - Allocate space ahead of time using a pre-allocating constructor - * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), - * SameStructure(), resize(), or append(). Do not use - * insert(Key, const Vector&), which always has to re-allocate the - * internal vector. - * - The vector() function permits access to the underlying Vector, for - * doing mathematical or other operations that require all values. - * - operator[]() returns a SubVector view of the underlying Vector, - * without copying any data. - * - * Access is through the variable index j, and returns a SubVector, - * which is a view on the underlying data structure. - * - * This class is additionally used in gradient descent and dog leg to store the gradient. - * \nosubgrouping - */ - class GTSAM_EXPORT VectorValues { - protected: - typedef VectorValues This; - typedef ConcurrentMap Values; ///< Typedef for the collection of Vectors making up a VectorValues - Values values_; ///< Collection of Vectors making up this VectorValues - - public: - typedef Values::iterator iterator; ///< Iterator over vector values - typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair - typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair - typedef std::map Dims; - - /// @name Standard Constructors - /// @{ - - /** - * Default constructor creates an empty VectorValues. - */ - VectorValues() {} - - /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ - VectorValues(const VectorValues& first, const VectorValues& second); - - /** Create from another container holding pair. */ - template - explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {} - - /** Implicit copy constructor to specialize the explicit constructor from any container. */ - VectorValues(const VectorValues& c) : values_(c.values_) {} - - /** Create from a pair of iterators over pair. */ - template - VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {} - - /** Constructor from Vector. */ - VectorValues(const Vector& c, const Dims& dims); - - /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ - static VectorValues Zero(const VectorValues& other); - - /// @} - /// @name Standard Interface - /// @{ - - /** Number of variables stored. */ - Key size() const { return values_.size(); } - - /** Return the dimension of variable \c j. */ - size_t dim(Key j) const { return at(j).rows(); } - - /** Check whether a variable with key \c j exists. */ - bool exists(Key j) const { return find(j) != end(); } - - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ - Vector& at(Key j) { - iterator item = find(j); - if(item == end()) - throw std::out_of_range( - "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); - else - return item->second; - } - - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ - const Vector& at(Key j) const { - const_iterator item = find(j); - if(item == end()) - throw std::out_of_range( - "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); - else - return item->second; - } - - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does - * not exist, identical to at(Key). */ - Vector& operator[](Key j) { return at(j); } - - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does - * not exist, identical to at(Key). */ - const Vector& operator[](Key j) const { return at(j); } - - /** For all key/value pairs in \c values, replace values with corresponding keys in this class - * with those in \c values. Throws std::out_of_range if any keys in \c values are not present - * in this class. */ - void update(const VectorValues& values); - - /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c - * j is already used. - * @param value The vector to be inserted. - * @param j The index with which the value will be associated. */ - iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector - } - - /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c - * j is already used. - * @param value The vector to be inserted. - * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. - std::pair result = values_.insert(key_value); - if(!result.second) - throw std::invalid_argument( - "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) - + "' already in this VectorValues."); - return result.first; - } - - /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be - * inserted are already used. */ - void insert(const VectorValues& values); - - /** insert that mimics the STL map insert - if the value already exists, the map is not modified - * and an iterator to the existing value is returned, along with 'false'. If the value did not - * exist, it is inserted and an iterator pointing to the new element, along with 'true', is - * returned. */ - std::pair tryInsert(Key j, const Vector& value) { - return values_.insert(std::make_pair(j, value)); } - - /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ - void erase(Key var) { - if(values_.unsafe_erase(var) == 0) - throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); - } - - /** Set all values to zero vectors. */ - void setZero(); - - iterator begin() { return values_.begin(); } ///< Iterator over variables - const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables - const_iterator end() const { return values_.end(); } ///< Iterator over variables - //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables - - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ - iterator find(Key j) { return values_.find(j); } - - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ - const_iterator find(Key j) const { return values_.find(j); } - - /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - /** equals required by Testable for unit testing */ - bool equals(const VectorValues& x, double tol = 1e-9) const; - - /// @{ - /// @name Advanced Interface - /// @{ - - /** Retrieve the entire solution as a single vector */ - Vector vector() const; - - /** Access a vector that is a subset of relevant keys. */ - Vector vector(const FastVector& keys) const; - - /** Access a vector that is a subset of relevant keys, dims version. */ - Vector vector(const Dims& dims) const; - - /** Swap the data in this VectorValues with another. */ - void swap(VectorValues& other); - - /** Check if this VectorValues has the same structure (keys and dimensions) as another */ - bool hasSameStructure(const VectorValues other) const; - - /// @} - /// @name Linear algebra operations - /// @{ - - /** Dot product with another VectorValues, interpreting both as vectors of - * their concatenated values. Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). */ - double dot(const VectorValues& v) const; - - /** Vector L2 norm */ - double norm() const; - - /** Squared vector L2 norm */ - double squaredNorm() const; - - /** Element-wise addition, synonym for add(). Both VectorValues must have the same structure - * (checked when NDEBUG is not defined). */ - VectorValues operator+(const VectorValues& c) const; - - /** Element-wise addition, synonym for operator+(). Both VectorValues must have the same - * structure (checked when NDEBUG is not defined). */ - VectorValues add(const VectorValues& c) const; - - /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). */ - VectorValues& operator+=(const VectorValues& c); - - /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the - * same structure (checked when NDEBUG is not defined). */ - VectorValues& addInPlace(const VectorValues& c); - - /** Element-wise addition in-place, but allows for empty slots in *this. Slower */ - VectorValues& addInPlace_(const VectorValues& c); - - /** Element-wise subtraction, synonym for subtract(). Both VectorValues must have the same - * structure (checked when NDEBUG is not defined). */ - VectorValues operator-(const VectorValues& c) const; - - /** Element-wise subtraction, synonym for operator-(). Both VectorValues must have the same - * structure (checked when NDEBUG is not defined). */ - VectorValues subtract(const VectorValues& c) const; - - /** Element-wise scaling by a constant. */ - friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v); - - /** Element-wise scaling by a constant. */ - VectorValues scale(const double a) const; - - /** Element-wise scaling by a constant in-place. */ - VectorValues& operator*=(double alpha); - - /** Element-wise scaling by a constant in-place. */ - VectorValues& scaleInPlace(double alpha); - - /// @} - - /// @} - /// @name Matlab syntactic sugar for linear algebra operations - /// @{ - - //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); } - - /// @} - - /** - * scale a vector by a scalar - */ - //friend VectorValues operator*(const double a, const VectorValues &v) { - // VectorValues result = VectorValues::SameStructure(v); - // for(Key j = 0; j < v.size(); ++j) - // result.values_[j] = a * v.values_[j]; - // return result; - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // y.values_[j] += alpha * x.values_[j]; - // else - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - //} - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void sqrt(VectorValues &x) { - // for(Key j = 0; j < x.size(); ++j) - // x.values_[j] = x.values_[j].cwiseSqrt(); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { - // if(numerator.size() != denominator.size() || numerator.size() != result.size()) - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < numerator.size(); ++j) - // if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) - // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); - // else - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void edivInPlace(VectorValues& x, const VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // x.values_[j].array() /= y.values_[j].array(); - // else - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - //} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(values_); - } - }; // VectorValues definition - -} // \namespace gtsam +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file VectorValues.h + * @brief Factor Graph Values + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + + /** + * This class represents a collection of vector-valued variables associated + * each with a unique integer index. It is typically used to store the variables + * of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet + * returns this class. + * + * For basic usage, such as receiving a linear solution from gtsam solving functions, + * or creating this class in unit tests and examples where speed is not important, + * you can use a simple interface: + * - The default constructor VectorValues() to create this class + * - insert(Key, const Vector&) to add vector variables + * - operator[](Key) for read and write access to stored variables + * - \ref exists (Key) to check if a variable is present + * - Other facilities like iterators, size(), dim(), etc. + * + * Indices can be non-consecutive and inserted out-of-order, but you should not + * use indices that are larger than a reasonable array size because the indices + * correspond to positions in an internal array. + * + * Example: + * \code + VectorValues values; + values.insert(3, (Vector(3) << 1.0, 2.0, 3.0)); + values.insert(4, (Vector(2) << 4.0, 5.0)); + values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0)); + + // Prints [ 3.0 4.0 ] + gtsam::print(values[1]); + + // Prints [ 8.0 9.0 ] + values[1] = (Vector(2) << 8.0, 9.0); + gtsam::print(values[1]); + \endcode + * + *

Advanced Interface and Performance Information

+ * + * Internally, all vector values are stored as part of one large vector. In + * gtsam this vector is always pre-allocated for efficiency, using the + * advanced interface described below. Accessing and modifying already-allocated + * values is \f$ O(1) \f$. Using the insert() function of the standard interface + * is slow because it requires re-allocating the internal vector. + * + * For advanced usage, or where speed is important: + * - Allocate space ahead of time using a pre-allocating constructor + * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), + * SameStructure(), resize(), or append(). Do not use + * insert(Key, const Vector&), which always has to re-allocate the + * internal vector. + * - The vector() function permits access to the underlying Vector, for + * doing mathematical or other operations that require all values. + * - operator[]() returns a SubVector view of the underlying Vector, + * without copying any data. + * + * Access is through the variable index j, and returns a SubVector, + * which is a view on the underlying data structure. + * + * This class is additionally used in gradient descent and dog leg to store the gradient. + * \nosubgrouping + */ + class GTSAM_EXPORT VectorValues { + protected: + typedef VectorValues This; + typedef ConcurrentMap Values; ///< Typedef for the collection of Vectors making up a VectorValues + Values values_; ///< Collection of Vectors making up this VectorValues + + public: + typedef Values::iterator iterator; ///< Iterator over vector values + typedef Values::const_iterator const_iterator; ///< Const iterator over vector values + //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values + //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair + typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair + typedef std::map Dims; + + /// @name Standard Constructors + /// @{ + + /** + * Default constructor creates an empty VectorValues. + */ + VectorValues() {} + + /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ + VectorValues(const VectorValues& first, const VectorValues& second); + + /** Create from another container holding pair. */ + template + explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {} + + /** Implicit copy constructor to specialize the explicit constructor from any container. */ + VectorValues(const VectorValues& c) : values_(c.values_) {} + + /** Create from a pair of iterators over pair. */ + template + VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {} + + /** Constructor from Vector. */ + VectorValues(const Vector& c, const Dims& dims); + + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ + static VectorValues Zero(const VectorValues& other); + + /// @} + /// @name Standard Interface + /// @{ + + /** Number of variables stored. */ + Key size() const { return values_.size(); } + + /** Return the dimension of variable \c j. */ + size_t dim(Key j) const { return at(j).rows(); } + + /** Check whether a variable with key \c j exists. */ + bool exists(Key j) const { return find(j) != end(); } + + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + Vector& at(Key j) { + iterator item = find(j); + if(item == end()) + throw std::out_of_range( + "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); + else + return item->second; + } + + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + const Vector& at(Key j) const { + const_iterator item = find(j); + if(item == end()) + throw std::out_of_range( + "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); + else + return item->second; + } + + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ + Vector& operator[](Key j) { return at(j); } + + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ + const Vector& operator[](Key j) const { return at(j); } + + /** For all key/value pairs in \c values, replace values with corresponding keys in this class + * with those in \c values. Throws std::out_of_range if any keys in \c values are not present + * in this class. */ + void update(const VectorValues& values); + + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. */ + iterator insert(Key j, const Vector& value) { + return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector + } + + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. */ + iterator insert(const std::pair& key_value) { + // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as + // it is inserted into the values_ map. + std::pair result = values_.insert(key_value); + if(!result.second) + throw std::invalid_argument( + "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) + + "' already in this VectorValues."); + return result.first; + } + + /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be + * inserted are already used. */ + void insert(const VectorValues& values); + + /** insert that mimics the STL map insert - if the value already exists, the map is not modified + * and an iterator to the existing value is returned, along with 'false'. If the value did not + * exist, it is inserted and an iterator pointing to the new element, along with 'true', is + * returned. */ + std::pair tryInsert(Key j, const Vector& value) { + return values_.insert(std::make_pair(j, value)); } + + /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ + void erase(Key var) { + if(values_.unsafe_erase(var) == 0) + throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + } + + /** Set all values to zero vectors. */ + void setZero(); + + iterator begin() { return values_.begin(); } ///< Iterator over variables + const_iterator begin() const { return values_.begin(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables + const_iterator end() const { return values_.end(); } ///< Iterator over variables + //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables + //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables + //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables + //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + + /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + iterator find(Key j) { return values_.find(j); } + + /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + const_iterator find(Key j) const { return values_.find(j); } + + /** print required by Testable for unit testing */ + void print(const std::string& str = "VectorValues: ", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + /** equals required by Testable for unit testing */ + bool equals(const VectorValues& x, double tol = 1e-9) const; + + /// @{ + /// @name Advanced Interface + /// @{ + + /** Retrieve the entire solution as a single vector */ + Vector vector() const; + + /** Access a vector that is a subset of relevant keys. */ + Vector vector(const FastVector& keys) const; + + /** Access a vector that is a subset of relevant keys, dims version. */ + Vector vector(const Dims& dims) const; + + /** Swap the data in this VectorValues with another. */ + void swap(VectorValues& other); + + /** Check if this VectorValues has the same structure (keys and dimensions) as another */ + bool hasSameStructure(const VectorValues other) const; + + /// @} + /// @name Linear algebra operations + /// @{ + + /** Dot product with another VectorValues, interpreting both as vectors of + * their concatenated values. Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + double dot(const VectorValues& v) const; + + /** Vector L2 norm */ + double norm() const; + + /** Squared vector L2 norm */ + double squaredNorm() const; + + /** Element-wise addition, synonym for add(). Both VectorValues must have the same structure + * (checked when NDEBUG is not defined). */ + VectorValues operator+(const VectorValues& c) const; + + /** Element-wise addition, synonym for operator+(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValues add(const VectorValues& c) const; + + /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + VectorValues& operator+=(const VectorValues& c); + + /** Element-wise addition in-place, synonym for operator+=(). Both VectorValues must have the + * same structure (checked when NDEBUG is not defined). */ + VectorValues& addInPlace(const VectorValues& c); + + /** Element-wise addition in-place, but allows for empty slots in *this. Slower */ + VectorValues& addInPlace_(const VectorValues& c); + + /** Element-wise subtraction, synonym for subtract(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValues operator-(const VectorValues& c) const; + + /** Element-wise subtraction, synonym for operator-(). Both VectorValues must have the same + * structure (checked when NDEBUG is not defined). */ + VectorValues subtract(const VectorValues& c) const; + + /** Element-wise scaling by a constant. */ + friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v); + + /** Element-wise scaling by a constant. */ + VectorValues scale(const double a) const; + + /** Element-wise scaling by a constant in-place. */ + VectorValues& operator*=(double alpha); + + /** Element-wise scaling by a constant in-place. */ + VectorValues& scaleInPlace(double alpha); + + /// @} + + /// @} + /// @name Matlab syntactic sugar for linear algebra operations + /// @{ + + //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); } + + /// @} + + /** + * scale a vector by a scalar + */ + //friend VectorValues operator*(const double a, const VectorValues &v) { + // VectorValues result = VectorValues::SameStructure(v); + // for(Key j = 0; j < v.size(); ++j) + // result.values_[j] = a * v.values_[j]; + // return result; + //} + + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { + // if(x.size() != y.size()) + // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + // for(Key j = 0; j < x.size(); ++j) + // if(x.values_[j].size() == y.values_[j].size()) + // y.values_[j] += alpha * x.values_[j]; + // else + // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); + //} + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void sqrt(VectorValues &x) { + // for(Key j = 0; j < x.size(); ++j) + // x.values_[j] = x.values_[j].cwiseSqrt(); + //} + + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { + // if(numerator.size() != denominator.size() || numerator.size() != result.size()) + // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + // for(Key j = 0; j < numerator.size(); ++j) + // if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) + // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); + // else + // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); + //} + + //// TODO: linear algebra interface seems to have been added for SPCG. + //friend void edivInPlace(VectorValues& x, const VectorValues& y) { + // if(x.size() != y.size()) + // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + // for(Key j = 0; j < x.size(); ++j) + // if(x.values_[j].size() == y.values_[j].size()) + // x.values_[j].array() /= y.values_[j].array(); + // else + // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); + //} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(values_); + } + }; // VectorValues definition + +} // \namespace gtsam diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index 1d041d017..9db8b7edc 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -24,7 +24,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector_(3, 1.0, 0.1, 0.0); + Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0a7fb270c..f7e3cfec8 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -236,7 +236,7 @@ namespace gtsam { // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; @@ -274,6 +274,7 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ + // deltaPij += deltaVij * deltaT; deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; @@ -341,8 +342,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ - typedef boost::shared_ptr shared_ptr; - +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index b535f5179..32911e589 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -113,7 +113,7 @@ namespace imuBias { // // return measurement - biasGyro_ - w_earth_rate_I; // -//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); +//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2))); //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c3753e5e7..bd8a0f80b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -304,7 +304,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 25a2765eb..2d766dc6a 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -168,9 +168,9 @@ TEST( CombinedImuFactor, ErrorWithBiases ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 2db7c7c68..cc88505bd 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -167,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1(3, 0.5, 0.0, 0.0); +// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2(3, 0.5, 0.0, 0.0); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1(3, 0.5, 0.0, 0.0); + LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2(3, 0.5, 0.0, 0.0); + LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index ef9ad234c..5765eca01 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -50,10 +50,10 @@ namespace gtsam { // and the key/index needs to be reset to 0, the first key in the next iteration. assert(marginal->nrFrontals() == 1); assert(marginal->nrParents() == 0); - newPrior = boost::make_shared( - marginal->keys().front(), - marginal->getA(marginal->begin()), - marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], + newPrior = boost::make_shared( + marginal->keys().front(), + marginal->getA(marginal->begin()), + marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], marginal->get_model()); return x; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index e0e0ced2f..9248617b5 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -166,39 +166,39 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // created above. if(!clique->solnPointers_.empty()) { - GaussianConditional& c = *clique->conditional(); - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(clique->conditional()->nrParents()); - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { - parentPointers.push_back(clique->solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + GaussianConditional& c = *clique->conditional(); + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(clique->conditional()->nrParents()); + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + parentPointers.push_back(clique->solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { const Vector& parentVector = parentPointer->second; xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); - vectorPos += parentVector.size(); - } - } - xS = c.getb() - c.get_S() * xS; - Vector soln = c.get_R().triangularView().solve(xS); - - // Check for indeterminant solution - if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); + vectorPos += parentVector.size(); + } + } + xS = c.getb() - c.get_S() * xS; + Vector soln = c.get_R().triangularView().solve(xS); + + // Check for indeterminant solution + if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { + clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); } } else diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 66a1ce1ae..469c78f33 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ @@ -552,8 +552,8 @@ public: * If provided, 'deletedFactorsIndices' will be augmented with the factor graph * indices of any factor that was removed during the 'marginalizeLeaves' call */ - void marginalizeLeaves(const FastList& leafKeys, - boost::optional&> marginalFactorsIndices = boost::none, + void marginalizeLeaves(const FastList& leafKeys, + boost::optional&> marginalFactorsIndices = boost::none, boost::optional&> deletedFactorsIndices = boost::none); /** Access the current linearization point */ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index c959473a8..4493a0b16 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -74,6 +74,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { return graph_.linearize(state_.values); } +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::increaseLambda(){ + state_.lambda *= params_.lambdaFactor; +} + +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::decreaseLambda(){ + state_.lambda /= params_.lambdaFactor; +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { @@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() { if (error <= state_.error) { state_.values.swap(newValues); state_.error = error; - state_.lambda /= params_.lambdaFactor; + decreaseLambda(); break; } else { // Either we're not cautious, or the same lambda was worse than the current error. @@ -157,7 +167,10 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - state_.lambda *= params_.lambdaFactor; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl; + + increaseLambda(); } } } catch (IndeterminantLinearSystemException& e) { @@ -172,7 +185,7 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - state_.lambda *= params_.lambdaFactor; + increaseLambda(); } } // Frank asks: why would we do that? diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index d8962da4a..15e14ab47 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -174,6 +174,12 @@ public: return state_.lambda; } + // Apply policy to increase lambda if the current update was successful + virtual void increaseLambda(); + + // Apply policy to decrease lambda if the current update was NOT successful + virtual void decreaseLambda(); + /// Access the current number of inner iterations int getInnerIterations() const { return state_.totalNumberInnerIterations; diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bf75f134b..fd70519dc 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -19,7 +19,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)); +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0)); const double tol = 1e-5; gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ec386950d..041ea0387 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -65,7 +65,8 @@ public: TEST( Values, equals1 ) { Values expected; - LieVector v(3, 5.0, 6.0, 7.0); + LieVector v((Vector(3) << 5.0, 6.0, 7.0)); + expected.insert(key1,v); Values actual; actual.insert(key1,v); @@ -76,8 +77,9 @@ TEST( Values, equals1 ) TEST( Values, equals2 ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 5.0, 6.0, 8.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -88,8 +90,9 @@ TEST( Values, equals2 ) TEST( Values, equals_nan ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, inf, inf, inf); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << inf, inf, inf)); + cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -100,10 +103,11 @@ TEST( Values, equals_nan ) TEST( Values, insert_good ) { Values cfg1, cfg2, expected; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -121,10 +125,11 @@ TEST( Values, insert_good ) TEST( Values, insert_bad ) { Values cfg1, cfg2; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); - LieVector v3(3, 2.0, 4.0, 3.0); - LieVector v4(3, 8.0, 3.0, 7.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); + LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); + LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); + cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); @@ -137,8 +142,8 @@ TEST( Values, insert_bad ) TEST( Values, update_element ) { Values cfg; - LieVector v1(3, 5.0, 6.0, 7.0); - LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); + LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); cfg.insert(key1, v1); CHECK(cfg.size() == 1); @@ -181,8 +186,8 @@ TEST(Values, basic_functions) //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, LieVector(2, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; @@ -195,16 +200,16 @@ TEST(Values, basic_functions) TEST(Values, expmap_a) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues increment = pair_list_of (key1, (Vector(3) << 1.0, 1.1, 1.2)) (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -213,15 +218,15 @@ TEST(Values, expmap_a) TEST(Values, expmap_b) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues increment = pair_list_of (key2, (Vector(3) << 1.3, 1.4, 1.5)); Values expected; - expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); CHECK(assert_equal(expected, config0.retract(increment))); } @@ -230,16 +235,16 @@ TEST(Values, expmap_b) //TEST(Values, expmap_c) //{ // Values config0; -// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); +// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); // // Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // // Values expected; -// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); +// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); +// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); // // CHECK(assert_equal(expected, config0.retract(increment))); //} @@ -248,8 +253,8 @@ TEST(Values, expmap_b) TEST(Values, expmap_d) { Values config0; - config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); @@ -266,8 +271,8 @@ TEST(Values, expmap_d) TEST(Values, localCoordinates) { Values valuesA; - valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); - valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); + valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); + valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); VectorValues expDelta = pair_list_of (key1, (Vector(3) << 0.1, 0.2, 0.3)) @@ -314,17 +319,17 @@ TEST(Values, exists_) TEST(Values, update) { Values config0; - config0.insert(key1, LieVector(1, 1.)); - config0.insert(key2, LieVector(1, 2.)); + config0.insert(key1, LieVector((Vector(1) << 1.))); + config0.insert(key2, LieVector((Vector(1) << 2.))); Values superset; - superset.insert(key1, LieVector(1, -1.)); - superset.insert(key2, LieVector(1, -2.)); + superset.insert(key1, LieVector((Vector(1) << -1.))); + superset.insert(key2, LieVector((Vector(1) << -2.))); config0.update(superset); Values expected; - expected.insert(key1, LieVector(1, -1.)); - expected.insert(key2, LieVector(1, -2.)); + expected.insert(key1, LieVector((Vector(1) << -1.))); + expected.insert(key2, LieVector((Vector(1) << -2.))); CHECK(assert_equal(expected,config0)); } diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 6f00c81a6..cf5760695 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -96,7 +96,7 @@ namespace gtsam { Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = Vector_(1, y2 - measuredRange_); + Vector e2 = (Vector(1) << y2 - measuredRange_); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp new file mode 100644 index 000000000..e27bbc8c5 --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EssentialMatrixConstraint.cpp + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#include +//#include +//#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void EssentialMatrixConstraint::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) + << "," << keyFormatter(this->key2()) << ")\n"; + measuredE_.print(" measured: "); + this->noiseModel_->print(" noise model: "); +} + +/* ************************************************************************* */ +bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && this->measuredE_.equals(e->measuredE_, tol); +} + +/* ************************************************************************* */ +Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1, + const Pose3& p2, boost::optional Hp1, + boost::optional Hp2) const { + + // compute relative Pose3 between p1 and p2 + Pose3 _1P2_ = p1.between(p2, Hp1, Hp2); + + // convert to EssentialMatrix + Matrix D_hx_1P2; + EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_, + (Hp1 || Hp2) ? boost::optional(D_hx_1P2) : boost::none); + + // Calculate derivatives if needed + if (Hp1) { + // Hp1 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p1 = *Hp1; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp1 = D_hx_1P2 * D_1P2_p1; + } + if (Hp2) { + // Hp2 will already contain the 6*6 derivative D_1P2_p1 + const Matrix& D_1P2_p2 = *Hp2; + // The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2: + *Hp2 = D_hx_1P2 * D_1P2_p2; + } + + // manifold equivalent of h(x)-z -> log(z,h(x)) + return measuredE_.localCoordinates(hx); // 5D error +} + +} /// namespace gtsam diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h new file mode 100644 index 000000000..f2eb76e2d --- /dev/null +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EssentialMatrixConstraint.h + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement + * @addtogroup SLAM + */ +class EssentialMatrixConstraint: public NoiseModelFactor2 { + +private: + + typedef EssentialMatrixConstraint This; + typedef NoiseModelFactor2 Base; + + EssentialMatrix measuredE_; /** The measurement is an essential matrix */ + +public: + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + EssentialMatrixConstraint() { + } + + /** + * Constructor + * @param key1 key for first pose + * @param key2 key for second pose + * @param measuredE measured EssentialMatrix + * @param model noise model, 5D ! + */ + EssentialMatrixConstraint(Key key1, Key key2, + const EssentialMatrix& measuredE, const SharedNoiseModel& model) : + Base(model, key1, key2), measuredE_(measuredE) { + } + + virtual ~EssentialMatrixConstraint() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + virtual Vector evaluateError(const Pose3& p1, const Pose3& p2, + boost::optional Hp1 = boost::none, // + boost::optional Hp2 = boost::none) const; + + /** return the measured */ + const EssentialMatrix& measured() const { + return measuredE_; + } + + /** number of variables attached to this factor */ + std::size_t size() const { + return 2; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measuredE_); + } +}; +// \class EssentialMatrixConstraint + +}/// namespace gtsam diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 7b807d88f..fe140a298 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -73,7 +73,7 @@ namespace gtsam { } else { hx = pose.range(point, H1, H2); } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /** return the measured */ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 43927757e..db91fbd45 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -229,7 +229,7 @@ pair load2D( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -403,7 +403,7 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } @@ -671,33 +671,36 @@ bool writeBAL(const string& filename, SfM_data &data) bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ - // CHECKS + // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() != data.number_cameras()){ - cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() - <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl; - return false; + if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = values.at(poseKey); + Cal3Bundler K = data.cameras[i].calibration(); + PinholeCamera camera(pose, K); + data.cameras[i] = camera; + } + } else { + Values valuesCameras = values.filter< PinholeCamera >(); + if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Key cameraKey = symbol('c',i); + PinholeCamera camera = values.at >(cameraKey); + data.cameras[i] = camera; + } + }else{ + cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() + <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; + return false; + } } + + // Store 3D points in SfM_data Values valuesPoints = values.filter(); if( valuesPoints.size() != data.number_tracks()){ cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() - <<") and values (#points " << valuesPoints.size() << ")!!" << endl; - } - if(valuesPoints.size() + valuesPoses.size() != values.size()){ - cout << "writeBALfromValues write only poses and points values!!" << endl; - return false; - } - if(valuesPoints.size()==0 || valuesPoses.size()==0){ - cout << "writeBALfromValues: No point or pose in values!!" << endl; - return false; - } - - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); - Pose3 pose = values.at(poseKey); - Cal3Bundler K = data.cameras[i].calibration(); - PinholeCamera camera(pose, K); - data.cameras[i] = camera; + <<") and values (#points " << valuesPoints.size() << ")!!" << endl; } for (size_t j = 0; j < data.number_tracks(); j++){ // for each point @@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ } } + // Write SfM_data to file return writeBAL(filename, data); } - } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index bd5a28cdd..0ee5aad9f 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); * while camera poses and values are read from Values) * @param filename The name of the BAL file to write * @param data SfM structure where the data is stored - * @param values structure where the graph values are stored + * @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera for the + * cameras, and should be Point3 for the 3D points). Note that the current version + * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * @return true if the parsing was successful, false otherwise */ GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp new file mode 100644 index 000000000..eb87100f6 --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testEssentialMatrixConstraint.cpp + * @brief Unit tests for EssentialMatrixConstraint Class + * @author Frank Dellaert + * @author Pablo Alcantarilla + * @date Jan 5, 2014 + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( EssentialMatrixConstraint, test ) { + // Create a factor + Key poseKey1(1); + Key poseKey2(2); + Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20); + Point3 trueTranslation(+0.5, -1.0, +1.0); + Sphere2 trueDirection(trueTranslation); + EssentialMatrix measurement(trueRotation, trueDirection); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25); + EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model); + + // Create a linearization point at the zero-error point + Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0)); + Pose3 pose2( + Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), + Point3(-3.37493895, 6.14660244, -8.93650986)); + + Vector expected = zero(5); + Vector actual = factor.evaluateError(pose1,pose2); + CHECK(assert_equal(expected, actual, 1e-8)); + + // Calculate numerical derivatives + Matrix expectedH1 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, + boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, + boost::none, boost::none), pose2); + + // Use the factor to calculate the derivative + Matrix actualH1; + Matrix actualH2; + factor.evaluateError(pose1, pose2, actualH1, actualH2); + + // Verify we get the expected error + CHECK(assert_equal(expectedH1, actualH1, 1e-9)); + CHECK(assert_equal(expectedH2, actualH2, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index a521e43fb..6d85685d5 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // Check evaluation - Point3 P1 = data.tracks[i].p; - const Point2 pi = camera2.project(P1); + Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); + const Point2 pi = SimpleCamera::project_to_camera(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); @@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) { truth.insert(i, LieScalar(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Optimize + LevenbergMarquardtParams parameters; + // parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(100); + EXPECT(assert_equal(bodyE, actual, 1e-1)); + for (size_t i = 0; i < 5; i++) + EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } } // namespace example1 diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b37b36546..a7c91de3f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h index 53a9190ce..dd3668087 100644 --- a/gtsam_unstable/base/FixedVector.h +++ b/gtsam_unstable/base/FixedVector.h @@ -45,23 +45,6 @@ public: std::copy(values, values+N, this->data()); } - /** - * nice constructor, dangerous as number of arguments must be exactly right - * and you have to pass doubles !!! always use 0.0 never 0 - * - * NOTE: this will throw warnings/explode if there is no argument - * before the variadic section, so there is a meaningless size argument. - */ - FixedVector(size_t n, ...) { - va_list ap; - va_start(ap, n); - for(size_t i = 0 ; i < N ; i++) { - double value = va_arg(ap, double); - (*this)(i) = value; - } - va_end(ap); - } - /** * Create vector initialized to a constant value * @param value constant value diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index 1fddf80ca..e5e297b7c 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -28,7 +28,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( testFixedVector, conversions ) { double data1[] = {1.0, 2.0, 3.0}; - Vector v1 = Vector_(3, data1); + Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]); TestVector3 fv1(v1), fv2(data1); Vector actFv2(fv2); @@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) { /* ************************************************************************* */ TEST( testFixedVector, variable_constructor ) { - TestVector3 act(3, 1.0, 2.0, 3.0); + TestVector3 act((Vector(3) << 1.0, 2.0, 3.0)); EXPECT_DOUBLES_EQUAL(1.0, act(0), tol); EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); @@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) { /* ************************************************************************* */ TEST( testFixedVector, equals ) { - TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0); - TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0); + TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)), + vec3((Vector(3) << 2.0, 3.0, 4.0)); + TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0)); EXPECT(assert_equal(vec1, vec1, tol)); EXPECT(assert_equal(vec1, vec2, tol)); @@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) { /* ************************************************************************* */ TEST( testFixedVector, static_constructors ) { TestVector3 actZero = TestVector3::zero(); - TestVector3 expZero(3, 0.0, 0.0, 0.0); + TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0)); EXPECT(assert_equal(expZero, actZero, tol)); TestVector3 actOnes = TestVector3::ones(); - TestVector3 expOnes(3, 1.0, 1.0, 1.0); + TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0)); EXPECT(assert_equal(expOnes, actOnes, tol)); TestVector3 actRepeat = TestVector3::repeat(2.3); - TestVector3 expRepeat(3, 2.3, 2.3, 2.3); + TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3)); EXPECT(assert_equal(expRepeat, actRepeat, tol)); TestVector3 actBasis = TestVector3::basis(1); - TestVector3 expBasis(3, 0.0, 1.0, 0.0); + TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0)); EXPECT(assert_equal(expBasis, actBasis, tol)); TestVector3 actDelta = TestVector3::delta(1, 2.3); - TestVector3 expDelta(3, 0.0, 2.3, 0.0); + TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0)); EXPECT(assert_equal(expDelta, actDelta, tol)); } diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index df9e14a36..3e1af8eb3 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const { /* ************************************************************************* */ Vector BearingS2::localCoordinates(const BearingS2& x) const { - return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0), + return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0), elevation_.localCoordinates(x.elevation_)(0)); } diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index c33505c54..4eb7992a2 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -166,7 +166,7 @@ public: gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi), + return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), gtsam::LieScalar(1./depth)); } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index 85b592b98..15fd47236 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend line by random dist and angle to get BC double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len); double tABC = randomAngle().theta(); - Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC)); + Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC)); // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 78347a077..894312556 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -128,7 +128,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // calculate angle to change by Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); - Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0))); + Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0))); // create a segment to use for intersection checking // find the closest intersection diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index e899da0c9..b477d3e44 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 1., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); LieScalar inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); + LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected(5,0.,0.,1., 0.1,0.2); + LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); LieScalar inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); @@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected(5,-5.,-5.,2., 3., -0.1); + LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); LieScalar inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 806f7dc16..dacf28992 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) { EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); - Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; + Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); EXPECT(assert_equal(x1, x2.retract(delta21), tol)); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 8eff62acb..234ad25bc 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -266,7 +266,7 @@ namespace gtsam { } } - return Vector_(2, p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h deleted file mode 100644 index bf86b6933..000000000 --- a/gtsam_unstable/slam/CombinedImuFactor.h +++ /dev/null @@ -1,673 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams - **/ - -#pragma once - -/* GTSAM includes */ -#include -#include -#include -#include -#include -#include - -/* External or standard includes */ -#include - - -namespace gtsam { - - /** - * - * @addtogroup SLAM - * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. - */ - - class CombinedImuFactor: public NoiseModelFactor6 { - - public: - - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ - - /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; - } - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; - } - - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class CombinedPreintegratedMeasurements { - public: - imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) - double deltaTij; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases - ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] - /** Default constructor, initialize with no IMU measurements */ - CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) - ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) - { - // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) - measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); - } - - CombinedPreintegratedMeasurements() : - biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) - { - } - - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat.print(" biasHat"); - std::cout << " deltaTij " << deltaTij << std::endl; - std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; - deltaRij.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; - } - - /** equals */ - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat.equals(expected.biasHat, tol) - && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) - && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) - && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) - && deltaRij.equals(expected.deltaRij, tol) - && std::fabs(deltaTij - expected.deltaTij) < tol - && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) - && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) - && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) - && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) - && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - delPdelBiasAcc += delVdelBiasAcc * deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT; - delVdelBiasAcc += -deltaRij.matrix() * deltaT; - delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; - delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we - // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) - const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); - - // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; - - - // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); - -// G_measCov_Gt.block(3,3,3,3) = (H_vel_biasacc) * (1/deltaT) * measurementCovariance.block(3,3,3,3) * (H_vel_biasacc.transpose()) + -// (H_vel_biasacc) * (1/deltaT) * -// ( measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * -// (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); - - G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); - - G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); - - // OFF BLOCK DIAGONAL TERMS - Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); - G_measCov_Gt.block(3,9,3,3) = block24; - G_measCov_Gt.block(9,3,3,3) = block24.transpose(); - - Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); - G_measCov_Gt.block(6,12,3,3) = block35; - G_measCov_Gt.block(12,6,3,3) = block35.transpose(); - - /* - // overall Jacobian wrt raw measurements (df/du) - Matrix3 H_vel_initbiasacc = H_vel_biasacc; - Matrix3 H_angles_initbiasomega = H_angles_biasomega; - - // COMBINED IMU FACTOR, preserves correlation with bias evolution and considers initial uncertainty on biases - Matrix G(15,21); - G << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, - H_vel_biasacc, Z_3x3, H_vel_biasacc, Z_3x3, H_vel_initbiasacc, Z_3x3, - Z_3x3, Z_3x3, - H_angles_biasomega, Z_3x3, H_angles_biasomega, Z_3x3, H_angles_initbiasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; - - Matrix ErrorMatrix = (1/deltaT) * G * measurementCovariance * G.transpose() - G_measCov_Gt; - std::cout << "---- matrix multiplication error = [" << ErrorMatrix << "];"<< std::endl; - double max_err=0; - for(int i=0;i<15;i++) - { - for(int j=0;j<15;j++) - { - if(fabs(ErrorMatrix(i,j))>max_err) - max_err = fabs(ErrorMatrix(i,j)); - } - } - std::cout << "---- max matrix multiplication error = [" << max_err << "];"<< std::endl; - - if(max_err>10e-15) - std::cout << "---- max matrix multiplication error *large* = [" << max_err << "];"<< std::endl; - - PreintMeasCov = F * PreintMeasCov * F.transpose() + (1/deltaT) * G * measurementCovariance * G.transpose(); - */ - - PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT; - deltaVij += deltaRij.matrix() * correctedAcc * deltaT; - deltaRij = deltaRij * Rincr; - deltaTij += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance); - ar & BOOST_SERIALIZATION_NVP(deltaPij); - ar & BOOST_SERIALIZATION_NVP(deltaVij); - ar & BOOST_SERIALIZATION_NVP(deltaRij); - ar & BOOST_SERIALIZATION_NVP(deltaTij); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); - } - }; - - private: - - typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; - - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - - public: - - /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - /** Default constructor - only use for serialization */ - CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} - - /** Constructor */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis) { - } - - virtual ~CombinedImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," - << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol); - } - - /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(15,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - - } - - if(H3) { - - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), - //dBiasAcc/dPosej - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPosej - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(), - //dBiasAcc/dVj - Matrix3::Zero(), - //dBiasOmega/dVj - Matrix3::Zero(); - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_i - Matrix3::Zero(), -Matrix3::Identity(); - } - - if(H6) { - - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfV/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfR/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - //dBiasAcc/dBias_j - Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_j - Matrix3::Zero(), Matrix3::Identity(); - } - - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - - return r; - } - - - /** predicted states from IMU */ - static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, - const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, - const CombinedPreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - pose_j = Pose3( Rot_j, Point3(pos_j) ); - - bias_j = bias_i; - } - - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - } - }; // \class CombinedImuFactor - -} /// namespace gtsam diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 5f09ec216..dec1b2378 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -537,7 +537,7 @@ public: Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5)); omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; // Calculating g @@ -551,7 +551,7 @@ public: double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + g_ENU = (Vector(3) << 0.0, 0.0, -g_calc); // Calculate rho @@ -560,7 +560,7 @@ public: double rho_E = -Vn/(Rm + height); double rho_N = Ve/(Rp + height); double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + rho_ENU = (Vector(3) << rho_E, rho_N, rho_U); } static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h deleted file mode 100644 index 0c08ed7be..000000000 --- a/gtsam_unstable/slam/ImuFactor.h +++ /dev/null @@ -1,565 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts - **/ - -#pragma once - -/* GTSAM includes */ -#include -#include -#include -#include -#include -#include - -/* External or standard includes */ -#include - - -namespace gtsam { - - /** - * - * @addtogroup SLAM - * * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. - */ - - class ImuFactor: public NoiseModelFactor5 { - - public: - - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ - - /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; - } - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; - } - - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class PreintegratedMeasurements { - public: - imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) - Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) - double deltaTij; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - - Vector3 initialRotationRate; ///< initial rotation rate reading from the IMU (at time i) - Vector3 finalRotationRate; ///< final rotation rate reading from the IMU (at time j) - - /** Default constructor, initialize with no IMU measurements */ - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Vector3& initialRotationRate = Vector3::Zero() ///< initial rotation rate reading from the IMU (at time i) - ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), - initialRotationRate(initialRotationRate), finalRotationRate(initialRotationRate) - { - measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; - PreintMeasCov = Matrix::Zero(9,9); - } - - PreintegratedMeasurements() : - biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), - delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), - delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), - initialRotationRate(Vector3::Zero()), finalRotationRate(Vector3::Zero()) - { - measurementCovariance = Matrix::Zero(9,9); - PreintMeasCov = Matrix::Zero(9,9); - } - - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat.print(" biasHat"); - std::cout << " deltaTij " << deltaTij << std::endl; - std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; - deltaRij.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; - } - - /** equals */ - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat.equals(expected.biasHat, tol) - && equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol) - && equal_with_abs_tol(deltaPij, expected.deltaPij, tol) - && equal_with_abs_tol(deltaVij, expected.deltaVij, tol) - && deltaRij.equals(expected.deltaRij, tol) - && std::fabs(deltaTij - expected.deltaTij) < tol - && equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol) - && equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol) - && equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol) - && equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol) - && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - - // NOTE: order is important here because each update uses old values. - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); - - finalRotationRate = correctedOmega; - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - delPdelBiasAcc += delVdelBiasAcc * deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT; - delVdelBiasAcc += -deltaRij.matrix() * deltaT; - delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; - delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - - // Update preintegrated mesurements covariance - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) - const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - - // first order uncertainty propagation - // the deltaT allows to pass from continuous time noise to discrete time noise - PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT; - deltaVij += deltaRij.matrix() * correctedAcc * deltaT; - deltaRij = deltaRij * Rincr; - deltaTij += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance); - ar & BOOST_SERIALIZATION_NVP(deltaPij); - ar & BOOST_SERIALIZATION_NVP(deltaVij); - ar & BOOST_SERIALIZATION_NVP(deltaRij); - ar & BOOST_SERIALIZATION_NVP(deltaTij); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega); - } - }; - - private: - - typedef ImuFactor This; - typedef NoiseModelFactor5 Base; - - PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - public: - - /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} - - /** Constructor */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { - } - - virtual ~ImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } - - /** Access the preintegrated measurements. */ - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); - } - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - - } - if(H3) { - - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); - } - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(); - } - if(H5) { - - const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; - } - - - /** predicted states from IMU */ - static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - pose_j = Pose3( Rot_j, Point3(pos_j) ); - } - - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // \class ImuFactor - -} /// namespace gtsam diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3cd87b8d2..29f9d4972 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -96,7 +96,7 @@ public: " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /** return the measurement */ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 5e67a39d3..d61787358 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -96,7 +96,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 2da440425..9d4113431 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -99,7 +99,7 @@ public: << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); } - return gtsam::Vector_(1, 0.0); + return (gtsam::Vector(1) << 0.0); } /// Evaluate error h(x)-z and optionally derivatives diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 711a44bf9..747ceafe7 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -35,7 +35,7 @@ public: } Vector b_g(double g_e) { - Vector n_g = Vector_(3, 0.0, 0.0, g_e); + Vector n_g = (Vector(3) << 0.0, 0.0, g_e); return (bRn_ * n_g).vector(); } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index bf0b2b6e3..1925a3fe4 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { + Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 11d8ed3a3..292e3f68f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p *H2 = zeros(1, 3); (*H2)(0, 2) = -1.0; } - return Vector_(1, hx - measured_); + return (Vector(1) << hx - measured_); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index 5ff8f66e7..0411765e8 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -276,7 +276,7 @@ namespace gtsam { } } - return Vector_(2, p_inlier, p_outlier); + return (Vector(2) << p_inlier, p_outlier); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index cc3d693ad..a3c963a58 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) { Mechanization_bRn2 mech; KalmanFilter::State state; // boost::tie(mech,state) = ahrs.initialize(g_e); -// Vector u = Vector_(3,0.05,0.0,0.0); +// Vector u = (Vector(3) << 0.05,0.0,0.0); // double dt = 2; // Rot3 expected; // Mechanization_bRn2 mech2 = mech.integrate(u,dt); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 53d94a4bc..7756e79d3 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -119,10 +119,10 @@ TEST( InertialNavFactor_GlobalVelocity, Predict) InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -157,8 +157,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -192,8 +192,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0)); + LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; @@ -447,10 +447,10 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector expectedVel2(3, 0.51, -0.48, 0.43); + LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Pose3 actualPose2; LieVector actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -488,8 +488,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); + LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); + LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -527,8 +527,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); + LieVector Vel1((Vector(3) << 0.0,0.0,0.0)); + LieVector Vel2((Vector(3) << 0.0,0.0,0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 65cc1adbc..5ea9fe29d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 0., 0., 1., 0., 0.); + LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.)); // initialize inverse depth with "incorrect" depth of 1/4 // in reality this is 1/5, but initial depth is guessed LieScalar inv_depth(1./4); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 7bbee65ee..24535854d 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected(6, x, y, z, theta, phi, rho); + LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 1512d0fc2..e99c9bcdf 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) { double theta = atan2(ray.y(), ray.x()); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double rho = 1./ray.norm(); - LieVector expected(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 799ebdf36..e65b7cacb 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) { double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); - LieVector expected(3, theta, phi, rho); + LieVector expected((Vector(3) << theta, phi, rho)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index dd16e6a9e..7b5f31660 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -187,7 +187,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(!rangeBound.isGreaterThan()); EXPECT(rangeBound.dim() == 1); - EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1))); + EXPECT(assert_equal(((Vector)Vector(1) << 2.0), rangeBound.evaluateError(pt1, pt1))); EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 4fbbcb1e6..be8165b10 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -1,858 +1,858 @@ -/** - * @file testGaussianISAM2.cpp - * @brief Unit tests for GaussianISAM2 - * @author Michael Kaess - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -using namespace boost::assign; -#include -namespace br { using namespace boost::adaptors; using namespace boost::range; } - -using namespace std; -using namespace gtsam; -using boost::shared_ptr; - -const double tol = 1e-4; - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - -// Set up parameters -SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); -SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); - -ISAM2 createSlamlikeISAM2( - boost::optional init_values = boost::none, - boost::optional full_graph = boost::none, - const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), - size_t maxPoses = 10) { - - // These variables will be reused and accumulate factors and values - ISAM2 isam(params); - Values fullinit; - NonlinearFactorGraph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - NonlinearFactorGraph newfactors; - newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - if(i > maxPoses) - goto done; - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - - if(i > maxPoses) - goto done; - } - - if(i > maxPoses) - goto done; - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - if(i > maxPoses) - goto done; - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - - if(i > maxPoses) - goto done; - } - - if(i > maxPoses) - goto done; - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } - -done: - - if (full_graph) - *full_graph = fullgraph; - - if (init_values) - *init_values = fullinit; - - return isam; -} - -/* ************************************************************************* */ -//TEST(ISAM2, CheckRelinearization) { -// -// typedef GaussianISAM2::Impl Impl; -// -// // Create values where indices 1 and 3 are above the threshold of 0.1 -// VectorValues values; -// values.reserve(4, 10); -// values.push_back_preallocated((Vector(2) << 0.09, 0.09)); -// values.push_back_preallocated((Vector(3) << 0.11, 0.11, 0.09)); -// values.push_back_preallocated((Vector(3) << 0.09, 0.09, 0.09)); -// values.push_back_preallocated((Vector(2) << 0.11, 0.11)); -// -// // Create a permutation -// Permutation permutation(4); -// permutation[0] = 2; -// permutation[1] = 0; -// permutation[2] = 1; -// permutation[3] = 3; -// -// Permuted permuted(permutation, values); -// -// // After permutation, the indices above the threshold are 2 and 2 -// FastSet expected; -// expected.insert(2); -// expected.insert(3); -// -// // Indices checked by CheckRelinearization -// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); -// -// EXPECT(assert_equal(expected, actual)); -//} - -/* ************************************************************************* */ -struct ConsistencyVisitor -{ - bool consistent; - const ISAM2& isam; - ConsistencyVisitor(const ISAM2& isam) : - consistent(true), isam(isam) {} - int operator()(const ISAM2::sharedClique& node, int& parentData) - { - if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) - { - if(node->parent_.expired()) - consistent = false; - if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) - consistent = false; - } - BOOST_FOREACH(Key j, node->conditional()->frontals()) - { - if(isam.nodes().at(j).get() != node.get()) - consistent = false; - } - return 0; - } -}; - -/* ************************************************************************* */ -bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { - - TestResult& result_ = result; - const std::string name_ = test.getName(); - - Values actual = isam.calculateEstimate(); - Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize()); - - bool isamEqual = assert_equal(expected, actual); - - // Check information - GaussianFactorGraph isamGraph(isam); - isamGraph += isam.roots().front()->cachedFactor_; - Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian(); - Matrix actualHessian = isamGraph.augmentedHessian(); - expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1); - bool isamTreeEqual = assert_equal(expectedHessian, actualHessian); - - // Check consistency - ConsistencyVisitor visitor(isam); - int data; // Unused - treeTraversal::DepthFirstForest(isam, data, visitor); - bool consistent = visitor.consistent; - - // The following two checks make sure that the cached gradients are maintained and used correctly - - // Check gradient at each node - bool nodeGradientsOk = true; - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { - // Compute expected gradient - GaussianFactorGraph jfg; - jfg += clique->conditional(); - VectorValues expectedGradient = jfg.gradientAtZero(); - // Compare with actual gradients - DenseIndex variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const DenseIndex dim = clique->conditional()->getDim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - bool gradOk = assert_equal(expectedGradient[*jit], actual); - EXPECT(gradOk); - nodeGradientsOk = nodeGradientsOk && gradOk; - variablePosition += dim; - } - bool dimOk = clique->gradientContribution().rows() == variablePosition; - EXPECT(dimOk); - nodeGradientsOk = nodeGradientsOk && dimOk; - } - - // Check gradient - VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); - VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); - VectorValues actualGradient; - gradientAtZero(isam, actualGradient); - bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); - EXPECT(expectedGradOk); - bool totalGradOk = assert_equal(expectedGradient, actualGradient); - EXPECT(totalGradOk); - - return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; -} - -/* ************************************************************************* */ -TEST(ISAM2, AddFactorsStep1) -{ - NonlinearFactorGraph nonlinearFactors; - nonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); - nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); - - NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); - - NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); - - const FastVector expectedNewFactorIndices = list_of(1)(3); - - FastVector actualNewFactorIndices; - - ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); - - EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); - EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); -} - -/* ************************************************************************* */ -TEST(ISAM2, simple) -{ - for(size_t i = 0; i < 10; ++i) { - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i); - - // Compare solutions - EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); - } -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_gaussnewton) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_dogleg) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_gaussnewton_qr) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_dogleg_qr) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, clone) { - - ISAM2 clone1; - - { - ISAM2 isam = createSlamlikeISAM2(); - clone1 = isam; - - ISAM2 clone2(isam); - - // Modify original isam - NonlinearFactorGraph factors; - factors += BetweenFactor(0, 10, - isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3)); - isam.update(factors); - - CHECK(assert_equal(createSlamlikeISAM2(), clone2)); - } - - // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced - // if the references in the iSAM2 copy point to the old instance which deleted at - // the end of the {...} section above. - ISAM2 temp = createSlamlikeISAM2(); - - CHECK(assert_equal(createSlamlikeISAM2(), clone1)); - CHECK(assert_equal(clone1, temp)); - - // Check clone empty - ISAM2 isam; - clone1 = isam; - CHECK(assert_equal(ISAM2(), clone1)); -} - -/* ************************************************************************* */ -TEST(ISAM2, removeFactors) -{ - // This test builds a graph in the same way as the "slamlike" test above, but - // then removes the 2nd-to-last landmark measurement - - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - - // Remove the 2nd measurement on landmark 0 (Key 100) - FastVector toRemove; - toRemove.push_back(12); - isam.update(NonlinearFactorGraph(), Values(), toRemove); - - // Remove the factor from the full system - fullgraph.remove(12); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, removeVariables) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - - // Remove the measurement on landmark 0 (Key 100) - FastVector toRemove; - toRemove.push_back(7); - toRemove.push_back(14); - isam.update(NonlinearFactorGraph(), Values(), toRemove); - - // Remove the factors and variable from the full system - fullgraph.remove(7); - fullgraph.remove(14); - fullinit.erase(100); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -/* ************************************************************************* */ -TEST(ISAM2, swapFactors) -{ - // This test builds a graph in the same way as the "slamlike" test above, but - // then swaps the 2nd-to-last landmark measurement with a different one - - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); - - // Remove the measurement on landmark 0 and replace with a different one - { - size_t swap_idx = isam.getFactorsUnsafe().size()-2; - FastVector toRemove; - toRemove.push_back(swap_idx); - fullgraph.remove(swap_idx); - - NonlinearFactorGraph swapfactors; -// swapfactors += BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor - swapfactors += BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); - fullgraph.push_back(swapfactors); - isam.update(swapfactors, Values(), toRemove); - } - - // Compare solutions - EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); - EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { - // Compute expected gradient - GaussianFactorGraph jfg; - jfg += clique->conditional(); - VectorValues expectedGradient = jfg.gradientAtZero(); - // Compare with actual gradients - DenseIndex variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const DenseIndex dim = clique->conditional()->getDim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); - } - - // Check gradient - VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); - VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); - VectorValues actualGradient; - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); -} - -/* ************************************************************************* */ -TEST(ISAM2, constrained_ordering) -{ - // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - Values fullinit; - NonlinearFactorGraph fullgraph; - - // We will constrain x3 and x4 to the end - FastMap constrained; - constrained.insert(make_pair((3), 1)); - constrained.insert(make_pair((4), 2)); - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - NonlinearFactorGraph newfactors; - newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - if(i >= 3) - isam.update(newfactors, init, FastVector(), constrained); - else - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init, FastVector(), constrained); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init, FastVector(), constrained); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init, FastVector(), constrained); - ++ i; - } - - // Compare solutions - EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { - // Compute expected gradient - GaussianFactorGraph jfg; - jfg += clique->conditional(); - VectorValues expectedGradient = jfg.gradientAtZero(); - // Compare with actual gradients - DenseIndex variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const DenseIndex dim = clique->conditional()->getDim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); - } - - // Check gradient - VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); - VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); - VectorValues actualGradient; - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); -} - -/* ************************************************************************* */ -TEST(ISAM2, slamlike_solution_partial_relinearization_check) -{ - // These variables will be reused and accumulate factors and values - Values fullinit; - NonlinearFactorGraph fullgraph; - ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); - params.enablePartialRelinearizationCheck = true; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); -} - -namespace { - bool checkMarginalizeLeaves(ISAM2& isam, const FastList& leafKeys) { - Matrix expectedAugmentedHessian, expected3AugmentedHessian; - vector toKeep; - BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys) - if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end()) - toKeep.push_back(j); - - // Calculate expected marginal from iSAM2 tree - expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian(); - - // Calculate expected marginal from cached linear factors - //assert(isam.params().cacheLinearizedFactors); - //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian(); - - // Calculate expected marginal from original nonlinear factors - expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint()) - ->marginal(toKeep, EliminateQR)->augmentedHessian(); - - // Do marginalization - isam.marginalizeLeaves(leafKeys); - - // Check - GaussianFactorGraph actualMarginalGraph(isam); - Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); - //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian(); - Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize( - isam.getLinearizationPoint())->augmentedHessian(); - assert(actualAugmentedHessian.allFinite()); - - // Check full marginalization - //cout << "treeEqual" << endl; - bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); - //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); - //cout << "nonlinEqual" << endl; - actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); - //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); - //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); - //cout << "nonlinCorrect" << endl; - bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); - - bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; - return ok; - } -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves1) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys = list_of(0); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves2) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - values.insert(3, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys = list_of(0); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves3) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); - - factors += BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - values.insert(3, LieVector(0.0)); - values.insert(4, LieVector(0.0)); - values.insert(5, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - constrainedKeys.insert(make_pair(3,3)); - constrainedKeys.insert(make_pair(4,4)); - constrainedKeys.insert(make_pair(5,5)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys = list_of(0); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves4) -{ - ISAM2 isam; - - NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - - Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); - - FastMap constrainedKeys; - constrainedKeys.insert(make_pair(0,0)); - constrainedKeys.insert(make_pair(1,1)); - constrainedKeys.insert(make_pair(2,2)); - - isam.update(factors, values, FastVector(), constrainedKeys); - - FastList leafKeys = list_of(1); - EXPECT(checkMarginalizeLeaves(isam, leafKeys)); -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalizeLeaves5) -{ - // Create isam2 - ISAM2 isam = createSlamlikeISAM2(); - - // Marginalize - FastList marginalizeKeys = list_of(0); - EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); -} - -/* ************************************************************************* */ -TEST(ISAM2, marginalCovariance) -{ - // Create isam2 - ISAM2 isam = createSlamlikeISAM2(); - - // Check marginal - Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5); - Matrix actual = isam.marginalCovariance(5); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(ISAM2, calculate_nnz) -{ - ISAM2 isam = createSlamlikeISAM2(); - int expected = 241; - int actual = calculate_nnz(isam.roots().front()); - - EXPECT_LONGS_EQUAL(expected, actual); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ +/** + * @file testGaussianISAM2.cpp + * @brief Unit tests for GaussianISAM2 + * @author Michael Kaess + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +using namespace boost::assign; +#include +namespace br { using namespace boost::adaptors; using namespace boost::range; } + +using namespace std; +using namespace gtsam; +using boost::shared_ptr; + +const double tol = 1e-4; + +// SETDEBUG("ISAM2 update", true); +// SETDEBUG("ISAM2 update verbose", true); +// SETDEBUG("ISAM2 recalculate", true); + +// Set up parameters +SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); +SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); + +ISAM2 createSlamlikeISAM2( + boost::optional init_values = boost::none, + boost::optional full_graph = boost::none, + const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), + size_t maxPoses = 10) { + + // These variables will be reused and accumulate factors and values + ISAM2 isam(params); + Values fullinit; + NonlinearFactorGraph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + NonlinearFactorGraph newfactors; + newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + + if(i > maxPoses) + goto done; + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + + if(i > maxPoses) + goto done; + } + + if(i > maxPoses) + goto done; + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + +done: + + if (full_graph) + *full_graph = fullgraph; + + if (init_values) + *init_values = fullinit; + + return isam; +} + +/* ************************************************************************* */ +//TEST(ISAM2, CheckRelinearization) { +// +// typedef GaussianISAM2::Impl Impl; +// +// // Create values where indices 1 and 3 are above the threshold of 0.1 +// VectorValues values; +// values.reserve(4, 10); +// values.push_back_preallocated((Vector(2) << 0.09, 0.09)); +// values.push_back_preallocated((Vector(3) << 0.11, 0.11, 0.09)); +// values.push_back_preallocated((Vector(3) << 0.09, 0.09, 0.09)); +// values.push_back_preallocated((Vector(2) << 0.11, 0.11)); +// +// // Create a permutation +// Permutation permutation(4); +// permutation[0] = 2; +// permutation[1] = 0; +// permutation[2] = 1; +// permutation[3] = 3; +// +// Permuted permuted(permutation, values); +// +// // After permutation, the indices above the threshold are 2 and 2 +// FastSet expected; +// expected.insert(2); +// expected.insert(3); +// +// // Indices checked by CheckRelinearization +// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// +// EXPECT(assert_equal(expected, actual)); +//} + +/* ************************************************************************* */ +struct ConsistencyVisitor +{ + bool consistent; + const ISAM2& isam; + ConsistencyVisitor(const ISAM2& isam) : + consistent(true), isam(isam) {} + int operator()(const ISAM2::sharedClique& node, int& parentData) + { + if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) + { + if(node->parent_.expired()) + consistent = false; + if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) + consistent = false; + } + BOOST_FOREACH(Key j, node->conditional()->frontals()) + { + if(isam.nodes().at(j).get() != node.get()) + consistent = false; + } + return 0; + } +}; + +/* ************************************************************************* */ +bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { + + TestResult& result_ = result; + const std::string name_ = test.getName(); + + Values actual = isam.calculateEstimate(); + Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize()); + + bool isamEqual = assert_equal(expected, actual); + + // Check information + GaussianFactorGraph isamGraph(isam); + isamGraph += isam.roots().front()->cachedFactor_; + Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian(); + Matrix actualHessian = isamGraph.augmentedHessian(); + expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1); + bool isamTreeEqual = assert_equal(expectedHessian, actualHessian); + + // Check consistency + ConsistencyVisitor visitor(isam); + int data; // Unused + treeTraversal::DepthFirstForest(isam, data, visitor); + bool consistent = visitor.consistent; + + // The following two checks make sure that the cached gradients are maintained and used correctly + + // Check gradient at each node + bool nodeGradientsOk = true; + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + // Compute expected gradient + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); + // Compare with actual gradients + DenseIndex variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const DenseIndex dim = clique->conditional()->getDim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + bool gradOk = assert_equal(expectedGradient[*jit], actual); + EXPECT(gradOk); + nodeGradientsOk = nodeGradientsOk && gradOk; + variablePosition += dim; + } + bool dimOk = clique->gradientContribution().rows() == variablePosition; + EXPECT(dimOk); + nodeGradientsOk = nodeGradientsOk && dimOk; + } + + // Check gradient + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient; + gradientAtZero(isam, actualGradient); + bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); + EXPECT(expectedGradOk); + bool totalGradOk = assert_equal(expectedGradient, actualGradient); + EXPECT(totalGradOk); + + return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; +} + +/* ************************************************************************* */ +TEST(ISAM2, AddFactorsStep1) +{ + NonlinearFactorGraph nonlinearFactors; + nonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += NonlinearFactor::shared_ptr(); + nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + + NonlinearFactorGraph newFactors; + newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); + newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + + NonlinearFactorGraph expectedNonlinearFactors; + expectedNonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + + const FastVector expectedNewFactorIndices = list_of(1)(3); + + FastVector actualNewFactorIndices; + + ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + + EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); + EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); +} + +/* ************************************************************************* */ +TEST(ISAM2, simple) +{ + for(size_t i = 0; i < 10; ++i) { + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i); + + // Compare solutions + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); + } +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_gaussnewton) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_dogleg) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_gaussnewton_qr) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_dogleg_qr) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, clone) { + + ISAM2 clone1; + + { + ISAM2 isam = createSlamlikeISAM2(); + clone1 = isam; + + ISAM2 clone2(isam); + + // Modify original isam + NonlinearFactorGraph factors; + factors += BetweenFactor(0, 10, + isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3)); + isam.update(factors); + + CHECK(assert_equal(createSlamlikeISAM2(), clone2)); + } + + // This is to (perhaps unsuccessfully) try to currupt unallocated memory referenced + // if the references in the iSAM2 copy point to the old instance which deleted at + // the end of the {...} section above. + ISAM2 temp = createSlamlikeISAM2(); + + CHECK(assert_equal(createSlamlikeISAM2(), clone1)); + CHECK(assert_equal(clone1, temp)); + + // Check clone empty + ISAM2 isam; + clone1 = isam; + CHECK(assert_equal(ISAM2(), clone1)); +} + +/* ************************************************************************* */ +TEST(ISAM2, removeFactors) +{ + // This test builds a graph in the same way as the "slamlike" test above, but + // then removes the 2nd-to-last landmark measurement + + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Remove the 2nd measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(12); + isam.update(NonlinearFactorGraph(), Values(), toRemove); + + // Remove the factor from the full system + fullgraph.remove(12); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, removeVariables) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Remove the measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(7); + toRemove.push_back(14); + isam.update(NonlinearFactorGraph(), Values(), toRemove); + + // Remove the factors and variable from the full system + fullgraph.remove(7); + fullgraph.remove(14); + fullinit.erase(100); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +/* ************************************************************************* */ +TEST(ISAM2, swapFactors) +{ + // This test builds a graph in the same way as the "slamlike" test above, but + // then swaps the 2nd-to-last landmark measurement with a different one + + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); + + // Remove the measurement on landmark 0 and replace with a different one + { + size_t swap_idx = isam.getFactorsUnsafe().size()-2; + FastVector toRemove; + toRemove.push_back(swap_idx); + fullgraph.remove(swap_idx); + + NonlinearFactorGraph swapfactors; +// swapfactors += BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor + swapfactors += BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + fullgraph.push_back(swapfactors); + isam.update(swapfactors, Values(), toRemove); + } + + // Compare solutions + EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); + + // Check gradient at each node + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + // Compute expected gradient + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); + // Compare with actual gradients + DenseIndex variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const DenseIndex dim = clique->conditional()->getDim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + EXPECT(assert_equal(expectedGradient[*jit], actual)); + variablePosition += dim; + } + EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); + } + + // Check gradient + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient; + gradientAtZero(isam, actualGradient); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + +/* ************************************************************************* */ +TEST(ISAM2, constrained_ordering) +{ + // These variables will be reused and accumulate factors and values + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; + NonlinearFactorGraph fullgraph; + + // We will constrain x3 and x4 to the end + FastMap constrained; + constrained.insert(make_pair((3), 1)); + constrained.insert(make_pair((4), 2)); + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + NonlinearFactorGraph newfactors; + newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + if(i >= 3) + isam.update(newfactors, init, FastVector(), constrained); + else + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init, FastVector(), constrained); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init, FastVector(), constrained); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + NonlinearFactorGraph newfactors; + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init, FastVector(), constrained); + ++ i; + } + + // Compare solutions + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); + + // Check gradient at each node + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { + // Compute expected gradient + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); + // Compare with actual gradients + DenseIndex variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const DenseIndex dim = clique->conditional()->getDim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + EXPECT(assert_equal(expectedGradient[*jit], actual)); + variablePosition += dim; + } + LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); + } + + // Check gradient + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient; + gradientAtZero(isam, actualGradient); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_partial_relinearization_check) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} + +namespace { + bool checkMarginalizeLeaves(ISAM2& isam, const FastList& leafKeys) { + Matrix expectedAugmentedHessian, expected3AugmentedHessian; + vector toKeep; + BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys) + if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end()) + toKeep.push_back(j); + + // Calculate expected marginal from iSAM2 tree + expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian(); + + // Calculate expected marginal from cached linear factors + //assert(isam.params().cacheLinearizedFactors); + //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian(); + + // Calculate expected marginal from original nonlinear factors + expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint()) + ->marginal(toKeep, EliminateQR)->augmentedHessian(); + + // Do marginalization + isam.marginalizeLeaves(leafKeys); + + // Check + GaussianFactorGraph actualMarginalGraph(isam); + Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); + //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian(); + Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize( + isam.getLinearizationPoint())->augmentedHessian(); + assert(actualAugmentedHessian.allFinite()); + + // Check full marginalization + //cout << "treeEqual" << endl; + bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); + //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); + //cout << "nonlinEqual" << endl; + actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); + //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); + //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); + //cout << "nonlinCorrect" << endl; + bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); + + bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; + return ok; + } +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves1) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves2) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + values.insert(3, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + constrainedKeys.insert(make_pair(3,3)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves3) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + + factors += BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + values.insert(3, LieVector(0.0)); + values.insert(4, LieVector(0.0)); + values.insert(5, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + constrainedKeys.insert(make_pair(3,3)); + constrainedKeys.insert(make_pair(4,4)); + constrainedKeys.insert(make_pair(5,5)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves4) +{ + ISAM2 isam; + + NonlinearFactorGraph factors; + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + + Values values; + values.insert(0, LieVector(0.0)); + values.insert(1, LieVector(0.0)); + values.insert(2, LieVector(0.0)); + + FastMap constrainedKeys; + constrainedKeys.insert(make_pair(0,0)); + constrainedKeys.insert(make_pair(1,1)); + constrainedKeys.insert(make_pair(2,2)); + + isam.update(factors, values, FastVector(), constrainedKeys); + + FastList leafKeys = list_of(1); + EXPECT(checkMarginalizeLeaves(isam, leafKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves5) +{ + // Create isam2 + ISAM2 isam = createSlamlikeISAM2(); + + // Marginalize + FastList marginalizeKeys = list_of(0); + EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); +} + +/* ************************************************************************* */ +TEST(ISAM2, marginalCovariance) +{ + // Create isam2 + ISAM2 isam = createSlamlikeISAM2(); + + // Check marginal + Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5); + Matrix actual = isam.marginalCovariance(5); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(ISAM2, calculate_nnz) +{ + ISAM2 isam = createSlamlikeISAM2(); + int expected = 241; + int actual = calculate_nnz(isam.roots().front()); + + EXPECT_LONGS_EQUAL(expected, actual); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index ea34afa79..0b84f137d 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { // Create a simple graph NonlinearFactorGraph fg; fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 247c6a6b0..390257f02 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -266,10 +266,10 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -312,11 +312,11 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); @@ -364,12 +364,12 @@ public: TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; Values tv; - tv.insert(X(1), LieVector(1, 1.0)); - tv.insert(X(2), LieVector(1, 2.0)); - tv.insert(X(3), LieVector(1, 3.0)); - tv.insert(X(4), LieVector(1, 4.0)); - tv.insert(X(5), LieVector(1, 5.0)); - tv.insert(X(6), LieVector(1, 6.0)); + tv.insert(X(1), LieVector((Vector(1) << 1.0))); + tv.insert(X(2), LieVector((Vector(1) << 2.0))); + tv.insert(X(3), LieVector((Vector(1) << 3.0))); + tv.insert(X(4), LieVector((Vector(1) << 4.0))); + tv.insert(X(5), LieVector((Vector(1) << 5.0))); + tv.insert(X(6), LieVector((Vector(1) << 6.0))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index ecf82d1bd..c9f434da2 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -287,8 +287,8 @@ TEST (testSerializationSLAM, smallExample_nonlinear) { /* ************************************************************************* */ TEST (testSerializationSLAM, factors) { - LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); - LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); + LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0)); + LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0)); Point2 point2(1.0, 2.0); StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); Point3 point3(1.0, 2.0, 3.0); diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index cf505c485..dcc31e0ec 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Values config; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 4529d45b3..fdbbf0e42 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -96,7 +96,7 @@ string Constructor::wrapper_fragment(FileWriter& file, if(!cppBaseClassName.empty()) { file.oss << "\n"; file.oss << " typedef boost::shared_ptr<" << cppBaseClassName << "> SharedBase;\n"; - file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; + file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; file.oss << " *reinterpret_cast(mxGetData(out[1])) = new SharedBase(*self);\n"; } diff --git a/wrap/matlab.h b/wrap/matlab.h index c84d6fdec..23f391903 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -1,427 +1,427 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file matlab.h - * @brief header file to be included in MATLAB wrappers - * @date 2008 - * @author Frank Dellaert - * @author Alex Cunningham - * @author Andrew Melim +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file matlab.h + * @brief header file to be included in MATLAB wrappers + * @date 2008 + * @author Frank Dellaert + * @author Alex Cunningham + * @author Andrew Melim * @author Richard Roberts - * - * wrapping and unwrapping is done using specialized templates, see - * http://www.cplusplus.com/doc/tutorial/templates.html - */ - -#include -#include - -using gtsam::Vector; -using gtsam::Matrix; - -extern "C" { -#include -} - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace boost; // not usual, but for conciseness of generated code - -// start GTSAM Specifics ///////////////////////////////////////////////// -// to enable Matrix and Vector constructor for SharedGaussian: -#define GTSAM_MAGIC_GAUSSIAN -// end GTSAM Specifics ///////////////////////////////////////////////// - -#if defined(__LP64__) || defined(_WIN64) -// 64-bit -#define mxUINT32OR64_CLASS mxUINT64_CLASS -#else -#define mxUINT32OR64_CLASS mxUINT32_CLASS -#endif - -// "Unique" key to signal calling the matlab object constructor with a raw pointer -// to a shared pointer of the same C++ object type as the MATLAB type. -// Also present in utilities.h -static const uint64_t ptr_constructor_key = - (uint64_t('G') << 56) | - (uint64_t('T') << 48) | - (uint64_t('S') << 40) | - (uint64_t('A') << 32) | - (uint64_t('M') << 24) | - (uint64_t('p') << 16) | - (uint64_t('t') << 8) | - (uint64_t('r')); - -//***************************************************************************** -// Utilities -//***************************************************************************** - -void error(const char* str) { - mexErrMsgIdAndTxt("wrap:error", str); -} - -mxArray *scalar(mxClassID classid) { - mwSize dims[1]; dims[0]=1; - return mxCreateNumericArray(1, dims, classid, mxREAL); -} - -void checkScalar(const mxArray* array, const char* str) { - int m = mxGetM(array), n = mxGetN(array); - if (m!=1 || n!=1) - mexErrMsgIdAndTxt("wrap: not a scalar in ", str); -} - -// Replacement streambuf for cout that writes to the MATLAB console -// Thanks to http://stackoverflow.com/a/249008 -class mstream : public std::streambuf { -protected: - virtual std::streamsize xsputn(const char *s, std::streamsize n) { - mexPrintf("%.*s",n,s); - return n; - } - virtual int overflow(int c = EOF) { - if (c != EOF) { - mexPrintf("%.1s",&c); - } - return 1; - } -}; - -//***************************************************************************** -// Check arguments -//***************************************************************************** - -void checkArguments(const string& name, int nargout, int nargin, int expected) { - stringstream err; - err << name << " expects " << expected << " arguments, not " << nargin; - if (nargin!=expected) - error(err.str().c_str()); -} - -//***************************************************************************** -// wrapping C++ basis types in MATLAB arrays -//***************************************************************************** - -// default wrapping throws an error: only basis types are allowed in wrap -template -mxArray* wrap(const Class& value) { - error("wrap internal error: attempted wrap of invalid type"); - return 0; -} - -// specialization to string -// wraps into a character array -template<> -mxArray* wrap(const string& value) { - return mxCreateString(value.c_str()); -} - -// specialization to char -template<> -mxArray* wrap(const char& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(char*)mxGetData(result) = value; - return result; -} - -// specialization to unsigned char -template<> -mxArray* wrap(const unsigned char& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(unsigned char*)mxGetData(result) = value; - return result; -} - -// specialization to bool -template<> -mxArray* wrap(const bool& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(bool*)mxGetData(result) = value; - return result; -} - -// specialization to size_t -template<> -mxArray* wrap(const size_t& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(size_t*)mxGetData(result) = value; - return result; -} - -// specialization to int -template<> -mxArray* wrap(const int& value) { - mxArray *result = scalar(mxUINT32OR64_CLASS); - *(int*)mxGetData(result) = value; - return result; -} - -// specialization to double -> just double -template<> -mxArray* wrap(const double& value) { - return mxCreateDoubleScalar(value); -} - -// wrap a const Eigen vector into a double vector -mxArray* wrap_Vector(const gtsam::Vector& v) { - int m = v.size(); - mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL); - double *data = mxGetPr(result); - for (int i=0;i double vector -template<> -mxArray* wrap(const gtsam::Vector& v) { - return wrap_Vector(v); -} - -// wrap a const Eigen MATRIX into a double matrix -mxArray* wrap_Matrix(const gtsam::Matrix& A) { - int m = A.rows(), n = A.cols(); -#ifdef DEBUG_WRAP - mexPrintf("wrap_Matrix called with A = \n", m,n); - gtsam::print(A); -#endif - mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL); - double *data = mxGetPr(result); - // converts from column-major to row-major - for (int j=0;j double matrix -template<> -mxArray* wrap(const gtsam::Matrix& A) { - return wrap_Matrix(A); -} - -//***************************************************************************** -// unwrapping MATLAB arrays into C++ basis types -//***************************************************************************** - -// default unwrapping throws an error -// as wrap only supports passing a reference or one of the basic types -template -T unwrap(const mxArray* array) { - error("wrap internal error: attempted unwrap of invalid type"); - return T(); -} - -// specialization to string -// expects a character array -// Warning: relies on mxChar==char -template<> -string unwrap(const mxArray* array) { - char *data = mxArrayToString(array); - if (data==NULL) error("unwrap: not a character array"); - string str(data); - mxFree(data); - return str; -} - -// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit -template -T myGetScalar(const mxArray* array) { - switch (mxGetClassID(array)) { - case mxINT64_CLASS: - return (T) *(int64_t*) mxGetData(array); - case mxUINT64_CLASS: - return (T) *(uint64_t*) mxGetData(array); - default: - // hope for the best! - return (T) mxGetScalar(array); - } -} - -// specialization to bool -template<> -bool unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to char -template<> -char unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to unsigned char -template<> -unsigned char unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to int -template<> -int unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to size_t -template<> -size_t unwrap(const mxArray* array) { - checkScalar(array, "unwrap"); - return myGetScalar(array); -} - -// specialization to double -template<> -double unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return myGetScalar(array); -} - -// specialization to Eigen vector -template<> -gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) { - int m = mxGetM(array), n = mxGetN(array); - if (mxIsDouble(array)==false || n!=1) error("unwrap: not a vector"); -#ifdef DEBUG_WRAP - mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n); -#endif - double* data = (double*)mxGetData(array); - gtsam::Vector v(m); - for (int i=0;i -gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) { - if (mxIsDouble(array)==false) error("unwrap: not a matrix"); - int m = mxGetM(array), n = mxGetN(array); -#ifdef DEBUG_WRAP - mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n); -#endif - double* data = (double*)mxGetData(array); - gtsam::Matrix A(m,n); - // converts from row-major to column-major - for (int j=0;j(mxGetData(input[0])) = ptr_constructor_key; - // Second input argument is the pointer - input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - *reinterpret_cast(mxGetData(input[1])) = pointer; - // If the class is virtual, use the RTTI name to look up the derived matlab type - const char *derivedClassName; - if(isVirtual) { - const mxArray *rttiRegistry = mexGetVariablePtr("global", "gtsamwrap_rttiRegistry"); - if(!rttiRegistry) - mexErrMsgTxt( - "gtsam wrap: RTTI registry is missing - it could have been cleared from the workspace." - " You can issue 'clear all' to completely clear the workspace, and next time a wrapped object is" - " created the RTTI registry will be recreated."); - const mxArray *derivedNameMx = mxGetField(rttiRegistry, 0, rttiName); - if(!derivedNameMx) - mexErrMsgTxt(( - "gtsam wrap: The derived class type " + string(rttiName) + " was not found in the RTTI registry. " - "Try calling 'clear all' twice consecutively - we have seen things not get unloaded properly the " - "first time. If this does not work, this may indicate an inconsistency in your wrap interface file. " - "The most likely cause for this is that a base class was marked virtual in the wrap interface " - "definition header file for gtsam or for your module, but a derived type was returned by a C++ " - "function and that derived type was not marked virtual (or was not specified in the wrap interface " - "definition header at all).").c_str()); - size_t strLen = mxGetN(derivedNameMx); - char *buf = new char[strLen+1]; - if(mxGetString(derivedNameMx, buf, strLen+1)) - mexErrMsgTxt("gtsam wrap: Internal error reading RTTI table, try 'clear all' to clear your workspace and reinitialize the toolbox."); - derivedClassName = buf; - input[2] = mxCreateString("void"); - nargin = 3; - } else { - derivedClassName = classname.c_str(); - } - // Call special pointer constructor, which sets 'self' - mexCallMATLAB(1,&result, nargin, input, derivedClassName); - // Deallocate our memory - mxDestroyArray(input[0]); - mxDestroyArray(input[1]); - if(isVirtual) { - mxDestroyArray(input[2]); - delete[] derivedClassName; - } - return result; -} - -/* - When the user calls a method that returns a shared pointer, we create - an ObjectHandle from the shared_pointer and return it as a proxy - class to matlab. -*/ -template -mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const std::string& matlabName, bool isVirtual) { - // Create actual class object from out pointer - mxArray* result; - if(isVirtual) { - boost::shared_ptr void_ptr(shared_ptr); - result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name()); - } else { - boost::shared_ptr *heapPtr = new boost::shared_ptr(shared_ptr); - result = create_object(matlabName, heapPtr, isVirtual, ""); - } - return result; -} - -template -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { - - mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str()); - if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh) - || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error( - "Parameter is not an Shared type."); - - boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); - return *spp; -} + * + * wrapping and unwrapping is done using specialized templates, see + * http://www.cplusplus.com/doc/tutorial/templates.html + */ + +#include +#include + +using gtsam::Vector; +using gtsam::Matrix; + +extern "C" { +#include +} + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost; // not usual, but for conciseness of generated code + +// start GTSAM Specifics ///////////////////////////////////////////////// +// to enable Matrix and Vector constructor for SharedGaussian: +#define GTSAM_MAGIC_GAUSSIAN +// end GTSAM Specifics ///////////////////////////////////////////////// + +#if defined(__LP64__) || defined(_WIN64) +// 64-bit +#define mxUINT32OR64_CLASS mxUINT64_CLASS +#else +#define mxUINT32OR64_CLASS mxUINT32_CLASS +#endif + +// "Unique" key to signal calling the matlab object constructor with a raw pointer +// to a shared pointer of the same C++ object type as the MATLAB type. +// Also present in utilities.h +static const uint64_t ptr_constructor_key = + (uint64_t('G') << 56) | + (uint64_t('T') << 48) | + (uint64_t('S') << 40) | + (uint64_t('A') << 32) | + (uint64_t('M') << 24) | + (uint64_t('p') << 16) | + (uint64_t('t') << 8) | + (uint64_t('r')); + +//***************************************************************************** +// Utilities +//***************************************************************************** + +void error(const char* str) { + mexErrMsgIdAndTxt("wrap:error", str); +} + +mxArray *scalar(mxClassID classid) { + mwSize dims[1]; dims[0]=1; + return mxCreateNumericArray(1, dims, classid, mxREAL); +} + +void checkScalar(const mxArray* array, const char* str) { + int m = mxGetM(array), n = mxGetN(array); + if (m!=1 || n!=1) + mexErrMsgIdAndTxt("wrap: not a scalar in ", str); +} + +// Replacement streambuf for cout that writes to the MATLAB console +// Thanks to http://stackoverflow.com/a/249008 +class mstream : public std::streambuf { +protected: + virtual std::streamsize xsputn(const char *s, std::streamsize n) { + mexPrintf("%.*s",n,s); + return n; + } + virtual int overflow(int c = EOF) { + if (c != EOF) { + mexPrintf("%.1s",&c); + } + return 1; + } +}; + +//***************************************************************************** +// Check arguments +//***************************************************************************** + +void checkArguments(const string& name, int nargout, int nargin, int expected) { + stringstream err; + err << name << " expects " << expected << " arguments, not " << nargin; + if (nargin!=expected) + error(err.str().c_str()); +} + +//***************************************************************************** +// wrapping C++ basis types in MATLAB arrays +//***************************************************************************** + +// default wrapping throws an error: only basis types are allowed in wrap +template +mxArray* wrap(const Class& value) { + error("wrap internal error: attempted wrap of invalid type"); + return 0; +} + +// specialization to string +// wraps into a character array +template<> +mxArray* wrap(const string& value) { + return mxCreateString(value.c_str()); +} + +// specialization to char +template<> +mxArray* wrap(const char& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(char*)mxGetData(result) = value; + return result; +} + +// specialization to unsigned char +template<> +mxArray* wrap(const unsigned char& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(unsigned char*)mxGetData(result) = value; + return result; +} + +// specialization to bool +template<> +mxArray* wrap(const bool& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(bool*)mxGetData(result) = value; + return result; +} + +// specialization to size_t +template<> +mxArray* wrap(const size_t& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(size_t*)mxGetData(result) = value; + return result; +} + +// specialization to int +template<> +mxArray* wrap(const int& value) { + mxArray *result = scalar(mxUINT32OR64_CLASS); + *(int*)mxGetData(result) = value; + return result; +} + +// specialization to double -> just double +template<> +mxArray* wrap(const double& value) { + return mxCreateDoubleScalar(value); +} + +// wrap a const Eigen vector into a double vector +mxArray* wrap_Vector(const gtsam::Vector& v) { + int m = v.size(); + mxArray *result = mxCreateDoubleMatrix(m, 1, mxREAL); + double *data = mxGetPr(result); + for (int i=0;i double vector +template<> +mxArray* wrap(const gtsam::Vector& v) { + return wrap_Vector(v); +} + +// wrap a const Eigen MATRIX into a double matrix +mxArray* wrap_Matrix(const gtsam::Matrix& A) { + int m = A.rows(), n = A.cols(); +#ifdef DEBUG_WRAP + mexPrintf("wrap_Matrix called with A = \n", m,n); + gtsam::print(A); +#endif + mxArray *result = mxCreateDoubleMatrix(m, n, mxREAL); + double *data = mxGetPr(result); + // converts from column-major to row-major + for (int j=0;j double matrix +template<> +mxArray* wrap(const gtsam::Matrix& A) { + return wrap_Matrix(A); +} + +//***************************************************************************** +// unwrapping MATLAB arrays into C++ basis types +//***************************************************************************** + +// default unwrapping throws an error +// as wrap only supports passing a reference or one of the basic types +template +T unwrap(const mxArray* array) { + error("wrap internal error: attempted unwrap of invalid type"); + return T(); +} + +// specialization to string +// expects a character array +// Warning: relies on mxChar==char +template<> +string unwrap(const mxArray* array) { + char *data = mxArrayToString(array); + if (data==NULL) error("unwrap: not a character array"); + string str(data); + mxFree(data); + return str; +} + +// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit +template +T myGetScalar(const mxArray* array) { + switch (mxGetClassID(array)) { + case mxINT64_CLASS: + return (T) *(int64_t*) mxGetData(array); + case mxUINT64_CLASS: + return (T) *(uint64_t*) mxGetData(array); + default: + // hope for the best! + return (T) mxGetScalar(array); + } +} + +// specialization to bool +template<> +bool unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to char +template<> +char unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to unsigned char +template<> +unsigned char unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to int +template<> +int unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to size_t +template<> +size_t unwrap(const mxArray* array) { + checkScalar(array, "unwrap"); + return myGetScalar(array); +} + +// specialization to double +template<> +double unwrap(const mxArray* array) { + checkScalar(array,"unwrap"); + return myGetScalar(array); +} + +// specialization to Eigen vector +template<> +gtsam::Vector unwrap< gtsam::Vector >(const mxArray* array) { + int m = mxGetM(array), n = mxGetN(array); + if (mxIsDouble(array)==false || n!=1) error("unwrap: not a vector"); +#ifdef DEBUG_WRAP + mexPrintf("unwrap< gtsam::Vector > called with %dx%d argument\n", m,n); +#endif + double* data = (double*)mxGetData(array); + gtsam::Vector v(m); + for (int i=0;i +gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) { + if (mxIsDouble(array)==false) error("unwrap: not a matrix"); + int m = mxGetM(array), n = mxGetN(array); +#ifdef DEBUG_WRAP + mexPrintf("unwrap< gtsam::Matrix > called with %dx%d argument\n", m,n); +#endif + double* data = (double*)mxGetData(array); + gtsam::Matrix A(m,n); + // converts from row-major to column-major + for (int j=0;j(mxGetData(input[0])) = ptr_constructor_key; + // Second input argument is the pointer + input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast(mxGetData(input[1])) = pointer; + // If the class is virtual, use the RTTI name to look up the derived matlab type + const char *derivedClassName; + if(isVirtual) { + const mxArray *rttiRegistry = mexGetVariablePtr("global", "gtsamwrap_rttiRegistry"); + if(!rttiRegistry) + mexErrMsgTxt( + "gtsam wrap: RTTI registry is missing - it could have been cleared from the workspace." + " You can issue 'clear all' to completely clear the workspace, and next time a wrapped object is" + " created the RTTI registry will be recreated."); + const mxArray *derivedNameMx = mxGetField(rttiRegistry, 0, rttiName); + if(!derivedNameMx) + mexErrMsgTxt(( + "gtsam wrap: The derived class type " + string(rttiName) + " was not found in the RTTI registry. " + "Try calling 'clear all' twice consecutively - we have seen things not get unloaded properly the " + "first time. If this does not work, this may indicate an inconsistency in your wrap interface file. " + "The most likely cause for this is that a base class was marked virtual in the wrap interface " + "definition header file for gtsam or for your module, but a derived type was returned by a C++ " + "function and that derived type was not marked virtual (or was not specified in the wrap interface " + "definition header at all).").c_str()); + size_t strLen = mxGetN(derivedNameMx); + char *buf = new char[strLen+1]; + if(mxGetString(derivedNameMx, buf, strLen+1)) + mexErrMsgTxt("gtsam wrap: Internal error reading RTTI table, try 'clear all' to clear your workspace and reinitialize the toolbox."); + derivedClassName = buf; + input[2] = mxCreateString("void"); + nargin = 3; + } else { + derivedClassName = classname.c_str(); + } + // Call special pointer constructor, which sets 'self' + mexCallMATLAB(1,&result, nargin, input, derivedClassName); + // Deallocate our memory + mxDestroyArray(input[0]); + mxDestroyArray(input[1]); + if(isVirtual) { + mxDestroyArray(input[2]); + delete[] derivedClassName; + } + return result; +} + +/* + When the user calls a method that returns a shared pointer, we create + an ObjectHandle from the shared_pointer and return it as a proxy + class to matlab. +*/ +template +mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const std::string& matlabName, bool isVirtual) { + // Create actual class object from out pointer + mxArray* result; + if(isVirtual) { + boost::shared_ptr void_ptr(shared_ptr); + result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name()); + } else { + boost::shared_ptr *heapPtr = new boost::shared_ptr(shared_ptr); + result = create_object(matlabName, heapPtr, isVirtual, ""); + } + return result; +} + +template +boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { + + mxArray* mxh = mxGetProperty(obj,0, propertyName.c_str()); + if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh) + || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error( + "Parameter is not an Shared type."); + + boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); + return *spp; +} diff --git a/wrap/utilities.h b/wrap/utilities.h index 54e1d008e..7280dd297 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -89,8 +89,8 @@ public: virtual const char* what() const throw() { return what_.c_str(); } }; -// "Unique" key to signal calling the matlab object constructor with a raw pointer -// to a shared pointer of the same C++ object type as the MATLAB type. +// "Unique" key to signal calling the matlab object constructor with a raw pointer +// to a shared pointer of the same C++ object type as the MATLAB type. // Also present in matlab.h static const uint64_t ptr_constructor_key = (uint64_t('G') << 56) |