From 686051c0328143b04fdc880a9376c536ade13d9a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 9 Jan 2014 16:38:47 -0500 Subject: [PATCH] Convert DOS line endings to UNIX --- .../Eigen/Eigen/src/Core/CommaInitializer.h | 140 +- gtsam/base/timing.h | 538 +++--- gtsam/base/types.h | 670 +++---- gtsam/discrete/DecisionTreeFactor.h | 8 +- gtsam/discrete/DiscreteFactorGraph.cpp | 268 +-- gtsam/discrete/DiscreteFactorGraph.h | 8 +- gtsam/geometry/Rot3.cpp | 396 ++-- gtsam/geometry/Rot3M.cpp | 616 +++--- gtsam/geometry/tests/timePose3.cpp | 22 +- gtsam/inference/BayesNet.h | 142 +- gtsam/inference/BayesTree.h | 570 +++--- gtsam/inference/BayesTreeCliqueBase.h | 346 ++-- gtsam/inference/Conditional.h | 304 +-- gtsam/inference/EliminationTree.h | 336 ++-- gtsam/inference/VariableIndex.cpp | 116 +- gtsam/inference/VariableIndex.h | 360 ++-- gtsam/linear/GaussianDensity.cpp | 2 +- gtsam/linear/JacobianFactor.h | 656 +++---- gtsam/linear/NoiseModel.h | 4 +- gtsam/linear/SubgraphPreconditioner.cpp | 16 +- gtsam/linear/VectorValues.cpp | 694 +++---- gtsam/linear/VectorValues.h | 768 ++++---- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 8 +- gtsam/nonlinear/ISAM2-inl.h | 62 +- gtsam/nonlinear/ISAM2.h | 4 +- tests/testGaussianISAM2.cpp | 1716 ++++++++--------- wrap/Constructor.cpp | 2 +- wrap/matlab.h | 852 ++++---- wrap/utilities.h | 4 +- 29 files changed, 4814 insertions(+), 4814 deletions(-) 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/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/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8dcf14d4b..fd2752450 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,198 +1,198 @@ -/* ---------------------------------------------------------------------------- - - * 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::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/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/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/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..a2f078ee2 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/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..d00be006b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -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/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/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) |