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

Convert DOS line endings to UNIX

release/4.3a0
Richard Roberts 2014-01-09 16:38:47 -05:00
parent ffd439d89a
commit 686051c032
29 changed files with 4814 additions and 4814 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

140
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h vendored
View File

@ -30,9 +30,9 @@ struct CommaInitializer :
{
typedef typename internal::dense_xpr_base<CommaInitializer<XprType> >::type Base;
EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator;
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::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<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(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<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(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<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType>
{
};
namespace internal {
template<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType>
{
};
}
/** \anchor MatrixBaseCommaInitRef

538
gtsam/base/timing.h
View File

@ -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 <string>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/version.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
// 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 <boost/timer/timer.hpp>
#else
# include <boost/timer.hpp>
#endif
#ifdef GTSAM_USE_TBB
# include <tbb/tick_count.h>
# 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<TimingOutline> parent_; ///< parent pointer
typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildMap;
ChildMap children_; ///< subtrees
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer timer_;
#else
boost::timer timer_;
gtsam::ValueWithDefault<bool,false> 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<TimingOutline>&
child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& 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<TimingOutline> timingRoot;
GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> 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<const ::gtsam::internal::TimingOutline> 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 <string>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/version.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
// 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 <boost/timer/timer.hpp>
#else
# include <boost/timer.hpp>
#endif
#ifdef GTSAM_USE_TBB
# include <tbb/tick_count.h>
# 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<TimingOutline> parent_; ///< parent pointer
typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildMap;
ChildMap children_; ///< subtrees
#ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer timer_;
#else
boost::timer timer_;
gtsam::ValueWithDefault<bool,false> 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<TimingOutline>&
child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& 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<TimingOutline> timingRoot;
GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> 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<const ::gtsam::internal::TimingOutline> 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
}

670
gtsam/base/types.h
View File

@ -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 <gtsam/dllexport.h>
#include <gtsam/config.h>
#include <cstddef>
#include <string>
#include <iostream>
#include <boost/function/function1.hpp>
#include <boost/range/concepts.hpp>
#include <boost/optional.hpp>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
#include <omp.h>
#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<std::string(Key)> 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<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
typename AS_CONST>
struct const_selector {
};
/** Specialization for the non-const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_NON_CONST type;
};
/** Specialization for the const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
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<typename T, T defaultValue>
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<typename T>
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<ListOfOneContainer<int> >));
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
template<typename T>
ListOfOneContainer<T> ListOfOne(const T& element) {
return ListOfOneContainer<T>(element);
}
/* ************************************************************************* */
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED>
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<char, std::char_traits<char>, tbb::tbb_allocator<char> > String;
#else
private:
typedef std::exception Base;
protected:
typedef std::string String;
#endif
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> 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<DERIVED&>(*this));
clone->dynamic_ = true;
return clone;
}
virtual void destroy() throw() {
if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED();
scalable_free(derivedPtr);
}
}
virtual void throw_self() {
throw *static_cast<DERIVED*>(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<RuntimeErrorThreadsafe>
{
public:
/// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe runtime error exception
class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
{
public:
/// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe invalid argument exception
class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
{
public:
/// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(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 <boost/math/special_functions/fpclassify.hpp>
namespace std {
template<typename T> inline int isfinite(T a) {
return (int)boost::math::isfinite(a); }
template<typename T> inline int isnan(T a) {
return (int)boost::math::isnan(a); }
template<typename T> inline int isinf(T a) {
return (int)boost::math::isinf(a); }
}
#include <boost/math/constants/constants.hpp>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())
#endif
#ifndef M_PI_2
#define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
#endif
#ifndef M_PI_4
#define M_PI_4 (boost::math::constants::pi<double>() / 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 <gtsam/dllexport.h>
#include <gtsam/config.h>
#include <cstddef>
#include <string>
#include <iostream>
#include <boost/function/function1.hpp>
#include <boost/range/concepts.hpp>
#include <boost/optional.hpp>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
#include <omp.h>
#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<std::string(Key)> 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<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
typename AS_CONST>
struct const_selector {
};
/** Specialization for the non-const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_NON_CONST type;
};
/** Specialization for the const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
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<typename T, T defaultValue>
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<typename T>
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<ListOfOneContainer<int> >));
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
template<typename T>
ListOfOneContainer<T> ListOfOne(const T& element) {
return ListOfOneContainer<T>(element);
}
/* ************************************************************************* */
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED>
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<char, std::char_traits<char>, tbb::tbb_allocator<char> > String;
#else
private:
typedef std::exception Base;
protected:
typedef std::string String;
#endif
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> 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<DERIVED&>(*this));
clone->dynamic_ = true;
return clone;
}
virtual void destroy() throw() {
if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED();
scalable_free(derivedPtr);
}
}
virtual void throw_self() {
throw *static_cast<DERIVED*>(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<RuntimeErrorThreadsafe>
{
public:
/// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe runtime error exception
class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
{
public:
/// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe invalid argument exception
class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
{
public:
/// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(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 <boost/math/special_functions/fpclassify.hpp>
namespace std {
template<typename T> inline int isfinite(T a) {
return (int)boost::math::isfinite(a); }
template<typename T> inline int isnan(T a) {
return (int)boost::math::isnan(a); }
template<typename T> inline int isinf(T a) {
return (int)boost::math::isinf(a); }
}
#include <boost/math/constants/constants.hpp>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())
#endif
#ifndef M_PI_2
#define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
#endif
#ifndef M_PI_4
#define M_PI_4 (boost::math::constants::pi<double>() / 4.0)
#endif
#endif
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#ifdef ERROR
#undef ERROR
#endif

8
gtsam/discrete/DecisionTreeFactor.h
View File

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

268
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -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 <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <boost/make_shared.hpp>
namespace gtsam {
// Instantiate base classes
template class FactorGraph<DiscreteFactor>;
template class EliminateableFactorGraph<DiscreteFactorGraph>;
/* ************************************************************************* */
bool DiscreteFactorGraph::equals(const This& fg, double tol) const
{
return Base::equals(fg, tol);
}
/* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> 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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
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 <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <boost/make_shared.hpp>
namespace gtsam {
// Instantiate base classes
template class FactorGraph<DiscreteFactor>;
template class EliminateableFactorGraph<DiscreteFactorGraph>;
/* ************************************************************************* */
bool DiscreteFactorGraph::equals(const This& fg, double tol) const
{
return Base::equals(fg, tol);
}
/* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> 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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
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

8
gtsam/discrete/DiscreteFactorGraph.h
View File

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

396
gtsam/geometry/Rot3.cpp
View File

@ -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 <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
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<double> 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<Matrix&> HR, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3, Vector3> 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 <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
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<double> 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<Matrix&> HR, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix3, Vector3> 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

616
gtsam/geometry/Rot3M.cpp
View File

@ -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 <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<double>();
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 <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
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<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> 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<Matrix&> H1, boost::optional<Matrix&> 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<double>();
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

22
gtsam/geometry/tests/timePose3.cpp
View File

@ -17,23 +17,23 @@
#include <iostream>
#include <gtsam/base/timing.h>
#include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h>
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;
}

142
gtsam/inference/BayesNet.h
View File

@ -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 <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorGraph.h>
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 CONDITIONAL>
class BayesNet : public FactorGraph<CONDITIONAL> {
private:
typedef FactorGraph<CONDITIONAL> Base;
public:
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
protected:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNet() {};
/** Construct from iterator over conditionals */
template<typename ITERATOR>
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 <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorGraph.h>
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 CONDITIONAL>
class BayesNet : public FactorGraph<CONDITIONAL> {
private:
typedef FactorGraph<CONDITIONAL> Base;
public:
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
protected:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNet() {};
/** Construct from iterator over conditionals */
template<typename ITERATOR>
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;
};
}

570
gtsam/inference/BayesTree.h
View File

@ -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 <string>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
// Forward declarations
template<class FACTOR> class FactorGraph;
template<class BAYESTREE, class GRAPH> 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<std::size_t> conditionalSizes;
FastVector<std::size_t> 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 CLIQUE>
class BayesTree
{
protected:
typedef BayesTree<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr;
public:
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> 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<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;
/** A convenience class for a list of shared cliques */
typedef FastList<sharedClique> Cliques;
/** Map from keys to Clique */
typedef ConcurrentMap<Key, sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root cliques */
FastVector<sharedClique> 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<sharedClique>& 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<class CONTAINER>
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<Key>& 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<FactorType>& 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<class BAYESRTEE, class GRAPH> friend class ClusterTree;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(roots_);
}
/// @}
}; // BayesTree
/* ************************************************************************* */
template<class CLIQUE>
class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType
{
public:
typedef CLIQUE CliqueType;
typedef typename CLIQUE::ConditionalType Base;
boost::shared_ptr<CliqueType> clique;
BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& 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 <string>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
// Forward declarations
template<class FACTOR> class FactorGraph;
template<class BAYESTREE, class GRAPH> 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<std::size_t> conditionalSizes;
FastVector<std::size_t> 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 CLIQUE>
class BayesTree
{
protected:
typedef BayesTree<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr;
public:
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> 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<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;
/** A convenience class for a list of shared cliques */
typedef FastList<sharedClique> Cliques;
/** Map from keys to Clique */
typedef ConcurrentMap<Key, sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root cliques */
FastVector<sharedClique> 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<sharedClique>& 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<class CONTAINER>
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<Key>& 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<FactorType>& 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<class BAYESRTEE, class GRAPH> friend class ClusterTree;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(roots_);
}
/// @}
}; // BayesTree
/* ************************************************************************* */
template<class CLIQUE>
class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType
{
public:
typedef CLIQUE CliqueType;
typedef typename CLIQUE::ConditionalType Base;
boost::shared_ptr<CliqueType> clique;
BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& 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

346
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -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 <boost/optional.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
// Forward declarations
template<class CLIQUE> class BayesTree;
template<class GRAPH> 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 DERIVED, class FACTORGRAPH>
class BayesTreeCliqueBase
{
private:
typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
typedef DERIVED DerivedType;
typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
public:
typedef FACTORGRAPH FactorGraphType;
typedef typename EliminationTraitsType::BayesNetType BayesNetType;
typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> 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<FactorGraphType> cachedSeparatorMarginal_;
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
FastVector<derived_ptr> 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<FactorGraphType>& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTree<DerivedType>;
protected:
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
FastVector<Key> 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<Key> 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<class ARCHIVE>
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 <boost/optional.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
namespace gtsam {
// Forward declarations
template<class CLIQUE> class BayesTree;
template<class GRAPH> 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 DERIVED, class FACTORGRAPH>
class BayesTreeCliqueBase
{
private:
typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
typedef DERIVED DerivedType;
typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
public:
typedef FACTORGRAPH FactorGraphType;
typedef typename EliminationTraitsType::BayesNetType BayesNetType;
typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> 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<FactorGraphType> cachedSeparatorMarginal_;
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
FastVector<derived_ptr> 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<FactorGraphType>& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTree<DerivedType>;
protected:
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
FastVector<Key> 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<Key> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children);
}
/// @}
};
}

304
gtsam/inference/Conditional.h
View File

@ -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 <boost/range.hpp>
#include <gtsam/inference/Key.h>
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 FACTOR, class DERIVEDCONDITIONAL>
class Conditional
{
protected:
/** The first nrFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
private:
/// Typedef to this class
typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
public:
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> 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<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
// Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 <boost/range.hpp>
#include <gtsam/inference/Key.h>
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 FACTOR, class DERIVEDCONDITIONAL>
class Conditional
{
protected:
/** The first nrFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
private:
/// Typedef to this class
typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
public:
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> 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<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
// Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
/// @}
};
} // gtsam

336
gtsam/inference/EliminationTree.h
View File

@ -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 <utility>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
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 BAYESNET, class GRAPH>
class EliminationTree
{
protected:
typedef EliminationTree<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> 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<FactorType> 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<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename GRAPH::Eliminate Eliminate;
struct Node {
typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root
Factors factors; ///< factors associated with root
Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const;
void print(const std::string& str, const KeyFormatter& keyFormatter) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
protected:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
FastVector<sharedNode> roots_;
FastVector<sharedFactor> 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<DERIVEDFACTOR>&)
* 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<BayesNetType>, boost::shared_ptr<FactorGraphType> >
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<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */
const FastVector<sharedFactor>& 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 <utility>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
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 BAYESNET, class GRAPH>
class EliminationTree
{
protected:
typedef EliminationTree<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> 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<FactorType> 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<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename GRAPH::Eliminate Eliminate;
struct Node {
typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root
Factors factors; ///< factors associated with root
Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const;
void print(const std::string& str, const KeyFormatter& keyFormatter) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
protected:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
FastVector<sharedNode> roots_;
FastVector<sharedFactor> 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<DERIVEDFACTOR>&)
* 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<BayesNetType>, boost::shared_ptr<FactorGraphType> >
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<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */
const FastVector<sharedFactor>& 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;
};
}

116
gtsam/inference/VariableIndex.cpp
View File

@ -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 <iostream>
#include <gtsam/inference/VariableIndex.h>
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 <iostream>
#include <gtsam/inference/VariableIndex.h>
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;
}
}

360
gtsam/inference/VariableIndex.h
View File

@ -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 <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <gtsam/inference/Key.h>
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<VariableIndex> shared_ptr;
typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected:
typedef FastMap<Key,Factors> 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<class FG>
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<class FG>
void augment(const FG& factors, boost::optional<const FastVector<size_t>&> 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<typename ITERATOR, class FG>
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
/** Remove unused empty variables (in debug mode verifies they are empty). */
template<typename ITERATOR>
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/inference/VariableIndex-inl.h>
/* ----------------------------------------------------------------------------
* 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 <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <gtsam/inference/Key.h>
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<VariableIndex> shared_ptr;
typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected:
typedef FastMap<Key,Factors> 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<class FG>
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<class FG>
void augment(const FG& factors, boost::optional<const FastVector<size_t>&> 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<typename ITERATOR, class FG>
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
/** Remove unused empty variables (in debug mode verifies they are empty). */
template<typename ITERATOR>
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/inference/VariableIndex-inl.h>

2
gtsam/linear/GaussianDensity.cpp
View File

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

656
gtsam/linear/JacobianFactor.h
View File

@ -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 <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/global_includes.h>
#include <boost/make_shared.hpp>
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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
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<This> 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<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
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<typename KEYS>
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<const Ordering&> ordering = boost::none,
boost::optional<const VariableSlots&> variableSlots = boost::none);
/** Virtual destructor */
virtual ~JacobianFactor() {}
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<JacobianFactor>(*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<Matrix, Vector> jacobian() const;
/**
* @brief Returns (dense) A,b pair associated with factor, does not bake in weights
*/
std::pair<Matrix, Vector> 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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
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<GaussianConditional> splitConditional(size_t nrFrontals);
protected:
/// Internal function to fill blocks and set dimensions
template<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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/linear/JacobianFactor-inl.h>
/* ----------------------------------------------------------------------------
* 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 <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/global_includes.h>
#include <boost/make_shared.hpp>
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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
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<This> 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<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
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<typename KEYS>
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<const Ordering&> ordering = boost::none,
boost::optional<const VariableSlots&> variableSlots = boost::none);
/** Virtual destructor */
virtual ~JacobianFactor() {}
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<JacobianFactor>(*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<Matrix, Vector> jacobian() const;
/**
* @brief Returns (dense) A,b pair associated with factor, does not bake in weights
*/
std::pair<Matrix, Vector> 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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
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<GaussianConditional> splitConditional(size_t nrFrontals);
protected:
/// Internal function to fill blocks and set dimensions
template<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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/linear/JacobianFactor-inl.h>

4
gtsam/linear/NoiseModel.h
View File

@ -719,8 +719,8 @@ namespace gtsam {
};
/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
/// Dipl.-Inform. Jan Oberl<72>nder (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany.
/// Dipl.-Inform. Jan Oberl<72>nder (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany.
/// oberlaender@fzi.de
/// Thanks Jan!
class GTSAM_EXPORT Cauchy : public Base {

16
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -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<JacobianFactor>(gf);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*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;
}
/* ************************************************************************* */

694
gtsam/linear/VectorValues.cpp
View File

@ -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 <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
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<Key>::operator(), less<Key>(), 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<Key, size_t> 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<value_type, value_type> 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<Key>& keys) const
{
// Count dimensions and collect pointers to avoid double lookups
DenseIndex totalDim = 0;
FastVector<const Vector*> 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<VectorValues::value_type,
VectorValues::value_type>& 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<bool>());
}
/* ************************************************************************* */
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<value_type, value_type> 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<VectorValues::iterator, bool> 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 <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>
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<Key>::operator(), less<Key>(), 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<Key, size_t> 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<value_type, value_type> 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<Key>& keys) const
{
// Count dimensions and collect pointers to avoid double lookups
DenseIndex totalDim = 0;
FastVector<const Vector*> 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<VectorValues::value_type,
VectorValues::value_type>& 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<bool>());
}
/* ************************************************************************* */
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<value_type, value_type> 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<VectorValues::iterator, bool> 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

768
gtsam/linear/VectorValues.h
View File

@ -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 <gtsam/base/Vector.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h>
#include <boost/shared_ptr.hpp>
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
*
* <h2>Advanced Interface and Performance Information</h2>
*
* 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<Key, Vector> 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<This> shared_ptr; ///< shared_ptr to this class
typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
typedef value_type KeyValuePair; ///< Typedef to pair<Key, Vector>, a key-value pair
typedef std::map<Key,size_t> 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<Key,Vector>. */
template<class CONTAINER>
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<Key,Vector>. */
template<typename ITERATOR>
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, Vector>& 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<iterator, bool> 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<iterator, bool> 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<Key>& 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<class ARCHIVE>
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 <gtsam/base/Vector.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h>
#include <boost/shared_ptr.hpp>
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
*
* <h2>Advanced Interface and Performance Information</h2>
*
* 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<Key, Vector> 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<This> shared_ptr; ///< shared_ptr to this class
typedef Values::value_type value_type; ///< Typedef to pair<Key, Vector>, a key-value pair
typedef value_type KeyValuePair; ///< Typedef to pair<Key, Vector>, a key-value pair
typedef std::map<Key,size_t> 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<Key,Vector>. */
template<class CONTAINER>
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<Key,Vector>. */
template<typename ITERATOR>
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, Vector>& 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<iterator, bool> 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<iterator, bool> 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<Key>& 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
}; // VectorValues definition
} // \namespace gtsam

8
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -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<JacobianFactor>(
marginal->keys().front(),
marginal->getA(marginal->begin()),
marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
newPrior = boost::make_shared<JacobianFactor>(
marginal->keys().front(),
marginal->getA(marginal->begin()),
marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
marginal->get_model());
return x;

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

@ -166,39 +166,39 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& 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<VectorValues::const_iterator> 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<VectorValues::const_iterator> 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<Eigen::Upper>().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<Eigen::Upper>().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

4
gtsam/nonlinear/ISAM2.h
View File

@ -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<Key>& leafKeys,
boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
void marginalizeLeaves(const FastList<Key>& leafKeys,
boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none);
/** Access the current linearization point */

1716
tests/testGaussianISAM2.cpp
View File

File diff suppressed because it is too large Load Diff

2
wrap/Constructor.cpp
View File

@ -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<SharedBase**>(mxGetData(out[1])) = new SharedBase(*self);\n";
}

852
wrap/matlab.h
View File

@ -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 <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
using gtsam::Vector;
using gtsam::Matrix;
extern "C" {
#include <mex.h>
}
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <list>
#include <string>
#include <sstream>
#include <typeinfo>
#include <set>
#include <streambuf>
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 <typename Class>
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<string>(const string& value) {
return mxCreateString(value.c_str());
}
// specialization to char
template<>
mxArray* wrap<char>(const char& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(char*)mxGetData(result) = value;
return result;
}
// specialization to unsigned char
template<>
mxArray* wrap<unsigned char>(const unsigned char& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(unsigned char*)mxGetData(result) = value;
return result;
}
// specialization to bool
template<>
mxArray* wrap<bool>(const bool& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(bool*)mxGetData(result) = value;
return result;
}
// specialization to size_t
template<>
mxArray* wrap<size_t>(const size_t& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(size_t*)mxGetData(result) = value;
return result;
}
// specialization to int
template<>
mxArray* wrap<int>(const int& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(int*)mxGetData(result) = value;
return result;
}
// specialization to double -> just double
template<>
mxArray* wrap<double>(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<m;i++) data[i]=v(i);
return result;
}
// specialization to Eigen vector -> double vector
template<>
mxArray* wrap<gtsam::Vector >(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<n;j++) for (int i=0;i<m;i++,data++) *data = A(i,j);
return result;
}
// specialization to Eigen MATRIX -> double matrix
template<>
mxArray* wrap<gtsam::Matrix >(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 <typename T>
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<string>(const mxArray* array) {
char *data = mxArrayToString(array);
if (data==NULL) error("unwrap<string>: 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 <typename T>
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<bool>(const mxArray* array) {
checkScalar(array,"unwrap<bool>");
return myGetScalar<bool>(array);
}
// specialization to char
template<>
char unwrap<char>(const mxArray* array) {
checkScalar(array,"unwrap<char>");
return myGetScalar<char>(array);
}
// specialization to unsigned char
template<>
unsigned char unwrap<unsigned char>(const mxArray* array) {
checkScalar(array,"unwrap<unsigned char>");
return myGetScalar<unsigned char>(array);
}
// specialization to int
template<>
int unwrap<int>(const mxArray* array) {
checkScalar(array,"unwrap<int>");
return myGetScalar<int>(array);
}
// specialization to size_t
template<>
size_t unwrap<size_t>(const mxArray* array) {
checkScalar(array, "unwrap<size_t>");
return myGetScalar<size_t>(array);
}
// specialization to double
template<>
double unwrap<double>(const mxArray* array) {
checkScalar(array,"unwrap<double>");
return myGetScalar<double>(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<vector>: 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<m;i++,data++) v(i) = *data;
#ifdef DEBUG_WRAP
gtsam::print(v);
#endif
return v;
}
// specialization to Eigen matrix
template<>
gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
if (mxIsDouble(array)==false) error("unwrap<matrix>: 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<n;j++) for (int i=0;i<m;i++,data++) A(i,j) = *data;
#ifdef DEBUG_WRAP
gtsam::print(A);
#endif
return A;
}
/*
[create_object] creates a MATLAB proxy class object with a mexhandle
in the self property. Matlab does not allow the creation of matlab
objects from within mex files, hence we resort to an ugly trick: we
invoke the proxy class constructor by calling MATLAB with a special
uint64 value ptr_constructor_key and the pointer itself. MATLAB
allocates the object. Then, the special constructor in our wrap code
that is activated when the ptr_constructor_key is passed in passes
the pointer back into a C++ function to add the pointer to its
collector. We go through this extra "C++ to MATLAB to C++ step" in
order to be able to add to the collector could be in a different wrap
module.
*/
mxArray* create_object(const std::string& classname, void *pointer, bool isVirtual, const char *rttiName) {
mxArray *result;
mxArray *input[3];
int nargin = 2;
// First input argument is pointer constructor key
input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL);
*reinterpret_cast<uint64_t*>(mxGetData(input[0])) = ptr_constructor_key;
// Second input argument is the pointer
input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
*reinterpret_cast<void**>(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 <typename Class>
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> void_ptr(shared_ptr);
result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name());
} else {
boost::shared_ptr<Class> *heapPtr = new boost::shared_ptr<Class>(shared_ptr);
result = create_object(matlabName, heapPtr, isVirtual, "");
}
return result;
}
template <typename Class>
boost::shared_ptr<Class> 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<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
return *spp;
}
*
* wrapping and unwrapping is done using specialized templates, see
* http://www.cplusplus.com/doc/tutorial/templates.html
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
using gtsam::Vector;
using gtsam::Matrix;
extern "C" {
#include <mex.h>
}
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <list>
#include <string>
#include <sstream>
#include <typeinfo>
#include <set>
#include <streambuf>
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 <typename Class>
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<string>(const string& value) {
return mxCreateString(value.c_str());
}
// specialization to char
template<>
mxArray* wrap<char>(const char& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(char*)mxGetData(result) = value;
return result;
}
// specialization to unsigned char
template<>
mxArray* wrap<unsigned char>(const unsigned char& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(unsigned char*)mxGetData(result) = value;
return result;
}
// specialization to bool
template<>
mxArray* wrap<bool>(const bool& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(bool*)mxGetData(result) = value;
return result;
}
// specialization to size_t
template<>
mxArray* wrap<size_t>(const size_t& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(size_t*)mxGetData(result) = value;
return result;
}
// specialization to int
template<>
mxArray* wrap<int>(const int& value) {
mxArray *result = scalar(mxUINT32OR64_CLASS);
*(int*)mxGetData(result) = value;
return result;
}
// specialization to double -> just double
template<>
mxArray* wrap<double>(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<m;i++) data[i]=v(i);
return result;
}
// specialization to Eigen vector -> double vector
template<>
mxArray* wrap<gtsam::Vector >(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<n;j++) for (int i=0;i<m;i++,data++) *data = A(i,j);
return result;
}
// specialization to Eigen MATRIX -> double matrix
template<>
mxArray* wrap<gtsam::Matrix >(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 <typename T>
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<string>(const mxArray* array) {
char *data = mxArrayToString(array);
if (data==NULL) error("unwrap<string>: 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 <typename T>
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<bool>(const mxArray* array) {
checkScalar(array,"unwrap<bool>");
return myGetScalar<bool>(array);
}
// specialization to char
template<>
char unwrap<char>(const mxArray* array) {
checkScalar(array,"unwrap<char>");
return myGetScalar<char>(array);
}
// specialization to unsigned char
template<>
unsigned char unwrap<unsigned char>(const mxArray* array) {
checkScalar(array,"unwrap<unsigned char>");
return myGetScalar<unsigned char>(array);
}
// specialization to int
template<>
int unwrap<int>(const mxArray* array) {
checkScalar(array,"unwrap<int>");
return myGetScalar<int>(array);
}
// specialization to size_t
template<>
size_t unwrap<size_t>(const mxArray* array) {
checkScalar(array, "unwrap<size_t>");
return myGetScalar<size_t>(array);
}
// specialization to double
template<>
double unwrap<double>(const mxArray* array) {
checkScalar(array,"unwrap<double>");
return myGetScalar<double>(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<vector>: 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<m;i++,data++) v(i) = *data;
#ifdef DEBUG_WRAP
gtsam::print(v);
#endif
return v;
}
// specialization to Eigen matrix
template<>
gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) {
if (mxIsDouble(array)==false) error("unwrap<matrix>: 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<n;j++) for (int i=0;i<m;i++,data++) A(i,j) = *data;
#ifdef DEBUG_WRAP
gtsam::print(A);
#endif
return A;
}
/*
[create_object] creates a MATLAB proxy class object with a mexhandle
in the self property. Matlab does not allow the creation of matlab
objects from within mex files, hence we resort to an ugly trick: we
invoke the proxy class constructor by calling MATLAB with a special
uint64 value ptr_constructor_key and the pointer itself. MATLAB
allocates the object. Then, the special constructor in our wrap code
that is activated when the ptr_constructor_key is passed in passes
the pointer back into a C++ function to add the pointer to its
collector. We go through this extra "C++ to MATLAB to C++ step" in
order to be able to add to the collector could be in a different wrap
module.
*/
mxArray* create_object(const std::string& classname, void *pointer, bool isVirtual, const char *rttiName) {
mxArray *result;
mxArray *input[3];
int nargin = 2;
// First input argument is pointer constructor key
input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL);
*reinterpret_cast<uint64_t*>(mxGetData(input[0])) = ptr_constructor_key;
// Second input argument is the pointer
input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
*reinterpret_cast<void**>(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 <typename Class>
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> void_ptr(shared_ptr);
result = create_object(matlabName, &void_ptr, isVirtual, typeid(*shared_ptr).name());
} else {
boost::shared_ptr<Class> *heapPtr = new boost::shared_ptr<Class>(shared_ptr);
result = create_object(matlabName, heapPtr, isVirtual, "");
}
return result;
}
template <typename Class>
boost::shared_ptr<Class> 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<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
return *spp;
}

4
wrap/utilities.h
View File

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