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

Merged develop into feature/partition

release/4.3a0
Andrew Melim 2014-01-28 18:35:46 -05:00
parent 9ec9f8c0d4 fe55148dd7
commit a6ff1eb9ec
112 changed files with 5831 additions and 6592 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,2 +1,3 @@
/build*
*.pyc
*.DS_Store

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

14
gtsam/base/LieMatrix.cpp
View File

@ -19,20 +19,6 @@
namespace gtsam {
/* ************************************************************************* */
LieMatrix::LieMatrix(size_t m, size_t n, ...)
: Matrix(m,n) {
va_list ap;
va_start(ap, n);
for(size_t i = 0; i < m; ++i) {
for(size_t j = 0; j < n; ++j) {
double value = va_arg(ap, double);
(*this)(i,j) = value;
}
}
va_end(ap);
}
/* ************************************************************************* */
void LieMatrix::print(const std::string& name) const {
gtsam::print(matrix(), name);

3
gtsam/base/LieMatrix.h
View File

@ -48,9 +48,6 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
LieMatrix(size_t m, size_t n, const double* const data) :
Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
/** Specify arguments directly, as in Matrix_() - always force these to be doubles */
GTSAM_EXPORT LieMatrix(size_t m, size_t n, ...);
/// @}
/// @name Testable interface
/// @{

15
gtsam/base/LieVector.cpp
View File

@ -24,21 +24,10 @@ namespace gtsam {
/* ************************************************************************* */
LieVector::LieVector(size_t m, const double* const data)
: Vector(Vector_(m,data))
{
}
/* ************************************************************************* */
LieVector::LieVector(size_t m, ...)
: Vector(m)
{
va_list ap;
va_start(ap, m);
for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
for(size_t i = 0; i < m; i++)
(*this)(i) = data[i];
}
/* ************************************************************************* */

3
gtsam/base/LieVector.h
View File

@ -44,9 +44,6 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);
/** Specify arguments directly, as in Vector_() - always force these to be doubles */
GTSAM_EXPORT LieVector(size_t m, ...);
/** get the underlying vector */
Vector vector() const {
return static_cast<Vector>(*this);

43
gtsam/base/Matrix.cpp
View File

@ -36,38 +36,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Matrix Matrix_( size_t m, size_t n, const double* const data) {
MatrixRowMajor A(m,n);
copy(data, data+m*n, A.data());
return Matrix(A);
}
/* ************************************************************************* */
Matrix Matrix_( size_t m, size_t n, const Vector& v)
{
Matrix A(m,n);
// column-wise copy
for( size_t j = 0, k=0 ; j < n ; j++)
for( size_t i = 0; i < m ; i++,k++)
A(i,j) = v(k);
return A;
}
/* ************************************************************************* */
Matrix Matrix_(size_t m, size_t n, ...) {
Matrix A(m,n);
va_list ap;
va_start(ap, n);
for( size_t i = 0 ; i < m ; i++)
for( size_t j = 0 ; j < n ; j++) {
double value = va_arg(ap, double);
A(i,j) = value;
}
va_end(ap);
return A;
}
/* ************************************************************************* */
Matrix zeros( size_t m, size_t n ) {
return Matrix::Zero(m,n);
@ -211,17 +179,6 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
x += alpha * A.transpose() * e;
}
/* ************************************************************************* */
Vector Vector_(const Matrix& A)
{
size_t m = A.rows(), n = A.cols();
Vector v(m*n);
for( size_t j = 0, k=0 ; j < n ; j++)
for( size_t i = 0; i < m ; i++,k++)
v(k) = A(i,j);
return v;
}
/* ************************************************************************* */
void print(const Matrix& A, const string &s, ostream& stream) {
size_t m = A.rows(), n = A.cols();

26
gtsam/base/Matrix.h
View File

@ -45,27 +45,6 @@ typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
/**
* constructor with size and initial data, row order !
*/
GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const double* const data);
/**
* constructor with size and vector data, column order !!!
*/
GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const Vector& v);
/**
* constructor from Vector yielding v.size()*1 vector
*/
inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);}
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*/
GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...);
// Matlab-like syntax
/**
@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
return result;
}
/**
* convert to column vector, column order !!!
*/
GTSAM_EXPORT Vector Vector_(const Matrix& A);
/**
* print a matrix
*/

27
gtsam/base/Vector.cpp
View File

@ -79,33 +79,6 @@ void odprintf(const char *format, ...) {
//#endif
}
/* ************************************************************************* */
Vector Vector_( size_t m, const double* const data) {
Vector A(m);
copy(data, data+m, A.data());
return A;
}
/* ************************************************************************* */
Vector Vector_(size_t m, ...) {
Vector v(m);
va_list ap;
va_start(ap, m);
for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double);
v(i) = value;
}
va_end(ap);
return v;
}
/* ************************************************************************* */
Vector Vector_(const std::vector<double>& d) {
Vector v(d.size());
copy(d.begin(), d.end(), v.data());
return v;
}
/* ************************************************************************* */
bool zero(const Vector& v) {
bool result = true;

16
gtsam/base/Vector.h
View File

@ -46,22 +46,6 @@ typedef Eigen::VectorBlock<const Vector> ConstSubVector;
*/
GTSAM_EXPORT void odprintf(const char *format, ...);
/**
* constructor with size and initial data, row order !
*/
GTSAM_EXPORT Vector Vector_( size_t m, const double* const data);
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*/
GTSAM_EXPORT Vector Vector_(size_t m, ...);
/**
* Create a numeric vector from an STL vector of doubles
*/
GTSAM_EXPORT Vector Vector_(const std::vector<double>& data);
/**
* Create vector initialized to a constant value
* @param n is the size of the vector

2
gtsam/base/numericalDerivative.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
/** global functions for converting to a LieVector for use with numericalDerivative */
inline LieVector makeLieVector(const Vector& v) { return LieVector(v); }
inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); }
inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); }
/**
* Numerically compute gradient of scalar function

11
gtsam/base/tests/testLieMatrix.cpp
View File

@ -39,20 +39,17 @@ TEST( LieMatrix, construction ) {
TEST( LieMatrix, other_constructors ) {
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0);
LieMatrix exp(init);
LieMatrix a(2,2,10.0,20.0,30.0,40.0);
double data[] = {10,30,20,40};
LieMatrix b(2,2,data);
EXPECT(assert_equal(exp, a));
EXPECT(assert_equal(exp, b));
EXPECT(assert_equal(b, a));
}
/* ************************************************************************* */
TEST(LieMatrix, retract) {
LieMatrix init(2,2, 1.0,2.0,3.0,4.0);
Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0);
LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0));
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0);
LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0);
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0));
LieMatrix actual = init.retract(update);
EXPECT(assert_equal(expected, actual));
@ -63,7 +60,7 @@ TEST(LieMatrix, retract) {
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4);
Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0));
Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}

2
gtsam/base/tests/testLieScalar.cpp
View File

@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) {
TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.);
EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2)));
EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2)));
}
/* ************************************************************************* */

5
gtsam/base/tests/testLieVector.cpp
View File

@ -39,14 +39,9 @@ TEST( testLieVector, construction ) {
TEST( testLieVector, other_constructors ) {
Vector init = (Vector(2) << 10.0, 20.0);
LieVector exp(init);
LieVector a(2,10.0,20.0);
double data[] = {10,20};
LieVector b(2,data);
LieVector c(2.3), c_exp(LieVector(1, 2.3));
EXPECT(assert_equal(exp, a));
EXPECT(assert_equal(exp, b));
EXPECT(assert_equal(b, a));
EXPECT(assert_equal(c_exp, c));
}
/* ************************************************************************* */

12
gtsam/base/tests/testMatrix.cpp
View File

@ -43,18 +43,6 @@ TEST( matrix, constructor_data )
EQUALITY(A,B);
}
/* ************************************************************************* */
/*
TEST( matrix, constructor_vector )
{
double data[] = { -5, 3, 0, -5 };
Matrix A = Matrix_(2, 2, -5, 3, 0, -5);
Vector v(4);
copy(data, data + 4, v.data());
Matrix B = Matrix_(2, 2, v); // this one is column order !
EQUALITY(A,trans(B));
}
*/
/* ************************************************************************* */
TEST( matrix, Matrix_ )
{

13
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -48,7 +48,8 @@ double f2(const LieVector& x) {
/* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian2) {
LieVector center(2, 0.5, 1.0);
Vector v_center = (Vector(2) << 0.5, 1.0);
LieVector center(v_center);
Matrix expected = (Matrix(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
@ -67,7 +68,9 @@ double f3(const LieVector& x1, const LieVector& x2) {
/* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian211) {
LieVector center1(1, 1.0), center2(1, 5.0);
Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 5.0);
LieVector center1(v_center1), center2(v_center2);
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2);
@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
/* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian311) {
LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0);
Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 2.0);
Vector v_center3 = (Vector(1) << 3.0);
LieVector center1(v_center1), center2(v_center2), center3(v_center3);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);

9
gtsam/base/tests/testVector.cpp
View File

@ -23,15 +23,6 @@
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( TestVector, Vector_variants )
{
Vector a = (Vector(2) << 10.0,20.0);
double data[] = {10,20};
Vector b = Vector_(2, data);
EXPECT(assert_equal(a, b));
}
namespace {
/* ************************************************************************* */
template<typename Derived>

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

152
gtsam/geometry/EssentialMatrix.cpp Normal file
View File

@ -0,0 +1,152 @@
/*
* @file EssentialMatrix.cpp
* @brief EssentialMatrix class
* @author Frank Dellaert
* @date December 5, 2014
*/
#include <gtsam/geometry/EssentialMatrix.h>
#include <iostream>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
if (!H) {
// just make a direction out of translation and create E
Sphere2 direction(_1T2_);
return EssentialMatrix(_1R2_, direction);
} else {
// Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Sphere2::FromPoint3
Matrix D_direction_1T2;
Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2);
H->resize(5, 6);
H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
return EssentialMatrix(_1R2_, direction);
}
}
/* ************************************************************************* */
void EssentialMatrix::print(const string& s) const {
cout << s;
aRb_.print("R:\n");
aTb_.print("d: ");
}
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
assert(xi.size() == 5);
Vector3 omega(sub(xi, 0, 3));
Vector2 z(sub(xi, 3, 5));
Rot3 R = aRb_.retract(omega);
Sphere2 t = aTb_.retract(z);
return EssentialMatrix(R, t);
}
/* ************************************************************************* */
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
return Vector(5) << //
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
}
/* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p,
boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3());
Point3 q = pose.transform_to(p, DE, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix H(3, 5);
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
}
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
if (!HE && !HR) {
// Rotate translation direction and return
Sphere2 c1Tc2 = cRb * aTb_;
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) {
*HE = zeros(5, 5);
HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
}
if (HR) {
throw runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet");
/*
HR->resize(5, 3);
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
*/
}
return EssentialMatrix(c1Rc2, c1Tc2);
}
}
/* ************************************************************************* */
double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H) const {
if (H) {
H->resize(1, 5);
// See math.lyx
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
*H << HR, HD;
}
return dot(vA, E_ * vB);
}
/* ************************************************************************* */
ostream& operator <<(ostream& os, const EssentialMatrix& E) {
Rot3 R = E.rotation();
Sphere2 d = E.direction();
os.precision(10);
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
return os;
}
/* ************************************************************************* */
istream& operator >>(istream& is, EssentialMatrix& E) {
double rx, ry, rz, dx, dy, dz;
is >> rx >> ry >> rz; // Read the rotation rxyz
is >> dx >> dy >> dz; // Read the translation dxyz
// Create EssentialMatrix from rotation and translation
Rot3 rot = Rot3::RzRyRx(rx, ry, rz);
Sphere2 dt = Sphere2(dx, dy, dz);
E = EssentialMatrix(rot, dt);
return is;
}
/* ************************************************************************* */
} // gtsam

101
gtsam/geometry/EssentialMatrix.h
View File

@ -26,7 +26,7 @@ private:
Rot3 aRb_; ///< Rotation between a and b
Sphere2 aTb_; ///< translation direction from a to b
Matrix E_; ///< Essential matrix
Matrix3 E_; ///< Essential matrix
public:
@ -48,6 +48,10 @@ public:
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
}
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H = boost::none);
/// Random, using Rot3::Random and Sphere2::Random
template<typename Engine>
static EssentialMatrix Random(Engine & rng) {
@ -60,11 +64,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const {
std::cout << s;
aRb_.print("R:\n");
aTb_.print("d: ");
}
void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
@ -87,20 +87,10 @@ public:
}
/// Retract delta to manifold
virtual EssentialMatrix retract(const Vector& xi) const {
assert(xi.size() == 5);
Vector3 omega(sub(xi, 0, 3));
Vector2 z(sub(xi, 3, 5));
Rot3 R = aRb_.retract(omega);
Sphere2 t = aTb_.retract(z);
return EssentialMatrix(R, t);
}
virtual EssentialMatrix retract(const Vector& xi) const;
/// Compute the coordinates in the tangent space
virtual Vector localCoordinates(const EssentialMatrix& other) const {
return Vector(5) << //
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
}
virtual Vector localCoordinates(const EssentialMatrix& other) const;
/// @}
@ -108,17 +98,17 @@ public:
/// @{
/// Rotation
const Rot3& rotation() const {
inline const Rot3& rotation() const {
return aRb_;
}
/// Direction
const Sphere2& direction() const {
inline const Sphere2& direction() const {
return aTb_;
}
/// Return 3*3 matrix representation
const Matrix& matrix() const {
inline const Matrix3& matrix() const {
return E_;
}
@ -131,20 +121,7 @@ public:
*/
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
Pose3 pose(aRb_, aTb_.point3());
Point3 q = pose.transform_to(p, DE, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix H(3, 5);
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
}
boost::optional<Matrix&> Dpoint = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
@ -152,36 +129,7 @@ public:
* @param E essential matrix E in camera frame C
*/
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
boost::none, boost::optional<Matrix&> HR = boost::none) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
if (!HE && !HR) {
// Rotate translation direction and return
Sphere2 c1Tc2 = cRb * aTb_;
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) {
*HE = zeros(5, 5);
HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
}
if (HR) {
throw std::runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet");
/*
HR->resize(5, 3);
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
*/
}
return EssentialMatrix(c1Rc2, c1Tc2);
}
}
boost::none, boost::optional<Matrix&> HR = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
@ -194,17 +142,18 @@ public:
/// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const {
if (H) {
H->resize(1, 5);
// See math.lyx
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
*H << HR, HD;
}
return dot(vA, E_ * vB);
}
boost::optional<Matrix&> H = boost::none) const;
/// @}
/// @name Streaming operators
/// @{
/// stream to stream
friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
/// stream from stream
friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
/// @}

2
gtsam/geometry/Point2.h
View File

@ -172,7 +172,7 @@ public:
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); }
/// @}
/// @name Vector Space

54
gtsam/geometry/Point3.cpp
View File

@ -26,7 +26,8 @@ INSTANTIATE_LIE(Point3);
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
&& fabs(z_ - q.z()) < tol);
}
/* ************************************************************************* */
@ -37,18 +38,18 @@ void Point3::print(const string& s) const {
/* ************************************************************************* */
bool Point3::operator== (const Point3& q) const {
bool Point3::operator==(const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
Point3 Point3::operator-(const Point3& q) const {
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
}
/* ************************************************************************* */
@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const {
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = eye(3, 3);
if (H2)
*H2 = eye(3, 3);
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = eye(3, 3);
if (H2)
*H2 = -eye(3, 3);
return *this - q;
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_,
z_*q.x_ - x_*q.z_,
x_*q.y_ - y_*q.x_ );
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
}
/* ************************************************************************* */
double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ );
return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
}
/* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> H) const {
Point3 normalized = *this / norm();
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
H->resize(3, 3);
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}
return normalized;
}
/* ************************************************************************* */

3
gtsam/geometry/Point3.h
View File

@ -176,6 +176,9 @@ namespace gtsam {
/** Distance of the point from the origin */
double norm() const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;

2
gtsam/geometry/Pose3.cpp
View File

@ -98,7 +98,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia
static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
static const int N = 5; // order of approximation
Matrix res = I6;

2
gtsam/geometry/Rot2.h
View File

@ -170,7 +170,7 @@ namespace gtsam {
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) {
return Vector_(1, r.theta());
return (Vector(1) << r.theta());
}
/// @}

407
gtsam/geometry/Rot3.cpp
View File

@ -1,198 +1,209 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Rot3.cpp
* @brief Rotation, common code between Rotation matrix and Quaternion
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <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::unrotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = unrotate(p.point3(Hp));
if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
if (HR)
(*HR) = q.basis().transpose() * q.skew();
return q;
}
/* ************************************************************************* */
Sphere2 Rot3::operator*(const Sphere2& p) const {
return rotate(p);
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<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

6
gtsam/geometry/Rot3.h
View File

@ -194,7 +194,7 @@ namespace gtsam {
* @return incremental rotation matrix
*/
static Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
{ return rodriguez((Vector(3) << wx, wy, wz));}
/// @}
/// @name Testable
@ -331,6 +331,10 @@ namespace gtsam {
Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
/// rotate 3D direction from rotated coordinate frame to world frame
Sphere2 operator*(const Sphere2& p) const;

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

102
gtsam/geometry/Sphere2.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief The Sphere2 class - basically a point on a unit sphere
*/
@ -27,6 +28,21 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Sphere2 Sphere2::FromPoint3(const Point3& point,
boost::optional<Matrix&> H) {
Sphere2 direction(point);
if (H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix D_p_point;
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
// Calculate the 2*3 Jacobian
H->resize(2, 3);
*H << direction.basis().transpose() * D_p_point;
}
return direction;
}
/* ************************************************************************* */
Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
@ -98,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
}
/* ************************************************************************* */
Sphere2 Sphere2::retract(const Vector& v) const {
Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
// Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_);
@ -106,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const {
// Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector newPoint = p + xi_hat;
if (mode == Sphere2::EXPMAP) {
double xi_hat_norm = xi_hat.norm();
// Project onto the manifold, i.e. the closest point on the circle to the new location;
// same as putting it onto the unit circle
Vector projected = newPoint / newPoint.norm();
// Avoid nan
if (xi_hat_norm == 0.0) {
if (v.norm () == 0.0)
return Sphere2 (point3 ());
else
return Sphere2 (-point3 ());
}
Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
return Sphere2(exp_p_xi_hat);
} else if (mode == Sphere2::RENORM) {
// Project onto the manifold, i.e. the closest point on the circle to the new location;
// same as putting it onto the unit circle
Vector newPoint = p + xi_hat;
Vector projected = newPoint / newPoint.norm();
return Sphere2(Point3::Expmap(projected));
return Sphere2(Point3::Expmap(projected));
} else {
assert (false);
exit (1);
}
}
/* ************************************************************************* */
Vector Sphere2::localCoordinates(const Sphere2& y) const {
Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
// Make sure that the angle different between x and y is less than 90. Otherwise,
// we can project x + xi_hat from the tangent space at x to y.
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
if (mode == Sphere2::EXPMAP) {
Matrix B = basis();
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
double theta = acos(p.transpose() * q);
// Get the basis matrix
Matrix B = basis();
// Create the vector forms of p and q (the Point3 of y).
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
double alpha = p.transpose() * q;
assert(alpha != 0.0);
Matrix coeffs = (B.transpose() * q) / alpha;
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
return result;
// the below will be nan if theta == 0.0
if (p == q)
return (Vector (2) << 0, 0);
else if (p == (Vector)-q)
return (Vector (2) << M_PI, 0);
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
Vector result = B.transpose() * result_hat;
return result;
} else if (mode == Sphere2::RENORM) {
// Make sure that the angle different between x and y is less than 90. Otherwise,
// we can project x + xi_hat from the tangent space at x to y.
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
// Get the basis matrix
Matrix B = basis();
// Create the vector forms of p and q (the Point3 of y).
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
double alpha = p.transpose() * q;
assert(alpha != 0.0);
Matrix coeffs = (B.transpose() * q) / alpha;
Vector result = (Vector(2) << coeffs(0, 0), coeffs(1, 0));
return result;
} else {
assert (false);
exit (1);
}
}
/* ************************************************************************* */

31
gtsam/geometry/Sphere2.h
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Develop a Sphere2 class - basically a point on a unit sphere
*/
@ -22,6 +23,10 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
#define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
#endif
// (Cumbersome) forward declaration for random generator
namespace boost {
namespace random {
@ -65,6 +70,10 @@ public:
p_ = p_ / p_.norm();
}
/// Named constructor from Point3 with optional Jacobian
static Sphere2 FromPoint3(const Point3& point,
boost::optional<Matrix&> H = boost::none);
/// Random direction, using boost::uniform_on_sphere
static Sphere2 Random(boost::random::mt19937 & rng);
@ -85,7 +94,11 @@ public:
/// @name Other functionality
/// @{
/// Returns the local coordinate frame to tangent plane
/**
* Returns the local coordinate frame to tangent plane
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
* tangent to the sphere at the current direction.
*/
Matrix basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere
@ -98,6 +111,13 @@ public:
return p_;
}
/// Return unit-norm Vector
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return (p_.vector ());
}
/// Signed, vector-valued error between two directions
Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const;
@ -121,11 +141,16 @@ public:
return 2;
}
enum CoordinatesMode {
EXPMAP, ///< Use the exponential map to retract
RENORM ///< Retract with vector addtion and renormalize.
};
/// The retract function
Sphere2 retract(const Vector& v) const;
Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
/// The local coordinates function
Vector localCoordinates(const Sphere2& s) const;
Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
/// @}
};

2
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate)
double g = 1+k[0]*r+k[1]*r*r ;
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ;
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ;
Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
Point2 q = K.uncalibrate(p);

33
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -10,6 +10,7 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <sstream>
using namespace std;
using namespace gtsam;
@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) {
// Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
}
//*************************************************************************
TEST (EssentialMatrix, FromPose3_a) {
Matrix actualH;
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
//*************************************************************************
TEST (EssentialMatrix, FromPose3_b) {
Matrix actualH;
Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
Point3 c1Tc2(0.4, 0.5, 0.6);
EssentialMatrix trueE(c1Rc2, c1Tc2);
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
//*************************************************************************
TEST (EssentialMatrix, streaming) {
EssentialMatrix expected(c1Rc2, c1Tc2), actual;
stringstream ss;
ss << expected;
ss >> actual;
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;

71
gtsam/geometry/tests/testPoint3.cpp
View File

@ -12,75 +12,86 @@
/**
* @file testPoint3.cpp
* @brief Unit tests for Point3 class
*/
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
GTSAM_CONCEPT_LIE_INST(Point3)
static Point3 P(0.2,0.7,-2);
static Point3 P(0.2, 0.7, -2);
/* ************************************************************************* */
TEST(Point3, Lie) {
Point3 p1(1,2,3);
Point3 p2(4,5,6);
Point3 p1(1, 2, 3);
Point3 p2(4, 5, 6);
Matrix H1, H2;
EXPECT(assert_equal(Point3(5,7,9), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(3,3,3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */
TEST( Point3, arithmetic)
{
CHECK(P*3==3*P);
CHECK(assert_equal( Point3(-1,-5,-6), -Point3(1,5,6) ));
CHECK(assert_equal( Point3(2,5,6), Point3(1,4,5)+Point3(1,1,1)));
CHECK(assert_equal( Point3(0,3,4), Point3(1,4,5)-Point3(1,1,1)));
CHECK(assert_equal( Point3(2,8,6), Point3(1,4,3)*2));
CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3)));
CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2));
TEST( Point3, arithmetic) {
CHECK(P * 3 == 3 * P);
CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
}
/* ************************************************************************* */
TEST( Point3, equals)
{
TEST( Point3, equals) {
CHECK(P.equals(P));
Point3 Q;
CHECK(!P.equals(Q));
}
/* ************************************************************************* */
TEST( Point3, dot)
{
Point3 origin, ones(1,1,1);
CHECK(origin.dot(Point3(1,1,0)) == 0);
CHECK(ones.dot(Point3(1,1,0)) == 2);
TEST( Point3, dot) {
Point3 origin, ones(1, 1, 1);
CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(ones.dot(Point3(1, 1, 0)) == 2);
}
/* ************************************************************************* */
TEST( Point3, stream)
{
Point3 p(1,2, -3);
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
os << p;
EXPECT(os.str() == "[1, 2, -3]';");
}
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;
Point3 point(1, -2, 3); // arbitrary point
Point3 expected(point / sqrt(14));
EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(&Point3::normalize, _1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testRot2.cpp
View File

@ -96,7 +96,7 @@ TEST(Rot2, logmap)
{
Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
Rot2 rot(Rot2::fromAngle(M_PI));
Vector expected = Vector_(1, M_PI/2.0);
Vector expected = (Vector(1) << M_PI/2.0);
Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual));
}

105
gtsam/geometry/tests/testSphere2.cpp
View File

@ -13,6 +13,8 @@
* @file testSphere2.cpp
* @date Feb 03, 2012
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Tests the Sphere2 class
*/
@ -25,6 +27,7 @@
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/assign/std/vector.hpp>
#include <cmath>
using namespace boost::assign;
using namespace gtsam;
@ -75,10 +78,35 @@ TEST(Sphere2, rotate) {
}
}
//*******************************************************************************
static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
return R.unrotate (p);
}
TEST(Sphere2, unrotate) {
Rot3 R = Rot3::yaw(-M_PI/4.0);
Sphere2 p(1, 0, 0);
Sphere2 expected = Sphere2(1, 1, 0);
Sphere2 actual = R.unrotate (p);
EXPECT(assert_equal(expected, actual, 1e-8));
Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian
{
expectedH = numericalDerivative21(unrotate_, R, p);
R.unrotate(p, actualH, boost::none);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
{
expectedH = numericalDerivative22(unrotate_, R, p);
R.unrotate(p, boost::none, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
}
//*******************************************************************************
TEST(Sphere2, error) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
@ -101,8 +129,8 @@ TEST(Sphere2, error) {
//*******************************************************************************
TEST(Sphere2, distance) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
@ -146,9 +174,20 @@ TEST(Sphere2, retract) {
Vector v(2);
v << 0.5, 0;
Sphere2 expected(Point3(1, 0, 0.5));
Sphere2 actual = p.retract(v);
Sphere2 actual = p.retract(v, Sphere2::RENORM);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
}
//*******************************************************************************
TEST(Sphere2, retract_expmap) {
Sphere2 p;
Vector v(2);
v << (M_PI/2.0), 0;
Sphere2 expected(Point3(0, 0, 1));
Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
}
//*******************************************************************************
@ -174,9 +213,9 @@ inline static Vector randomVector(const Vector& minLimits,
TEST(Sphere2, localCoordinates_retract) {
size_t numIterations = 10000;
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
Vector_(3, 1.0, 1.0, 1.0);
Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0);
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
@ -198,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) {
}
}
//*******************************************************************************
// Let x and y be two Sphere2's.
// The equality x.localCoordinates(x.retract(v)) == v should hold.
TEST(Sphere2, localCoordinates_retract_expmap) {
size_t numIterations = 10000;
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
sleep(0);
// Create the two Sphere2s.
// Unlike the above case, we can use any two sphers.
Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
Vector v12 = randomVector(minXiLimit, maxXiLimit);
// Magnitude of the rotation can be at most pi
if (v12.norm () > M_PI)
v12 = v12 / M_PI;
Sphere2 s2 = s1.retract(v12);
// Check if the local coordinates and retract return the same results.
Vector actual_v12 = s1.localCoordinates(s2);
EXPECT(assert_equal(v12, actual_v12, 1e-3));
Sphere2 actual_s2 = s1.retract(actual_v12);
EXPECT(assert_equal(s2, actual_s2, 1e-3));
}
}
//*******************************************************************************
//TEST( Pose2, between )
//{
@ -216,7 +288,7 @@ TEST(Sphere2, localCoordinates_retract) {
// EXPECT(assert_equal(expected,actual1));
// EXPECT(assert_equal(expected,actual2));
//
// Matrix expectedH1 = Matrix_(3,3,
// Matrix expectedH1 = (Matrix(3,3) <<
// 0.0,-1.0,-2.0,
// 1.0, 0.0,-2.0,
// 0.0, 0.0,-1.0
@ -227,7 +299,7 @@ TEST(Sphere2, localCoordinates_retract) {
// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
//
// Matrix expectedH2 = Matrix_(3,3,
// Matrix expectedH2 = (Matrix(3,3) <<
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0
@ -253,6 +325,17 @@ TEST(Sphere2, Random) {
EXPECT(assert_equal(expectedMean,actualMean,0.1));
}
//*************************************************************************
TEST (Sphere2, FromPoint3) {
Matrix actualH;
Point3 point(1, -2, 3); // arbitrary point
Sphere2 expected(point);
EXPECT(assert_equal(expected, Sphere2::FromPoint3(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Sphere2, Point3>(
boost::bind(Sphere2::FromPoint3, _1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
int main() {
srand(time(NULL));

2
gtsam/geometry/tests/timePose2.cpp
View File

@ -138,7 +138,7 @@ int main()
// create a random pose:
double x = 4.0, y = 2.0, r = 0.3;
Vector v = Vector_(3,x,y,r);
Vector v = (Vector(3) << x, y, r);
Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
TEST(Expmap, Pose2::Expmap(v));

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

2
gtsam/geometry/tests/timeRot2.cpp
View File

@ -95,7 +95,7 @@ int main()
// create a random direction:
double norm=sqrt(16.0+4.0);
double x=4.0/norm, y=2.0/norm;
Vector v = Vector_(2,x,y);
Vector v = (Vector(2) << x, y);
Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6);
TEST(Rot2_Expmap, Rot2::Expmap(v));

2
gtsam/geometry/tests/timeRot3.cpp
View File

@ -39,7 +39,7 @@ int main()
// create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector v = Vector_(3,x,y,z);
Vector v = (Vector(3) << x, y, z);
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v);
TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))

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

2
gtsam/linear/tests/testSampler.cpp
View File

@ -24,7 +24,7 @@ const double tol = 1e-5;
/* ************************************************************************* */
TEST(testSampler, basic) {
Vector sigmas = Vector_(3, 1.0, 0.1, 0.0);
Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -236,7 +236,7 @@ namespace gtsam {
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
@ -274,6 +274,7 @@ namespace gtsam {
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
// deltaPij += deltaVij * deltaT;
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
@ -341,8 +342,11 @@ namespace gtsam {
public:
/** Shorthand for a smart pointer to a factor */
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#ifndef _MSC_VER
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}

2
gtsam/navigation/ImuBias.h
View File

@ -113,7 +113,7 @@ namespace imuBias {
//
// return measurement - biasGyro_ - w_earth_rate_I;
//
//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
// }

4
gtsam/navigation/ImuFactor.h
View File

@ -304,7 +304,11 @@ namespace gtsam {
public:
/** Shorthand for a smart pointer to a factor */
#ifndef _MSC_VER
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -168,9 +168,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;

20
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -167,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1(3, 0.5, 0.0, 0.0);
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2(3, 0.5, 0.0, 0.0);
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{
// // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1(3, 0.5, 0.0, 0.0);
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2(3, 0.5, 0.0, 0.0);
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
// // Pre-integrator
@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;

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

8
gtsam/nonlinear/ISAM2.h
View File

@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params {
* entries would be added with:
* \code
FastMap<char,Vector> thresholds;
thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds;
\endcode
*/
@ -552,8 +552,8 @@ public:
* If provided, 'deletedFactorsIndices' will be augmented with the factor graph
* indices of any factor that was removed during the 'marginalizeLeaves' call
*/
void marginalizeLeaves(const FastList<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 */

19
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -74,6 +74,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const {
return graph_.linearize(state_.values);
}
/* ************************************************************************* */
void LevenbergMarquardtOptimizer::increaseLambda(){
state_.lambda *= params_.lambdaFactor;
}
/* ************************************************************************* */
void LevenbergMarquardtOptimizer::decreaseLambda(){
state_.lambda /= params_.lambdaFactor;
}
/* ************************************************************************* */
void LevenbergMarquardtOptimizer::iterate() {
@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() {
if (error <= state_.error) {
state_.values.swap(newValues);
state_.error = error;
state_.lambda /= params_.lambdaFactor;
decreaseLambda();
break;
} else {
// Either we're not cautious, or the same lambda was worse than the current error.
@ -157,7 +167,10 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
break;
} else {
state_.lambda *= params_.lambdaFactor;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl;
increaseLambda();
}
}
} catch (IndeterminantLinearSystemException& e) {
@ -172,7 +185,7 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
break;
} else {
state_.lambda *= params_.lambdaFactor;
increaseLambda();
}
}
// Frank asks: why would we do that?

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -174,6 +174,12 @@ public:
return state_.lambda;
}
// Apply policy to increase lambda if the current update was successful
virtual void increaseLambda();
// Apply policy to decrease lambda if the current update was NOT successful
virtual void decreaseLambda();
/// Access the current number of inner iterations
int getInnerIterations() const {
return state_.totalNumberInnerIterations;

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

@ -19,7 +19,7 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0));
const double tol = 1e-5;
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;

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

@ -65,7 +65,8 @@ public:
TEST( Values, equals1 )
{
Values expected;
LieVector v(3, 5.0, 6.0, 7.0);
LieVector v((Vector(3) << 5.0, 6.0, 7.0));
expected.insert(key1,v);
Values actual;
actual.insert(key1,v);
@ -76,8 +77,9 @@ TEST( Values, equals1 )
TEST( Values, equals2 )
{
Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 5.0, 6.0, 8.0);
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 5.0, 6.0, 8.0));
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2));
@ -88,8 +90,9 @@ TEST( Values, equals2 )
TEST( Values, equals_nan )
{
Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, inf, inf, inf);
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << inf, inf, inf));
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2));
@ -100,10 +103,11 @@ TEST( Values, equals_nan )
TEST( Values, insert_good )
{
Values cfg1, cfg2, expected;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v3(3, 2.0, 4.0, 3.0);
LieVector v4(3, 8.0, 3.0, 7.0);
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
cfg2.insert(key3, v4);
@ -121,10 +125,11 @@ TEST( Values, insert_good )
TEST( Values, insert_bad )
{
Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v3(3, 2.0, 4.0, 3.0);
LieVector v4(3, 8.0, 3.0, 7.0);
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
cfg2.insert(key2, v3);
@ -137,8 +142,8 @@ TEST( Values, insert_bad )
TEST( Values, update_element )
{
Values cfg;
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
cfg.insert(key1, v1);
CHECK(cfg.size() == 1);
@ -181,8 +186,8 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero)
//{
// Values config0;
// config0.insert(key1, LieVector(2, 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim());
//
// VectorValues expected;
@ -195,16 +200,16 @@ TEST(Values, basic_functions)
TEST(Values, expmap_a)
{
Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected;
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
CHECK(assert_equal(expected, config0.retract(increment)));
}
@ -213,15 +218,15 @@ TEST(Values, expmap_a)
TEST(Values, expmap_b)
{
Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
CHECK(assert_equal(expected, config0.retract(increment)));
}
@ -230,16 +235,16 @@ TEST(Values, expmap_b)
//TEST(Values, expmap_c)
//{
// Values config0;
// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
//
// Vector increment = LieVector(6,
// 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5);
//
// Values expected;
// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
//
// CHECK(assert_equal(expected, config0.retract(increment)));
//}
@ -248,8 +253,8 @@ TEST(Values, expmap_b)
TEST(Values, expmap_d)
{
Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
//config0.print("config0");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
@ -266,8 +271,8 @@ TEST(Values, expmap_d)
TEST(Values, localCoordinates)
{
Values valuesA;
valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3))
@ -314,17 +319,17 @@ TEST(Values, exists_)
TEST(Values, update)
{
Values config0;
config0.insert(key1, LieVector(1, 1.));
config0.insert(key2, LieVector(1, 2.));
config0.insert(key1, LieVector((Vector(1) << 1.)));
config0.insert(key2, LieVector((Vector(1) << 2.)));
Values superset;
superset.insert(key1, LieVector(1, -1.));
superset.insert(key2, LieVector(1, -2.));
superset.insert(key1, LieVector((Vector(1) << -1.)));
superset.insert(key2, LieVector((Vector(1) << -2.)));
config0.update(superset);
Values expected;
expected.insert(key1, LieVector(1, -1.));
expected.insert(key2, LieVector(1, -2.));
expected.insert(key1, LieVector((Vector(1) << -1.)));
expected.insert(key2, LieVector((Vector(1) << -2.)));
CHECK(assert_equal(expected,config0));
}

2
gtsam/slam/BearingRangeFactor.h
View File

@ -96,7 +96,7 @@ namespace gtsam {
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - measuredRange_);
Vector e2 = (Vector(1) << y2 - measuredRange_);
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);

75
gtsam/slam/EssentialMatrixConstraint.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file EssentialMatrixConstraint.cpp
* @author Frank Dellaert
* @author Pablo Alcantarilla
* @date Jan 5, 2014
**/
#include <gtsam/slam/EssentialMatrixConstraint.h>
//#include <gtsam/linear/GaussianFactor.h>
//#include <gtsam/base/Testable.h>
#include <ostream>
namespace gtsam {
/* ************************************************************************* */
void EssentialMatrixConstraint::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1())
<< "," << keyFormatter(this->key2()) << ")\n";
measuredE_.print(" measured: ");
this->noiseModel_->print(" noise model: ");
}
/* ************************************************************************* */
bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
double tol) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
&& this->measuredE_.equals(e->measuredE_, tol);
}
/* ************************************************************************* */
Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
const Pose3& p2, boost::optional<Matrix&> Hp1,
boost::optional<Matrix&> Hp2) const {
// compute relative Pose3 between p1 and p2
Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
// convert to EssentialMatrix
Matrix D_hx_1P2;
EssentialMatrix hx = EssentialMatrix::FromPose3(_1P2_,
(Hp1 || Hp2) ? boost::optional<Matrix&>(D_hx_1P2) : boost::none);
// Calculate derivatives if needed
if (Hp1) {
// Hp1 will already contain the 6*6 derivative D_1P2_p1
const Matrix& D_1P2_p1 = *Hp1;
// The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
*Hp1 = D_hx_1P2 * D_1P2_p1;
}
if (Hp2) {
// Hp2 will already contain the 6*6 derivative D_1P2_p1
const Matrix& D_1P2_p2 = *Hp2;
// The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
*Hp2 = D_hx_1P2 * D_1P2_p2;
}
// manifold equivalent of h(x)-z -> log(z,h(x))
return measuredE_.localCoordinates(hx); // 5D error
}
} /// namespace gtsam

109
gtsam/slam/EssentialMatrixConstraint.h Normal file
View File

@ -0,0 +1,109 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file EssentialMatrixConstraint.h
* @author Frank Dellaert
* @author Pablo Alcantarilla
* @date Jan 5, 2014
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h>
namespace gtsam {
/**
* Binary factor between two Pose3 variables induced by an EssentialMatrix measurement
* @addtogroup SLAM
*/
class EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
private:
typedef EssentialMatrixConstraint This;
typedef NoiseModelFactor2<Pose3, Pose3> Base;
EssentialMatrix measuredE_; /** The measurement is an essential matrix */
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
/** default constructor - only use for serialization */
EssentialMatrixConstraint() {
}
/**
* Constructor
* @param key1 key for first pose
* @param key2 key for second pose
* @param measuredE measured EssentialMatrix
* @param model noise model, 5D !
*/
EssentialMatrixConstraint(Key key1, Key key2,
const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
Base(model, key1, key2), measuredE_(measuredE) {
}
virtual ~EssentialMatrixConstraint() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** implement functions needed to derive from Factor */
/** vector of errors */
virtual Vector evaluateError(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> Hp1 = boost::none, //
boost::optional<Matrix&> Hp2 = boost::none) const;
/** return the measured */
const EssentialMatrix& measured() const {
return measuredE_;
}
/** number of variables attached to this factor */
std::size_t size() const {
return 2;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measuredE_);
}
};
// \class EssentialMatrixConstraint
}/// namespace gtsam

2
gtsam/slam/RangeFactor.h
View File

@ -73,7 +73,7 @@ namespace gtsam {
} else {
hx = pose.range(point, H1, H2);
}
return Vector_(1, hx - measured_);
return (Vector(1) << hx - measured_);
}
/** return the measured */

53
gtsam/slam/dataset.cpp
View File

@ -229,7 +229,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
}
cout << "load2D read a graph file with " << initial->size()
<< " vertices and " << graph->nrFactors() << " factors" << endl;
<< " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, initial);
}
@ -403,7 +403,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
}
cout << "load2D read a graph file with " << initial->size()
<< " vertices and " << graph->nrFactors() << " factors" << endl;
<< " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, initial);
}
@ -671,33 +671,36 @@ bool writeBAL(const string& filename, SfM_data &data)
bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
// CHECKS
// Store poses or cameras in SfM_data
Values valuesPoses = values.filter<Pose3>();
if( valuesPoses.size() != data.number_cameras()){
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ")!!" << endl;
return false;
if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = values.at<Pose3>(poseKey);
Cal3Bundler K = data.cameras[i].calibration();
PinholeCamera<Cal3Bundler> camera(pose, K);
data.cameras[i] = camera;
}
} else {
Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
Key cameraKey = symbol('c',i);
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
data.cameras[i] = camera;
}
}else{
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
return false;
}
}
// Store 3D points in SfM_data
Values valuesPoints = values.filter<Point3>();
if( valuesPoints.size() != data.number_tracks()){
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
}
if(valuesPoints.size() + valuesPoses.size() != values.size()){
cout << "writeBALfromValues write only poses and points values!!" << endl;
return false;
}
if(valuesPoints.size()==0 || valuesPoses.size()==0){
cout << "writeBALfromValues: No point or pose in values!!" << endl;
return false;
}
for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = values.at<Pose3>(poseKey);
Cal3Bundler K = data.cameras[i].calibration();
PinholeCamera<Cal3Bundler> camera(pose, K);
data.cameras[i] = camera;
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
}
for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
}
}
// Write SfM_data to file
return writeBAL(filename, data);
}
} // \namespace gtsam

4
gtsam/slam/dataset.h
View File

@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
* while camera poses and values are read from Values)
* @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored
* @param values structure where the graph values are stored
* @param values structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the
* cameras, and should be Point3 for the 3D points). Note that the current version
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);

76
gtsam/slam/tests/testEssentialMatrixConstraint.cpp Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testEssentialMatrixConstraint.cpp
* @brief Unit tests for EssentialMatrixConstraint Class
* @author Frank Dellaert
* @author Pablo Alcantarilla
* @date Jan 5, 2014
*/
#include <gtsam/slam/EssentialMatrixConstraint.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( EssentialMatrixConstraint, test ) {
// Create a factor
Key poseKey1(1);
Key poseKey2(2);
Rot3 trueRotation = Rot3::RzRyRx(0.15, 0.15, -0.20);
Point3 trueTranslation(+0.5, -1.0, +1.0);
Sphere2 trueDirection(trueTranslation);
EssentialMatrix measurement(trueRotation, trueDirection);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 0.25);
EssentialMatrixConstraint factor(poseKey1, poseKey2, measurement, model);
// Create a linearization point at the zero-error point
Pose3 pose1(Rot3::RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0));
Pose3 pose2(
Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
Point3(-3.37493895, 6.14660244, -8.93650986));
Vector expected = zero(5);
Vector actual = factor.evaluateError(pose1,pose2);
CHECK(assert_equal(expected, actual, 1e-8));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
Matrix actualH1;
Matrix actualH2;
factor.evaluateError(pose1, pose2, actualH1, actualH2);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

19
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -154,8 +154,8 @@ TEST (EssentialMatrixFactor2, factor) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = SimpleCamera::project_to_camera(P2);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) {
truth.insert(i, LieScalar(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
} // namespace example1

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

@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values)));
EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values)));
}
static const double baseline = 5.0 ;

17
gtsam_unstable/base/FixedVector.h
View File

@ -45,23 +45,6 @@ public:
std::copy(values, values+N, this->data());
}
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*
* NOTE: this will throw warnings/explode if there is no argument
* before the variadic section, so there is a meaningless size argument.
*/
FixedVector(size_t n, ...) {
va_list ap;
va_start(ap, n);
for(size_t i = 0 ; i < N ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
}
/**
* Create vector initialized to a constant value
* @param value constant value

19
gtsam_unstable/base/tests/testFixedVector.cpp
View File

@ -28,7 +28,7 @@ static const double tol = 1e-9;
/* ************************************************************************* */
TEST( testFixedVector, conversions ) {
double data1[] = {1.0, 2.0, 3.0};
Vector v1 = Vector_(3, data1);
Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]);
TestVector3 fv1(v1), fv2(data1);
Vector actFv2(fv2);
@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) {
/* ************************************************************************* */
TEST( testFixedVector, variable_constructor ) {
TestVector3 act(3, 1.0, 2.0, 3.0);
TestVector3 act((Vector(3) << 1.0, 2.0, 3.0));
EXPECT_DOUBLES_EQUAL(1.0, act(0), tol);
EXPECT_DOUBLES_EQUAL(2.0, act(1), tol);
EXPECT_DOUBLES_EQUAL(3.0, act(2), tol);
@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) {
/* ************************************************************************* */
TEST( testFixedVector, equals ) {
TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0);
TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0);
TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)),
vec3((Vector(3) << 2.0, 3.0, 4.0));
TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0));
EXPECT(assert_equal(vec1, vec1, tol));
EXPECT(assert_equal(vec1, vec2, tol));
@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) {
/* ************************************************************************* */
TEST( testFixedVector, static_constructors ) {
TestVector3 actZero = TestVector3::zero();
TestVector3 expZero(3, 0.0, 0.0, 0.0);
TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0));
EXPECT(assert_equal(expZero, actZero, tol));
TestVector3 actOnes = TestVector3::ones();
TestVector3 expOnes(3, 1.0, 1.0, 1.0);
TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0));
EXPECT(assert_equal(expOnes, actOnes, tol));
TestVector3 actRepeat = TestVector3::repeat(2.3);
TestVector3 expRepeat(3, 2.3, 2.3, 2.3);
TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3));
EXPECT(assert_equal(expRepeat, actRepeat, tol));
TestVector3 actBasis = TestVector3::basis(1);
TestVector3 expBasis(3, 0.0, 1.0, 0.0);
TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0));
EXPECT(assert_equal(expBasis, actBasis, tol));
TestVector3 actDelta = TestVector3::delta(1, 2.3);
TestVector3 expDelta(3, 0.0, 2.3, 0.0);
TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0));
EXPECT(assert_equal(expDelta, actDelta, tol));
}

2
gtsam_unstable/geometry/BearingS2.cpp
View File

@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const {
/* ************************************************************************* */
Vector BearingS2::localCoordinates(const BearingS2& x) const {
return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0),
return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0),
elevation_.localCoordinates(x.elevation_)(0));
}

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -166,7 +166,7 @@ public:
gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi),
return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
gtsam::LieScalar(1./depth));
}

2
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
// extend line by random dist and angle to get BC
double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
double tABC = randomAngle().theta();
Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC));
Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC));
// extend from B to find C
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);

2
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -128,7 +128,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
// calculate angle to change by
Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());
Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0)));
Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0)));
// create a segment to use for intersection checking
// find the closest intersection

18
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) {
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
LieScalar inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)));
LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
LieScalar inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)));
LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
LieScalar inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)));
LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
LieScalar inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i
TEST( InvDepthFactor, Dproject_pose)
{
LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2);
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark)
{
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth)
{
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */
TEST(InvDepthFactor, backproject)
{
LieVector expected(5,0.,0.,1., 0.1,0.2);
LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth);
@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2)
{
// backwards facing camera
LieVector expected(5,-5.,-5.,2., 3., -0.1);
LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth);

2
gtsam_unstable/geometry/tests/testPose3Upright.cpp
View File

@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) {
EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
EXPECT(assert_equal(x2, x1.retract(delta12), tol));
EXPECT(assert_equal(x1, x2.retract(delta21), tol));

2
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -266,7 +266,7 @@ namespace gtsam {
}
}
return Vector_(2, p_inlier, p_outlier);
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */

673
gtsam_unstable/slam/CombinedImuFactor.h
View File

@ -1,673 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file CombinedImuFactor.h
* @author Luca Carlone, Stephen Williams
**/
#pragma once
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/
class CombinedImuFactor: public NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
* incrementally so as to avoid costly integration at time of factor construction. */
/** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
}
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
}
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
class CombinedPreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
double deltaTij; ///< Time interval from i to j
Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
/** Default constructor, initialize with no IMU measurements */
CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
{
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
}
CombinedPreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
{
}
/** print */
void print(const std::string& s = "Preintegrated Measurements:") const {
std::cout << s << std::endl;
biasHat.print(" biasHat");
std::cout << " deltaTij " << deltaTij << std::endl;
std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
deltaRij.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
}
/** equals */
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const {
return biasHat.equals(expected.biasHat, tol)
&& equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol)
&& equal_with_abs_tol(deltaPij, expected.deltaPij, tol)
&& equal_with_abs_tol(deltaVij, expected.deltaVij, tol)
&& deltaRij.equals(expected.deltaRij, tol)
&& std::fabs(deltaTij - expected.deltaTij) < tol
&& equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol)
&& equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol)
&& equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol)
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
}
/** Add a single IMU measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
double deltaT, ///< Time step
boost::optional<Pose3> body_P_sensor = boost::none ///< Sensor frame
) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
delPdelBiasAcc += delVdelBiasAcc * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT;
delVdelBiasAcc += -deltaRij.matrix() * deltaT;
delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j);
// Single Jacobians to propagate covariance
Matrix3 H_pos_pos = I_3x3;
Matrix3 H_pos_vel = I_3x3 * deltaT;
Matrix3 H_pos_angles = Z_3x3;
Matrix3 H_vel_pos = Z_3x3;
Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT;
Matrix3 H_angles_pos = Z_3x3;
Matrix3 H_angles_vel = Z_3x3;
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
// first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
Matrix G_measCov_Gt = Matrix::Zero(15,15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3);
// G_measCov_Gt.block(3,3,3,3) = (H_vel_biasacc) * (1/deltaT) * measurementCovariance.block(3,3,3,3) * (H_vel_biasacc.transpose()) +
// (H_vel_biasacc) * (1/deltaT) *
// ( measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) *
// (H_vel_biasacc.transpose());
G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) *
(measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) *
(H_vel_biasacc.transpose());
G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) *
(measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3);
// OFF BLOCK DIAGONAL TERMS
Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(3,9,3,3) = block24;
G_measCov_Gt.block(9,3,3,3) = block24.transpose();
Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3);
G_measCov_Gt.block(6,12,3,3) = block35;
G_measCov_Gt.block(12,6,3,3) = block35.transpose();
/*
// overall Jacobian wrt raw measurements (df/du)
Matrix3 H_vel_initbiasacc = H_vel_biasacc;
Matrix3 H_angles_initbiasomega = H_angles_biasomega;
// COMBINED IMU FACTOR, preserves correlation with bias evolution and considers initial uncertainty on biases
Matrix G(15,21);
G << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, - H_vel_biasacc, Z_3x3, H_vel_biasacc, Z_3x3, H_vel_initbiasacc, Z_3x3,
Z_3x3, Z_3x3, - H_angles_biasomega, Z_3x3, H_angles_biasomega, Z_3x3, H_angles_initbiasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3;
Matrix ErrorMatrix = (1/deltaT) * G * measurementCovariance * G.transpose() - G_measCov_Gt;
std::cout << "---- matrix multiplication error = [" << ErrorMatrix << "];"<< std::endl;
double max_err=0;
for(int i=0;i<15;i++)
{
for(int j=0;j<15;j++)
{
if(fabs(ErrorMatrix(i,j))>max_err)
max_err = fabs(ErrorMatrix(i,j));
}
}
std::cout << "---- max matrix multiplication error = [" << max_err << "];"<< std::endl;
if(max_err>10e-15)
std::cout << "---- max matrix multiplication error *large* = [" << max_err << "];"<< std::endl;
PreintMeasCov = F * PreintMeasCov * F.transpose() + (1/deltaT) * G * measurementCovariance * G.transpose();
*/
PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
deltaPij += deltaVij * deltaT;
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(biasHat);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance);
ar & BOOST_SERIALIZATION_NVP(deltaPij);
ar & BOOST_SERIALIZATION_NVP(deltaVij);
ar & BOOST_SERIALIZATION_NVP(deltaRij);
ar & BOOST_SERIALIZATION_NVP(deltaTij);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
}
};
private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
public:
/** Shorthand for a smart pointer to a factor */
#ifndef _MSC_VER
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
/** Constructor */
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
const SharedNoiseModel& model) :
Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis) {
}
virtual ~CombinedImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "CombinedImuFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol);
}
/** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const
{
const double& deltaTij = preintegratedMeasurements_.deltaTij;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
const Rot3 Rot_j = pose_j.rotation();
const Vector3 pos_i = pose_i.translation().vector();
const Vector3 pos_j = pose_j.translation().vector();
// We compute factor's Jacobians, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(15,6);
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix(),
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
Matrix3::Zero(),
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero(),
//dBiasAcc/dPi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero();
}
if(H2) {
H2->resize(15,3);
(*H2) <<
// dfP/dVi
- Matrix3::Identity() * deltaTij
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero(),
//dBiasAcc/dVi
Matrix3::Zero(),
//dBiasOmega/dVi
Matrix3::Zero();
}
if(H3) {
H3->resize(15,6);
(*H3) <<
// dfP/dPosej
Matrix3::Zero(), Rot_j.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(),
//dBiasAcc/dPosej
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPosej
Matrix3::Zero(), Matrix3::Zero();
}
if(H4) {
H4->resize(15,3);
(*H4) <<
// dfP/dVj
Matrix3::Zero(),
// dfV/dVj
Matrix3::Identity(),
// dfR/dVj
Matrix3::Zero(),
//dBiasAcc/dVj
Matrix3::Zero(),
//dBiasOmega/dVj
Matrix3::Zero();
}
if(H5) {
const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
H5->resize(15,6);
(*H5) <<
// dfP/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
// dfV/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
// dfR/dBias_i
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
//dBiasAcc/dBias_i
-Matrix3::Identity(), Matrix3::Zero(),
//dBiasOmega/dBias_i
Matrix3::Zero(), -Matrix3::Identity();
}
if(H6) {
H6->resize(15,6);
(*H6) <<
// dfP/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfV/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfR/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dBias_j
Matrix3::Identity(), Matrix3::Zero(),
//dBiasOmega/dBias_j
Matrix3::Zero(), Matrix3::Identity();
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat);
const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
return r;
}
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
const CombinedPreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
bias_j = bias_i;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
}
}; // \class CombinedImuFactor
} /// namespace gtsam

6
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -537,7 +537,7 @@ public:
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g
@ -551,7 +551,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
g_ENU = (Vector(3) << 0.0, 0.0, -g_calc);
// Calculate rho
@ -560,7 +560,7 @@ public:
double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(Rp + height);
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
rho_ENU = (Vector(3) << rho_E, rho_N, rho_U);
}
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

565
gtsam_unstable/slam/ImuFactor.h
View File

@ -1,565 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts
**/
#pragma once
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
namespace gtsam {
/**
*
* @addtogroup SLAM
* * REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/
class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
* incrementally so as to avoid costly integration at time of factor construction. */
/** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
}
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
}
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
class PreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
double deltaTij; ///< Time interval from i to j
Matrix3 delPdelBiasAcc; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
Vector3 initialRotationRate; ///< initial rotation rate reading from the IMU (at time i)
Vector3 finalRotationRate; ///< final rotation rate reading from the IMU (at time j)
/** Default constructor, initialize with no IMU measurements */
PreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
const Vector3& initialRotationRate = Vector3::Zero() ///< initial rotation rate reading from the IMU (at time i)
) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9),
initialRotationRate(initialRotationRate), finalRotationRate(initialRotationRate)
{
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(9,9);
}
PreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9),
initialRotationRate(Vector3::Zero()), finalRotationRate(Vector3::Zero())
{
measurementCovariance = Matrix::Zero(9,9);
PreintMeasCov = Matrix::Zero(9,9);
}
/** print */
void print(const std::string& s = "Preintegrated Measurements:") const {
std::cout << s << std::endl;
biasHat.print(" biasHat");
std::cout << " deltaTij " << deltaTij << std::endl;
std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
deltaRij.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
}
/** equals */
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const {
return biasHat.equals(expected.biasHat, tol)
&& equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol)
&& equal_with_abs_tol(deltaPij, expected.deltaPij, tol)
&& equal_with_abs_tol(deltaVij, expected.deltaVij, tol)
&& deltaRij.equals(expected.deltaRij, tol)
&& std::fabs(deltaTij - expected.deltaTij) < tol
&& equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol)
&& equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol)
&& equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol)
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
}
/** Add a single IMU measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
double deltaT, ///< Time step
boost::optional<Pose3> body_P_sensor = boost::none ///< Sensor frame
) {
// NOTE: order is important here because each update uses old values.
// First we compensate the measurements for the bias
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
finalRotationRate = correctedOmega;
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
delPdelBiasAcc += delVdelBiasAcc * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT;
delVdelBiasAcc += -deltaRij.matrix() * deltaT;
delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
// Update preintegrated mesurements covariance
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework
Matrix H_pos_pos = I_3x3;
Matrix H_pos_vel = I_3x3 * deltaT;
Matrix H_pos_angles = Z_3x3;
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(9,9);
F << H_pos_pos, H_pos_vel, H_pos_angles,
H_vel_pos, H_vel_vel, H_vel_angles,
H_angles_pos, H_angles_vel, H_angles_angles;
// first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise
PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
deltaPij += deltaVij * deltaT;
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(biasHat);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance);
ar & BOOST_SERIALIZATION_NVP(deltaPij);
ar & BOOST_SERIALIZATION_NVP(deltaVij);
ar & BOOST_SERIALIZATION_NVP(deltaRij);
ar & BOOST_SERIALIZATION_NVP(deltaTij);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
}
};
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
public:
/** Shorthand for a smart pointer to a factor */
#ifndef _MSC_VER
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
/** Constructor */
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
const SharedNoiseModel& model, boost::optional<Pose3> body_P_sensor = boost::none) :
Base(model, pose_i, vel_i, pose_j, vel_j, bias),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor) {
}
virtual ~ImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ImuFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const
{
const double& deltaTij = preintegratedMeasurements_.deltaTij;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
const Rot3 Rot_j = pose_j.rotation();
const Vector3 pos_i = pose_i.translation().vector();
const Vector3 pos_j = pose_j.translation().vector();
// We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(9,6);
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix(),
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
Matrix3::Zero(),
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero();
}
if(H2) {
H2->resize(9,3);
(*H2) <<
// dfP/dVi
- Matrix3::Identity() * deltaTij
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero();
}
if(H3) {
H3->resize(9,6);
(*H3) <<
// dfP/dPosej
Matrix3::Zero(), Rot_j.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero();
}
if(H4) {
H4->resize(9,3);
(*H4) <<
// dfP/dVj
Matrix3::Zero(),
// dfV/dVj
Matrix3::Identity(),
// dfR/dVj
Matrix3::Zero();
}
if(H5) {
const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
H5->resize(9,6);
(*H5) <<
// dfP/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
// dfV/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat);
Vector r(9); r << fp, fv, fR;
return r;
}
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<Pose3> body_P_sensor = boost::none)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \class ImuFactor
} /// namespace gtsam

2
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -96,7 +96,7 @@ public:
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/** return the measurement */

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -96,7 +96,7 @@ public:
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/// Evaluate error h(x)-z and optionally derivatives

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -99,7 +99,7 @@ public:
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/// Evaluate error h(x)-z and optionally derivatives

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -35,7 +35,7 @@ public:
}
Vector b_g(double g_e) {
Vector n_g = Vector_(3, 0.0, 0.0, g_e);
Vector n_g = (Vector(3) << 0.0, 0.0, g_e);
return (bRn_ * n_g).vector();
}

2
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1);
this->fillH();
}

2
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
*H2 = zeros(1, 3);
(*H2)(0, 2) = -1.0;
}
return Vector_(1, hx - measured_);
return (Vector(1) << hx - measured_);
}
/* ************************************************************************* */

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -276,7 +276,7 @@ namespace gtsam {
}
}
return Vector_(2, p_inlier, p_outlier);
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */

2
gtsam_unstable/slam/tests/testAHRS.cpp
View File

@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) {
Mechanization_bRn2 mech;
KalmanFilter::State state;
// boost::tie(mech,state) = ahrs.initialize(g_e);
// Vector u = Vector_(3,0.05,0.0,0.0);
// Vector u = (Vector(3) << 0.05,0.0,0.0);
// double dt = 2;
// Rot3 expected;
// Mechanization_bRn2 mech2 = mech.integrate(u,dt);

36
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -119,10 +119,10 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
LieVector Vel1(3, 0.50, -0.50, 0.40);
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector expectedVel2(3, 0.51, -0.48, 0.43);
LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
Pose3 actualPose2;
LieVector actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
@ -157,8 +157,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40);
LieVector Vel2(3, 0.51, -0.48, 0.43);
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -192,8 +192,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
LieVector Vel1(3,0.0,0.0,0.0);
LieVector Vel2(3,0.0,0.0,0.0);
LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0));
LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0));
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4);
LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4);
LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2);
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000);
LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000));
imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
@ -447,10 +447,10 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
LieVector Vel1(3, 0.50, -0.50, 0.40);
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector expectedVel2(3, 0.51, -0.48, 0.43);
LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
Pose3 actualPose2;
LieVector actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
@ -488,8 +488,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40);
LieVector Vel2(3, 0.51, -0.48, 0.43);
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -527,8 +527,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0));
LieVector Vel1(3,0.0,0.0,0.0);
LieVector Vel2(3,0.0,0.0,0.0);
LieVector Vel1((Vector(3) << 0.0,0.0,0.0));
LieVector Vel2((Vector(3) << 0.0,0.0,0.0));
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4);
LieVector Vel1((Vector(3) << 0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4);
LieVector Vel1((Vector(3) << 0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2);
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000);
LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000));
imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;

2
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) {
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 0., 0., 1., 0., 0.);
LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
// initialize inverse depth with "incorrect" depth of 1/4
// in reality this is 1/5, but initial depth is guessed
LieScalar inv_depth(1./4);

Some files were not shown because too many files have changed in this diff Show More