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* /build*
*.pyc *.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; typedef typename internal::dense_xpr_base<CommaInitializer<XprType> >::type Base;
EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer) EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret, typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested; XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator; typedef typename XprType::InnerIterator InnerIterator;
inline CommaInitializer(XprType& xpr, const Scalar& s) inline CommaInitializer(XprType& xpr, const Scalar& s)
: m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1) : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
@ -108,67 +108,67 @@ struct CommaInitializer :
*/ */
inline XprType& finished() { return m_xpr; } inline XprType& finished() { return m_xpr; }
// The following implement the DenseBase interface // The following implement the DenseBase interface
inline Index rows() const { return m_xpr.rows(); } inline Index rows() const { return m_xpr.rows(); }
inline Index cols() const { return m_xpr.cols(); } inline Index cols() const { return m_xpr.cols(); }
inline Index outerStride() const { return m_xpr.outerStride(); } inline Index outerStride() const { return m_xpr.outerStride(); }
inline Index innerStride() const { return m_xpr.innerStride(); } inline Index innerStride() const { return m_xpr.innerStride(); }
inline CoeffReturnType coeff(Index row, Index col) const inline CoeffReturnType coeff(Index row, Index col) const
{ {
return m_xpr.coeff(row, col); return m_xpr.coeff(row, col);
} }
inline CoeffReturnType coeff(Index index) const inline CoeffReturnType coeff(Index index) const
{ {
return m_xpr.coeff(index); return m_xpr.coeff(index);
} }
inline const Scalar& coeffRef(Index row, Index col) const inline const Scalar& coeffRef(Index row, Index col) const
{ {
return m_xpr.const_cast_derived().coeffRef(row, col); return m_xpr.const_cast_derived().coeffRef(row, col);
} }
inline const Scalar& coeffRef(Index index) const inline const Scalar& coeffRef(Index index) const
{ {
return m_xpr.const_cast_derived().coeffRef(index); return m_xpr.const_cast_derived().coeffRef(index);
} }
inline Scalar& coeffRef(Index row, Index col) inline Scalar& coeffRef(Index row, Index col)
{ {
return m_xpr.const_cast_derived().coeffRef(row, col); return m_xpr.const_cast_derived().coeffRef(row, col);
} }
inline Scalar& coeffRef(Index index) inline Scalar& coeffRef(Index index)
{ {
return m_xpr.const_cast_derived().coeffRef(index); return m_xpr.const_cast_derived().coeffRef(index);
} }
template<int LoadMode> template<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const inline const PacketScalar packet(Index row, Index col) const
{ {
return m_xpr.template packet<LoadMode>(row, col); return m_xpr.template packet<LoadMode>(row, col);
} }
template<int LoadMode> template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x) inline void writePacket(Index row, Index col, const PacketScalar& x)
{ {
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x); m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
} }
template<int LoadMode> template<int LoadMode>
inline const PacketScalar packet(Index index) const inline const PacketScalar packet(Index index) const
{ {
return m_xpr.template packet<LoadMode>(index); return m_xpr.template packet<LoadMode>(index);
} }
template<int LoadMode> template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x) inline void writePacket(Index index, const PacketScalar& x)
{ {
m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x); m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
} }
const XprType& _expression() const { return m_xpr; } const XprType& _expression() const { return m_xpr; }
XprType& m_xpr; // target expression XprType& m_xpr; // target expression
@ -176,12 +176,12 @@ struct CommaInitializer :
Index m_col; // current col id Index m_col; // current col id
Index m_currentBlockRows; // current block height Index m_currentBlockRows; // current block height
}; };
namespace internal { namespace internal {
template<typename XprType> template<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType> struct traits<CommaInitializer<XprType> > : traits<XprType>
{ {
}; };
} }
/** \anchor MatrixBaseCommaInitRef /** \anchor MatrixBaseCommaInitRef

14
gtsam/base/LieMatrix.cpp
View File

@ -19,20 +19,6 @@
namespace gtsam { 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 { void LieMatrix::print(const std::string& name) const {
gtsam::print(matrix(), name); 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) : LieMatrix(size_t m, size_t n, const double* const data) :
Matrix(Eigen::Map<const Matrix>(data, m, n)) {} 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 /// @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) LieVector::LieVector(size_t m, const double* const data)
: Vector(Vector_(m,data))
{
}
/* ************************************************************************* */
LieVector::LieVector(size_t m, ...)
: Vector(m) : Vector(m)
{ {
va_list ap; for(size_t i = 0; i < m; i++)
va_start(ap, m); (*this)(i) = data[i];
for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
} }
/* ************************************************************************* */ /* ************************************************************************* */

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 ! */ /** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data); 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 */ /** get the underlying vector */
Vector vector() const { Vector vector() const {
return static_cast<Vector>(*this); return static_cast<Vector>(*this);

43
gtsam/base/Matrix.cpp
View File

@ -36,38 +36,6 @@ using namespace std;
namespace gtsam { 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 ) { Matrix zeros( size_t m, size_t n ) {
return Matrix::Zero(m,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; 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) { void print(const Matrix& A, const string &s, ostream& stream) {
size_t m = A.rows(), n = A.cols(); 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<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix; 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 // Matlab-like syntax
/** /**
@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
return result; return result;
} }
/**
* convert to column vector, column order !!!
*/
GTSAM_EXPORT Vector Vector_(const Matrix& A);
/** /**
* print a matrix * print a matrix
*/ */

27
gtsam/base/Vector.cpp
View File

@ -79,33 +79,6 @@ void odprintf(const char *format, ...) {
//#endif //#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 zero(const Vector& v) {
bool result = true; 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, ...); 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 * Create vector initialized to a constant value
* @param n is the size of the vector * @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 */ /** global functions for converting to a LieVector for use with numericalDerivative */
inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } 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 * 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 ) { TEST( LieMatrix, other_constructors ) {
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0); Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0);
LieMatrix exp(init); LieMatrix exp(init);
LieMatrix a(2,2,10.0,20.0,30.0,40.0);
double data[] = {10,30,20,40}; double data[] = {10,30,20,40};
LieMatrix b(2,2,data); LieMatrix b(2,2,data);
EXPECT(assert_equal(exp, a));
EXPECT(assert_equal(exp, b)); EXPECT(assert_equal(exp, b));
EXPECT(assert_equal(b, a));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LieMatrix, retract) { TEST(LieMatrix, retract) {
LieMatrix init(2,2, 1.0,2.0,3.0,4.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); 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); LieMatrix actual = init.retract(update);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -63,7 +60,7 @@ TEST(LieMatrix, retract) {
EXPECT(assert_equal(expectedUpdate, actualUpdate)); EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4); 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)); EXPECT(assert_equal(expectedLogmap, actualLogmap));
} }

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

@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) {
TEST( testLieScalar, localCoordinates ) { TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.); 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 ) { TEST( testLieVector, other_constructors ) {
Vector init = (Vector(2) << 10.0, 20.0); Vector init = (Vector(2) << 10.0, 20.0);
LieVector exp(init); LieVector exp(init);
LieVector a(2,10.0,20.0);
double data[] = {10,20}; double data[] = {10,20};
LieVector b(2,data); 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(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); 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_ ) TEST( matrix, Matrix_ )
{ {

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

@ -48,7 +48,8 @@ double f2(const LieVector& x) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian2) { 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) << Matrix expected = (Matrix(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), -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) { 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 expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2); Matrix actual11 = numericalHessian211(f3, center1, center2);
@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian311) { 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); double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3); Matrix actual11 = numericalHessian311(f4, center1, center2, center3);

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

@ -23,15 +23,6 @@
using namespace std; using namespace std;
using namespace gtsam; 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 { namespace {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename Derived> template<typename Derived>

538
gtsam/base/timing.h
View File

@ -1,270 +1,270 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file timing.h * @file timing.h
* @brief Timing utilities * @brief Timing utilities
* @author Richard Roberts, Michael Kaess * @author Richard Roberts, Michael Kaess
* @date Oct 5, 2010 * @date Oct 5, 2010
*/ */
#pragma once #pragma once
#include <string> #include <string>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
#include <boost/version.hpp> #include <boost/version.hpp>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
// This file contains the GTSAM timing instrumentation library, a low-overhead method for // 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 // learning at a medium-fine level how much time various components of an algorithm take
// in CPU and wall time. // in CPU and wall time.
// //
// The output of this instrumentation is a call-tree-like printout containing statistics // 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 // about each instrumented code block. To print this output at any time, call
// tictoc_print() or tictoc_print_(). // tictoc_print() or tictoc_print_().
// //
// An overall point to be aware of is that there are two versions of each function - one // 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 // 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 // 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_ENABLE_TIMING is defined (automatically defined in our CMake Timing build type).
// GTSAM algorithms are all instrumented with the non-underscore versions, so generally // 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. // 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* // 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 // 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). // 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(...) // 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 // block, for example, will only time code until the closing brace '}'. See advanced
// usage below if you need to avoid this. // usage below if you need to avoid this.
// //
// Multiple calls nest automatically - each gttic nests under the previous gttic called // 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). // for which gttoc has not been called (or the previous gttic did not go out of scope).
// //
// Basic usage examples are as follows: // Basic usage examples are as follows:
// //
// - Timing an entire function: // - Timing an entire function:
// void myFunction() { // void myFunction() {
// gttic_(myFunction); // gttic_(myFunction);
// ........ // ........
// } // }
// //
// - Timing an entire function as well as its component parts: // - Timing an entire function as well as its component parts:
// void myLongFunction() { // void myLongFunction() {
// gttic_(myLongFunction); // gttic_(myLongFunction);
// gttic_(step1); // Will nest under the 'myLongFunction' label // gttic_(step1); // Will nest under the 'myLongFunction' label
// ........ // ........
// gttoc_(step1); // gttoc_(step1);
// gttic_(step2); // Will nest under the 'myLongFunction' label // gttic_(step2); // Will nest under the 'myLongFunction' label
// ........ // ........
// gttoc_(step2); // gttoc_(step2);
// ........ // ........
// } // }
// //
// - Timing functions calling/called by other functions: // - Timing functions calling/called by other functions:
// void oneStep() { // void oneStep() {
// gttic_(oneStep); // Will automatically nest under the gttic label of the calling function // gttic_(oneStep); // Will automatically nest under the gttic label of the calling function
// ....... // .......
// } // }
// void algorithm() { // void algorithm() {
// gttic_(algorithm); // gttic_(algorithm);
// oneStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label // 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 // twoStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label
// } // }
// //
// //
// Advanced usage: // Advanced usage:
// //
// - "Finishing iterations" - to get correct min/max times for each call, you must define // - "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 // 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 // accumulated within each iteration. If you don't care about min/max times, you don't
// need to worry about this. For example: // need to worry about this. For example:
// void myOuterLoop() { // void myOuterLoop() {
// while(true) { // while(true) {
// iterateMyAlgorithm(); // iterateMyAlgorithm();
// tictoc_finishedIteration_(); // tictoc_finishedIteration_();
// tictoc_print_(); // Optional // tictoc_print_(); // Optional
// } // }
// } // }
// //
// - Stopping timing a section in a different scope than it is started. Normally, a gttoc // - 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 // 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 // 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 // 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 // have matching gttic/gttoc statments. You may want to consider reorganizing your timing
// outline to match the scope of your code. // outline to match the scope of your code.
// Automatically use the new Boost timers if version is recent enough. // Automatically use the new Boost timers if version is recent enough.
#if BOOST_VERSION >= 104800 #if BOOST_VERSION >= 104800
# ifndef GTSAM_DISABLE_NEW_TIMERS # ifndef GTSAM_DISABLE_NEW_TIMERS
# define GTSAM_USING_NEW_BOOST_TIMERS # define GTSAM_USING_NEW_BOOST_TIMERS
# endif # endif
#endif #endif
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
# include <boost/timer/timer.hpp> # include <boost/timer/timer.hpp>
#else #else
# include <boost/timer.hpp> # include <boost/timer.hpp>
#endif #endif
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
# include <tbb/tick_count.h> # include <tbb/tick_count.h>
# undef min # undef min
# undef max # undef max
# undef ERROR # undef ERROR
#endif #endif
namespace gtsam { namespace gtsam {
namespace internal { namespace internal {
GTSAM_EXPORT size_t getTicTocID(const char *description); GTSAM_EXPORT size_t getTicTocID(const char *description);
GTSAM_EXPORT void ticInternal(size_t id, const char *label); GTSAM_EXPORT void ticInternal(size_t id, const char *label);
GTSAM_EXPORT void tocInternal(size_t id, const char *label); GTSAM_EXPORT void tocInternal(size_t id, const char *label);
/** /**
* Timing Entry, arranged in a tree * Timing Entry, arranged in a tree
*/ */
class GTSAM_EXPORT TimingOutline { class GTSAM_EXPORT TimingOutline {
protected: protected:
size_t myId_; size_t myId_;
size_t t_; size_t t_;
size_t tWall_; size_t tWall_;
double t2_ ; ///< cache the \sum t_i^2 double t2_ ; ///< cache the \sum t_i^2
size_t tIt_; size_t tIt_;
size_t tMax_; size_t tMax_;
size_t tMin_; size_t tMin_;
size_t n_; size_t n_;
size_t myOrder_; size_t myOrder_;
size_t lastChildOrder_; size_t lastChildOrder_;
std::string label_; std::string label_;
// Tree structure // Tree structure
boost::weak_ptr<TimingOutline> parent_; ///< parent pointer boost::weak_ptr<TimingOutline> parent_; ///< parent pointer
typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildMap; typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildMap;
ChildMap children_; ///< subtrees ChildMap children_; ///< subtrees
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer timer_; boost::timer::cpu_timer timer_;
#else #else
boost::timer timer_; boost::timer timer_;
gtsam::ValueWithDefault<bool,false> timerActive_; gtsam::ValueWithDefault<bool,false> timerActive_;
#endif #endif
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
tbb::tick_count tbbTimer_; tbb::tick_count tbbTimer_;
#endif #endif
void add(size_t usecs, size_t usecsWall); void add(size_t usecs, size_t usecsWall);
public: public:
/// Constructor /// Constructor
TimingOutline(const std::string& label, size_t myId); TimingOutline(const std::string& label, size_t myId);
size_t time() const; ///< time taken, including children size_t time() const; ///< time taken, including children
double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, 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 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 wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds
double min() const { return double(tMin_) / 1000000.0;} ///< min 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 max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds
double mean() const { return self() / double(n_); } ///< mean self time, in seconds double mean() const { return self() / double(n_); } ///< mean self time, in seconds
void print(const std::string& outline = "") const; void print(const std::string& outline = "") const;
void print2(const std::string& outline = "", const double parentTotal = -1.0) const; void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
const boost::shared_ptr<TimingOutline>& const boost::shared_ptr<TimingOutline>&
child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr); child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
void ticInternal(); void ticInternal();
void tocInternal(); void tocInternal();
void finishedIteration(); void finishedIteration();
GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); GTSAM_EXPORT friend void tocInternal(size_t id, const char *label);
}; // \TimingOutline }; // \TimingOutline
/** /**
* No documentation * No documentation
*/ */
class AutoTicToc { class AutoTicToc {
private: private:
size_t id_; size_t id_;
const char *label_; const char *label_;
bool isSet_; bool isSet_;
public: public:
AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); }
void stop() { tocInternal(id_, label_); isSet_ = false; } void stop() { tocInternal(id_, label_); isSet_ = false; }
~AutoTicToc() { if(isSet_) stop(); } ~AutoTicToc() { if(isSet_) stop(); }
}; };
GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> timingRoot; GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> timingRoot;
GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> timingCurrent; GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> timingCurrent;
} }
// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) // 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 // 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 // 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 // integer ID is only looked up by string once when the static variable is initialized
// as the program starts. // as the program starts.
// tic // tic
#define gttic_(label) \ #define gttic_(label) \
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label)
// toc // toc
#define gttoc_(label) \ #define gttoc_(label) \
label##_obj.stop() label##_obj.stop()
// tic // tic
#define longtic_(label) \ #define longtic_(label) \
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
::gtsam::internal::ticInternal(label##_id_tic, #label) ::gtsam::internal::ticInternal(label##_id_tic, #label)
// toc // toc
#define longtoc_(label) \ #define longtoc_(label) \
static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \
::gtsam::internal::tocInternal(label##_id_toc, #label) ::gtsam::internal::tocInternal(label##_id_toc, #label)
// indicate iteration is finished // indicate iteration is finished
inline void tictoc_finishedIteration_() { inline void tictoc_finishedIteration_() {
::gtsam::internal::timingRoot->finishedIteration(); } ::gtsam::internal::timingRoot->finishedIteration(); }
// print // print
inline void tictoc_print_() { inline void tictoc_print_() {
::gtsam::internal::timingRoot->print(); } ::gtsam::internal::timingRoot->print(); }
// print mean and standard deviation // print mean and standard deviation
inline void tictoc_print2_() { inline void tictoc_print2_() {
::gtsam::internal::timingRoot->print2(); } ::gtsam::internal::timingRoot->print2(); }
// get a node by label and assign it to variable // get a node by label and assign it to variable
#define tictoc_getNode(variable, label) \ #define tictoc_getNode(variable, label) \
static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \ const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent);
// reset // reset
inline void tictoc_reset_() { inline void tictoc_reset_() {
::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; }
#ifdef ENABLE_TIMING #ifdef ENABLE_TIMING
#define gttic(label) gttic_(label) #define gttic(label) gttic_(label)
#define gttoc(label) gttoc_(label) #define gttoc(label) gttoc_(label)
#define longtic(label) longtic_(label) #define longtic(label) longtic_(label)
#define longtoc(label) longtoc_(label) #define longtoc(label) longtoc_(label)
#define tictoc_finishedIteration tictoc_finishedIteration_ #define tictoc_finishedIteration tictoc_finishedIteration_
#define tictoc_print tictoc_print_ #define tictoc_print tictoc_print_
#define tictoc_reset tictoc_reset_ #define tictoc_reset tictoc_reset_
#else #else
#define gttic(label) ((void)0) #define gttic(label) ((void)0)
#define gttoc(label) ((void)0) #define gttoc(label) ((void)0)
#define longtic(label) ((void)0) #define longtic(label) ((void)0)
#define longtoc(label) ((void)0) #define longtoc(label) ((void)0)
#define tictoc_finishedIteration() ((void)0) #define tictoc_finishedIteration() ((void)0)
#define tictoc_print() ((void)0) #define tictoc_print() ((void)0)
#define tictoc_reset() ((void)0) #define tictoc_reset() ((void)0)
#endif #endif
} }

670
gtsam/base/types.h
View File

@ -1,335 +1,335 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file types.h * @file types.h
* @brief Typedefs for easier changing of types * @brief Typedefs for easier changing of types
* @author Richard Roberts * @author Richard Roberts
* @date Aug 21, 2010 * @date Aug 21, 2010
* @addtogroup base * @addtogroup base
*/ */
#pragma once #pragma once
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/config.h> #include <gtsam/config.h>
#include <cstddef> #include <cstddef>
#include <string> #include <string>
#include <iostream> #include <iostream>
#include <boost/function/function1.hpp> #include <boost/function/function1.hpp>
#include <boost/range/concepts.hpp> #include <boost/range/concepts.hpp>
#include <boost/optional.hpp> #include <boost/optional.hpp>
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h> #include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h> #include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h> #include <tbb/scalable_allocator.h>
#endif #endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP #ifdef GTSAM_USE_EIGEN_MKL_OPENMP
#include <omp.h> #include <omp.h>
#endif #endif
#ifdef __clang__ #ifdef __clang__
# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ # define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \
_Pragma("clang diagnostic push") \ _Pragma("clang diagnostic push") \
_Pragma("clang diagnostic ignored \"" diag "\"") _Pragma("clang diagnostic ignored \"" diag "\"")
#else #else
# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) # define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
#endif #endif
#ifdef __clang__ #ifdef __clang__
# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") # define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
#else #else
# define CLANG_DIAGNOSTIC_POP() # define CLANG_DIAGNOSTIC_POP()
#endif #endif
namespace gtsam { namespace gtsam {
/// Integer nonlinear key type /// Integer nonlinear key type
typedef size_t Key; typedef size_t Key;
/// Typedef for a function to format a key, i.e. to convert it to a string /// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter; typedef boost::function<std::string(Key)> KeyFormatter;
// Helper function for DefaultKeyFormatter // Helper function for DefaultKeyFormatter
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
/// The default KeyFormatter, which is used if no KeyFormatter is passed to /// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys /// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys. /// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
/// The index type for Eigen objects /// The index type for Eigen objects
typedef ptrdiff_t DenseIndex; typedef ptrdiff_t DenseIndex;
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Helper class that uses templates to select between two types based on * Helper class that uses templates to select between two types based on
* whether TEST_TYPE is const or not. * whether TEST_TYPE is const or not.
*/ */
template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST, template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
typename AS_CONST> typename AS_CONST>
struct const_selector { struct const_selector {
}; };
/** Specialization for the non-const version */ /** Specialization for the non-const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST> template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> { struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_NON_CONST type; typedef AS_NON_CONST type;
}; };
/** Specialization for the const version */ /** Specialization for the const version */
template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST> template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> { struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
typedef AS_CONST type; typedef AS_CONST type;
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Helper struct that encapsulates a value with a default, this is just used * 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 * as a member object so you don't have to specify defaults in the class
* constructor. * constructor.
*/ */
template<typename T, T defaultValue> template<typename T, T defaultValue>
struct ValueWithDefault { struct ValueWithDefault {
T value; T value;
/** Default constructor, initialize to default value supplied in template argument */ /** Default constructor, initialize to default value supplied in template argument */
ValueWithDefault() : value(defaultValue) {} ValueWithDefault() : value(defaultValue) {}
/** Initialize to the given value */ /** Initialize to the given value */
ValueWithDefault(const T& _value) : value(_value) {} ValueWithDefault(const T& _value) : value(_value) {}
/** Operator to access the value */ /** Operator to access the value */
T& operator*() { return value; } T& operator*() { return value; }
/** Operator to access the value */ /** Operator to access the value */
const T& operator*() const { return value; } const T& operator*() const { return value; }
/** Implicit conversion allows use in if statements for bool type, etc. */ /** Implicit conversion allows use in if statements for bool type, etc. */
operator T() const { return value; } operator T() const { return value; }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/** A helper class that behaves as a container with one element, and works with /** A helper class that behaves as a container with one element, and works with
* boost::range */ * boost::range */
template<typename T> template<typename T>
class ListOfOneContainer { class ListOfOneContainer {
T element_; T element_;
public: public:
typedef T value_type; typedef T value_type;
typedef const T* const_iterator; typedef const T* const_iterator;
typedef T* iterator; typedef T* iterator;
ListOfOneContainer(const T& element) : element_(element) {} ListOfOneContainer(const T& element) : element_(element) {}
const T* begin() const { return &element_; } const T* begin() const { return &element_; }
const T* end() const { return &element_ + 1; } const T* end() const { return &element_ + 1; }
T* begin() { return &element_; } T* begin() { return &element_; }
T* end() { return &element_ + 1; } T* end() { return &element_ + 1; }
size_t size() const { return 1; } size_t size() const { return 1; }
}; };
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >)); BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
template<typename T> template<typename T>
ListOfOneContainer<T> ListOfOne(const T& element) { ListOfOneContainer<T> ListOfOne(const T& element) {
return ListOfOneContainer<T>(element); return ListOfOneContainer<T>(element);
} }
/* ************************************************************************* */ /* ************************************************************************* */
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED> template<class DERIVED>
class ThreadsafeException : class ThreadsafeException :
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
public tbb::tbb_exception public tbb::tbb_exception
#else #else
public std::exception public std::exception
#endif #endif
{ {
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
private: private:
typedef tbb::tbb_exception Base; typedef tbb::tbb_exception Base;
protected: protected:
typedef std::basic_string<char, std::char_traits<char>, tbb::tbb_allocator<char> > String; typedef std::basic_string<char, std::char_traits<char>, tbb::tbb_allocator<char> > String;
#else #else
private: private:
typedef std::exception Base; typedef std::exception Base;
protected: protected:
typedef std::string String; typedef std::string String;
#endif #endif
protected: protected:
bool dynamic_; ///< Whether this object was moved bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> description_; ///< Optional description mutable boost::optional<String> description_; ///< Optional description
/// Default constructor is protected - may only be created from derived classes /// Default constructor is protected - may only be created from derived classes
ThreadsafeException() : dynamic_(false) {} ThreadsafeException() : dynamic_(false) {}
/// Copy constructor is protected - may only be created from derived classes /// Copy constructor is protected - may only be created from derived classes
ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {}
/// Construct with description string /// Construct with description string
ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {}
/// Default destructor doesn't have the throw() /// Default destructor doesn't have the throw()
virtual ~ThreadsafeException() throw () {} virtual ~ThreadsafeException() throw () {}
public: public:
// Implement functions for tbb_exception // Implement functions for tbb_exception
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
virtual tbb::tbb_exception* move() throw () { virtual tbb::tbb_exception* move() throw () {
void* cloneMemory = scalable_malloc(sizeof(DERIVED)); void* cloneMemory = scalable_malloc(sizeof(DERIVED));
if (!cloneMemory) { if (!cloneMemory) {
std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
exit(-1); exit(-1);
} }
DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this)); DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
clone->dynamic_ = true; clone->dynamic_ = true;
return clone; return clone;
} }
virtual void destroy() throw() { virtual void destroy() throw() {
if (dynamic_) { if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this); DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED(); derivedPtr->~DERIVED();
scalable_free(derivedPtr); scalable_free(derivedPtr);
} }
} }
virtual void throw_self() { virtual void throw_self() {
throw *static_cast<DERIVED*>(this); } throw *static_cast<DERIVED*>(this); }
virtual const char* name() const throw() { virtual const char* name() const throw() {
return typeid(DERIVED).name(); } return typeid(DERIVED).name(); }
#endif #endif
virtual const char* what() const throw() { virtual const char* what() const throw() {
return description_ ? description_->c_str() : ""; } return description_ ? description_->c_str() : ""; }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/// Threadsafe runtime error exception /// Threadsafe runtime error exception
class RuntimeErrorThreadsafe : public ThreadsafeException<RuntimeErrorThreadsafe> class RuntimeErrorThreadsafe : public ThreadsafeException<RuntimeErrorThreadsafe>
{ {
public: public:
/// Construct with a string describing the exception /// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {} RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {}
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/// Threadsafe runtime error exception /// Threadsafe runtime error exception
class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe> class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
{ {
public: public:
/// Construct with a string describing the exception /// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {} OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {}
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/// Threadsafe invalid argument exception /// Threadsafe invalid argument exception
class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe> class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
{ {
public: public:
/// Construct with a string describing the exception /// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(description) {} InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(description) {}
}; };
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef __clang__ #ifdef __clang__
# pragma clang diagnostic push # pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below # pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below
#endif #endif
/// An object whose scope defines a block where TBB and OpenMP parallelism are mixed. In such a /// 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 /// 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. /// use both TBB and OpenMP, this has no effect.
class TbbOpenMPMixedScope class TbbOpenMPMixedScope
{ {
int previousOpenMPThreads; int previousOpenMPThreads;
public: public:
#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP #if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP
TbbOpenMPMixedScope() : TbbOpenMPMixedScope() :
previousOpenMPThreads(omp_get_num_threads()) previousOpenMPThreads(omp_get_num_threads())
{ {
omp_set_num_threads(omp_get_num_procs() / 4); omp_set_num_threads(omp_get_num_procs() / 4);
} }
~TbbOpenMPMixedScope() ~TbbOpenMPMixedScope()
{ {
omp_set_num_threads(previousOpenMPThreads); omp_set_num_threads(previousOpenMPThreads);
} }
#else #else
TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {} TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {}
~TbbOpenMPMixedScope() {} ~TbbOpenMPMixedScope() {}
#endif #endif
}; };
#ifdef __clang__ #ifdef __clang__
# pragma clang diagnostic pop # pragma clang diagnostic pop
#endif #endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** An assertion that throws an exception if NDEBUG is not defined and /** An assertion that throws an exception if NDEBUG is not defined and
* evaluates to an empty statement otherwise. */ * evaluates to an empty statement otherwise. */
#ifdef NDEBUG #ifdef NDEBUG
#define assert_throw(CONDITION, EXCEPTION) ((void)0) #define assert_throw(CONDITION, EXCEPTION) ((void)0)
#else #else
#define assert_throw(CONDITION, EXCEPTION) \ #define assert_throw(CONDITION, EXCEPTION) \
if (!(CONDITION)) { \ if (!(CONDITION)) { \
throw (EXCEPTION); \ throw (EXCEPTION); \
} }
#endif #endif
#ifdef _MSC_VER #ifdef _MSC_VER
// Define some common g++ functions and macros we use that MSVC does not have // Define some common g++ functions and macros we use that MSVC does not have
#include <boost/math/special_functions/fpclassify.hpp> #include <boost/math/special_functions/fpclassify.hpp>
namespace std { namespace std {
template<typename T> inline int isfinite(T a) { template<typename T> inline int isfinite(T a) {
return (int)boost::math::isfinite(a); } return (int)boost::math::isfinite(a); }
template<typename T> inline int isnan(T a) { template<typename T> inline int isnan(T a) {
return (int)boost::math::isnan(a); } return (int)boost::math::isnan(a); }
template<typename T> inline int isinf(T a) { template<typename T> inline int isinf(T a) {
return (int)boost::math::isinf(a); } return (int)boost::math::isinf(a); }
} }
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#ifndef M_PI #ifndef M_PI
#define M_PI (boost::math::constants::pi<double>()) #define M_PI (boost::math::constants::pi<double>())
#endif #endif
#ifndef M_PI_2 #ifndef M_PI_2
#define M_PI_2 (boost::math::constants::pi<double>() / 2.0) #define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
#endif #endif
#ifndef M_PI_4 #ifndef M_PI_4
#define M_PI_4 (boost::math::constants::pi<double>() / 4.0) #define M_PI_4 (boost::math::constants::pi<double>() / 4.0)
#endif #endif
#endif #endif
#ifdef min #ifdef min
#undef min #undef min
#endif #endif
#ifdef max #ifdef max
#undef max #undef max
#endif #endif
#ifdef ERROR #ifdef ERROR
#undef ERROR #undef ERROR
#endif #endif

8
gtsam/discrete/DecisionTreeFactor.h
View File

@ -158,10 +158,10 @@ namespace gtsam {
// /** // /**
// * Apply a reduction, which is a remapping of variable indices. // * Apply a reduction, which is a remapping of variable indices.
// */ // */
// virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { // virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
// DiscreteFactor::reduceWithInverse(inverseReduction); // DiscreteFactor::reduceWithInverse(inverseReduction);
// Potentials::reduceWithInverse(inverseReduction); // Potentials::reduceWithInverse(inverseReduction);
// } // }
/// @} /// @}
}; };

268
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -1,134 +1,134 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file DiscreteFactorGraph.cpp * @file DiscreteFactorGraph.cpp
* @date Feb 14, 2011 * @date Feb 14, 2011
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
* @author Frank Dellaert * @author Frank Dellaert
*/ */
//#define ENABLE_TIMING //#define ENABLE_TIMING
#include <gtsam/discrete/DiscreteFactorGraph.h> #include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteBayesTree.h> #include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteEliminationTree.h> #include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteJunctionTree.h> #include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h> #include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
// Instantiate base classes // Instantiate base classes
template class FactorGraph<DiscreteFactor>; template class FactorGraph<DiscreteFactor>;
template class EliminateableFactorGraph<DiscreteFactorGraph>; template class EliminateableFactorGraph<DiscreteFactorGraph>;
/* ************************************************************************* */ /* ************************************************************************* */
bool DiscreteFactorGraph::equals(const This& fg, double tol) const bool DiscreteFactorGraph::equals(const This& fg, double tol) const
{ {
return Base::equals(fg, tol); return Base::equals(fg, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const { FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> keys; FastSet<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end()); if (factor) keys.insert(factor->begin(), factor->end());
return keys; return keys;
} }
/* ************************************************************************* */ /* ************************************************************************* */
DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor DiscreteFactorGraph::product() const {
DecisionTreeFactor result; DecisionTreeFactor result;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) result = (*factor) * result; if (factor) result = (*factor) * result;
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double DiscreteFactorGraph::operator()( double DiscreteFactorGraph::operator()(
const DiscreteFactor::Values &values) const { const DiscreteFactor::Values &values) const {
double product = 1.0; double product = 1.0;
BOOST_FOREACH( const sharedFactor& factor, factors_ ) BOOST_FOREACH( const sharedFactor& factor, factors_ )
product *= (*factor)(values); product *= (*factor)(values);
return product; return product;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void DiscreteFactorGraph::print(const std::string& s, void DiscreteFactorGraph::print(const std::string& s,
const KeyFormatter& formatter) const { const KeyFormatter& formatter) const {
std::cout << s << std::endl; std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl; std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss; std::stringstream ss;
ss << "factor " << i << ": "; ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
} }
} }
// /* ************************************************************************* */ // /* ************************************************************************* */
// void DiscreteFactorGraph::permuteWithInverse( // void DiscreteFactorGraph::permuteWithInverse(
// const Permutation& inversePermutation) { // const Permutation& inversePermutation) {
// BOOST_FOREACH(const sharedFactor& factor, factors_) { // BOOST_FOREACH(const sharedFactor& factor, factors_) {
// if(factor) // if(factor)
// factor->permuteWithInverse(inversePermutation); // factor->permuteWithInverse(inversePermutation);
// } // }
// } // }
// //
// /* ************************************************************************* */ // /* ************************************************************************* */
// void DiscreteFactorGraph::reduceWithInverse( // void DiscreteFactorGraph::reduceWithInverse(
// const internal::Reduction& inverseReduction) { // const internal::Reduction& inverseReduction) {
// BOOST_FOREACH(const sharedFactor& factor, factors_) { // BOOST_FOREACH(const sharedFactor& factor, factors_) {
// if(factor) // if(factor)
// factor->reduceWithInverse(inverseReduction); // factor->reduceWithInverse(inverseReduction);
// } // }
// } // }
/* ************************************************************************* */ /* ************************************************************************* */
DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const
{ {
gttic(DiscreteFactorGraph_optimize); gttic(DiscreteFactorGraph_optimize);
return BaseEliminateable::eliminateSequential()->optimize(); return BaseEliminateable::eliminateSequential()->optimize();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> // std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) {
// PRODUCT: multiply all factors // PRODUCT: multiply all factors
gttic(product); gttic(product);
DecisionTreeFactor product; DecisionTreeFactor product;
BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors) BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors)
product = (*factor) * product; product = (*factor) * product;
gttoc(product); gttoc(product);
// sum out frontals, this is the factor on the separator // sum out frontals, this is the factor on the separator
gttic(sum); gttic(sum);
DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
gttoc(sum); gttoc(sum);
// Ordering keys for the conditional so that frontalKeys are really in front // Ordering keys for the conditional so that frontalKeys are really in front
Ordering orderedKeys; Ordering orderedKeys;
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end()); orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end()); orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
// now divide product/sum to get conditional // now divide product/sum to get conditional
gttic(divide); gttic(divide);
DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys)); DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys));
gttoc(divide); gttoc(divide);
return std::make_pair(cond, sum); return std::make_pair(cond, sum);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace } // namespace

8
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -138,10 +138,10 @@ public:
* to calling graph.eliminateSequential()->optimize(). */ * to calling graph.eliminateSequential()->optimize(). */
DiscreteFactor::sharedValues optimize() const; DiscreteFactor::sharedValues optimize() const;
// /** Permute the variables in the factors */ // /** Permute the variables in the factors */
// GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); // GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
// //
// /** Apply a reduction, which is a remapping of variable indices. */ // /** Apply a reduction, which is a remapping of variable indices. */
// GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); // 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 Rot3 aRb_; ///< Rotation between a and b
Sphere2 aTb_; ///< translation direction from a to b Sphere2 aTb_; ///< translation direction from a to b
Matrix E_; ///< Essential matrix Matrix3 E_; ///< Essential matrix
public: public:
@ -48,6 +48,10 @@ public:
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { 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 /// Random, using Rot3::Random and Sphere2::Random
template<typename Engine> template<typename Engine>
static EssentialMatrix Random(Engine & rng) { static EssentialMatrix Random(Engine & rng) {
@ -60,11 +64,7 @@ public:
/// @{ /// @{
/// print with optional string /// print with optional string
void print(const std::string& s = "") const { void print(const std::string& s = "") const;
std::cout << s;
aRb_.print("R:\n");
aTb_.print("d: ");
}
/// assert equality up to a tolerance /// assert equality up to a tolerance
bool equals(const EssentialMatrix& other, double tol = 1e-8) const { bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
@ -87,20 +87,10 @@ public:
} }
/// Retract delta to manifold /// Retract delta to manifold
virtual EssentialMatrix retract(const Vector& xi) const { 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);
}
/// Compute the coordinates in the tangent space /// Compute the coordinates in the tangent space
virtual Vector localCoordinates(const EssentialMatrix& other) const { virtual Vector localCoordinates(const EssentialMatrix& other) const;
return Vector(5) << //
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
}
/// @} /// @}
@ -108,17 +98,17 @@ public:
/// @{ /// @{
/// Rotation /// Rotation
const Rot3& rotation() const { inline const Rot3& rotation() const {
return aRb_; return aRb_;
} }
/// Direction /// Direction
const Sphere2& direction() const { inline const Sphere2& direction() const {
return aTb_; return aTb_;
} }
/// Return 3*3 matrix representation /// Return 3*3 matrix representation
const Matrix& matrix() const { inline const Matrix3& matrix() const {
return E_; return E_;
} }
@ -131,20 +121,7 @@ public:
*/ */
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const { 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;
}
/** /**
* Given essential matrix E in camera frame B, convert to body frame C * 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 * @param E essential matrix E in camera frame C
*/ */
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE = EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
boost::none, boost::optional<Matrix&> HR = boost::none) const { 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);
}
}
/** /**
* Given essential matrix E in camera frame B, convert to body frame C * Given essential matrix E in camera frame B, convert to body frame C
@ -194,17 +142,18 @@ public:
/// epipolar error, algebraic /// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, // double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const;
if (H) {
H->resize(1, 5); /// @}
// See math.lyx
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); /// @name Streaming operators
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) /// @{
* aTb_.basis();
*H << HR, HD; /// stream to stream
} friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
return dot(vA, E_ * vB);
} /// 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); } static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/// Log map around identity - just return the Point2 as a vector /// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); }
/// @} /// @}
/// @name Vector Space /// @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 { 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_; return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const { 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 { 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_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -62,36 +63,53 @@ Point3 Point3::operator/(double s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::add(const Point3 &q, Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3); if (H1)
if (H2) *H2 = eye(3,3); *H1 = eye(3, 3);
if (H2)
*H2 = eye(3, 3);
return *this + q; return *this + q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3); if (H1)
if (H2) *H2 = -eye(3,3); *H1 = eye(3, 3);
if (H2)
*H2 = -eye(3, 3);
return *this - q; return *this - q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const { Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_, return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
z_*q.x_ - x_*q.z_, x_ * q.y_ - y_ * q.x_);
x_*q.y_ - y_*q.x_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Point3::dot(const Point3 &q) const { 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 { 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 */ /** Distance of the point from the origin */
double norm() const; double norm() const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
/** cross product @return this x q */ /** cross product @return this x q */
Point3 cross(const Point3 &q) const; 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) { Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia // 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); 0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
static const int N = 5; // order of approximation static const int N = 5; // order of approximation
Matrix res = I6; 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 ///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) { 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, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Rot3.cpp * @file Rot3.cpp
* @brief Rotation, common code between Rotation matrix and Quaternion * @brief Rotation, common code between Rotation matrix and Quaternion
* @author Alireza Fathi * @author Alireza Fathi
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <boost/random.hpp> #include <boost/random.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity(); static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */ /* ************************************************************************* */
void Rot3::print(const std::string& s) const { void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s); gtsam::print((Matrix)matrix(), s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) { Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta); return rodriguez((Vector)w.vector(),theta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
return rodriguez(w.point3(),theta); return rodriguez(w.point3(),theta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Random(boost::random::mt19937 & rng) { Rot3 Rot3::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-( // TODO allow any engine without including all of boost :-(
Sphere2 w = Sphere2::Random(rng); Sphere2 w = Sphere2::Random(rng);
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI); boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
double angle = randomAngle(rng); double angle = randomAngle(rng);
return rodriguez(w,angle); return rodriguez(w,angle);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) { Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm(); double t = w.norm();
if (t < 1e-10) return Rot3(); if (t < 1e-10) return Rot3();
return rodriguez(w/t, t); return rodriguez(w/t, t);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const { bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol); return equal_with_abs_tol(matrix(), R.matrix(), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const { Point3 Rot3::operator*(const Point3& p) const {
return rotate(p); return rotate(p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Rot3::rotate(const Sphere2& p, Sphere2 Rot3::rotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const { boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = rotate(p.point3(Hp)); Sphere2 q = rotate(p.point3(Hp));
if (Hp) if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp); (*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR) if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew(); (*HR) = -q.basis().transpose() * matrix() * p.skew();
return q; return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Rot3::operator*(const Sphere2& p) const { Sphere2 Rot3::unrotate(const Sphere2& p,
return rotate(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);
// see doc/math.lyx, SO(3) section if (HR)
Point3 Rot3::unrotate(const Point3& p, (*HR) = q.basis().transpose() * q.skew();
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { return q;
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; Sphere2 Rot3::operator*(const Sphere2& p) const {
return q; return rotate(p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) // see doc/math.lyx, SO(3) section
Matrix3 Rot3::dexpL(const Vector3& v) { Point3 Rot3::unrotate(const Point3& p,
if(zero(v)) return eye(3); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix x = skewSymmetric(v); const Matrix Rt(transpose());
Matrix x2 = x*x; Point3 q(Rt*p.vector()); // q = Rt*p
double theta = v.norm(), vi = theta/2.0; if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
double s1 = sin(vi)/vi; if (H2) *H2 = Rt;
double s2 = (theta - sin(theta))/(theta*theta*theta); return q;
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; }
return res;
} /* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
/* ************************************************************************* */ Matrix3 Rot3::dexpL(const Vector3& v) {
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) if(zero(v)) return eye(3);
Matrix3 Rot3::dexpInvL(const Vector3& v) { Matrix x = skewSymmetric(v);
if(zero(v)) return eye(3); Matrix x2 = x*x;
Matrix x = skewSymmetric(v); double theta = v.norm(), vi = theta/2.0;
Matrix x2 = x*x; double s1 = sin(vi)/vi;
double theta = v.norm(), vi = theta/2.0; double s2 = (theta - sin(theta))/(theta*theta*theta);
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
Matrix res = eye(3) + 0.5*x - s2*x2; return res;
return res; }
}
/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
/* ************************************************************************* */ Matrix3 Rot3::dexpInvL(const Vector3& v) {
Point3 Rot3::column(int index) const{ if(zero(v)) return eye(3);
if(index == 3) Matrix x = skewSymmetric(v);
return r3(); Matrix x2 = x*x;
else if(index == 2) double theta = v.norm(), vi = theta/2.0;
return r2(); double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
else if(index == 1) Matrix res = eye(3) + 0.5*x - s2*x2;
return r1(); // default returns r1 return res;
else }
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
/* ************************************************************************* */
/* ************************************************************************* */ Point3 Rot3::column(int index) const{
Vector3 Rot3::xyz() const { if(index == 3)
Matrix I;Vector3 q; return r3();
boost::tie(I,q)=RQ(matrix()); else if(index == 2)
return q; return r2();
} else if(index == 1)
return r1(); // default returns r1
/* ************************************************************************* */ else
Vector3 Rot3::ypr() const { throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
Vector3 q = xyz(); }
return Vector3(q(2),q(1),q(0));
} /* ************************************************************************* */
Vector3 Rot3::xyz() const {
/* ************************************************************************* */ Matrix I;Vector3 q;
Vector3 Rot3::rpy() const { boost::tie(I,q)=RQ(matrix());
return xyz(); return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3::quaternion() const { Vector3 Rot3::ypr() const {
Quaternion q = toQuaternion(); Vector3 q = xyz();
Vector v(4); return Vector3(q(2),q(1),q(0));
v(0) = q.w(); }
v(1) = q.x();
v(2) = q.y(); /* ************************************************************************* */
v(3) = q.z(); Vector3 Rot3::rpy() const {
return v; return xyz();
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) { Vector Rot3::quaternion() const {
Quaternion q = toQuaternion();
double x = -atan2(-A(2, 1), A(2, 2)); Vector v(4);
Rot3 Qx = Rot3::Rx(-x); v(0) = q.w();
Matrix3 B = A * Qx.matrix(); v(1) = q.x();
v(2) = q.y();
double y = -atan2(B(2, 0), B(2, 2)); v(3) = q.z();
Rot3 Qy = Rot3::Ry(-y); return v;
Matrix3 C = B * Qy.matrix(); }
double z = -atan2(-C(1, 0), C(1, 1)); /* ************************************************************************* */
Rot3 Qz = Rot3::Rz(-z); pair<Matrix3, Vector3> RQ(const Matrix3& A) {
Matrix3 R = C * Qz.matrix();
double x = -atan2(-A(2, 1), A(2, 2));
Vector xyz = Vector3(x, y, z); Rot3 Qx = Rot3::Rx(-x);
return make_pair(R, xyz); Matrix3 B = A * Qx.matrix();
}
double y = -atan2(B(2, 0), B(2, 2));
/* ************************************************************************* */ Rot3 Qy = Rot3::Ry(-y);
ostream &operator<<(ostream &os, const Rot3& R) { Matrix3 C = B * Qy.matrix();
os << "\n";
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; double z = -atan2(-C(1, 0), C(1, 1));
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; Rot3 Qz = Rot3::Rz(-z);
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; Matrix3 R = C * Qz.matrix();
return os;
} Vector xyz = Vector3(x, y, z);
return make_pair(R, xyz);
/* ************************************************************************* */ }
} // namespace gtsam /* ************************************************************************* */
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 * @return incremental rotation matrix
*/ */
static Rot3 rodriguez(double wx, double wy, double wz) 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 /// @name Testable
@ -331,6 +331,10 @@ namespace gtsam {
Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none, Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const; 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 /// rotate 3D direction from rotated coordinate frame to world frame
Sphere2 operator*(const Sphere2& p) const; Sphere2 operator*(const Sphere2& p) const;

616
gtsam/geometry/Rot3M.cpp
View File

@ -1,308 +1,308 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Rot3M.cpp * @file Rot3M.cpp
* @brief Rotation (internal: 3*3 matrix representation*) * @brief Rotation (internal: 3*3 matrix representation*)
* @author Alireza Fathi * @author Alireza Fathi
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#ifndef GTSAM_USE_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity(); static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {} Rot3::Rot3() : rot_(Matrix3::Identity()) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1.vector(); rot_.col(0) = col1.vector();
rot_.col(1) = col2.vector(); rot_.col(1) = col2.vector();
rot_.col(2) = col3.vector(); rot_.col(2) = col3.vector();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13, Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23, double R21, double R22, double R23,
double R31, double R32, double R33) { double R31, double R32, double R33) {
rot_ << R11, R12, R13, rot_ << R11, R12, R13,
R21, R22, R23, R21, R22, R23,
R31, R32, R33; R31, R32, R33;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) { Rot3::Rot3(const Matrix3& R) {
rot_ = R; rot_ = R;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Matrix& R) { Rot3::Rot3(const Matrix& R) {
if (R.rows()!=3 || R.cols()!=3) if (R.rows()!=3 || R.cols()!=3)
throw invalid_argument("Rot3 constructor expects 3*3 matrix"); throw invalid_argument("Rot3 constructor expects 3*3 matrix");
rot_ = R; rot_ = R;
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {} //Rot3::Rot3(const Matrix3& R) : rot_(R) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { Rot3 Rot3::Rx(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
1, 0, 0, 1, 0, 0,
0, ct,-st, 0, ct,-st,
0, st, ct); 0, st, ct);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Ry(double t) { Rot3 Rot3::Ry(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
ct, 0, st, ct, 0, st,
0, 1, 0, 0, 1, 0,
-st, 0, ct); -st, 0, ct);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rz(double t) { Rot3 Rot3::Rz(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
ct,-st, 0, ct,-st, 0,
st, ct, 0, st, ct, 0,
0, 0, 1); 0, 0, 1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Considerably faster than composing matrices above ! // Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) { Rot3 Rot3::RzRyRx(double x, double y, double z) {
double cx=cos(x),sx=sin(x); double cx=cos(x),sx=sin(x);
double cy=cos(y),sy=sin(y); double cy=cos(y),sy=sin(y);
double cz=cos(z),sz=sin(z); double cz=cos(z),sz=sin(z);
double ss_ = sx * sy; double ss_ = sx * sy;
double cs_ = cx * sy; double cs_ = cx * sy;
double sc_ = sx * cy; double sc_ = sx * cy;
double cc_ = cx * cy; double cc_ = cx * cy;
double c_s = cx * sz; double c_s = cx * sz;
double s_s = sx * sz; double s_s = sx * sz;
double _cs = cy * sz; double _cs = cy * sz;
double _cc = cy * cz; double _cc = cy * cz;
double s_c = sx * cz; double s_c = sx * cz;
double c_c = cx * cz; double c_c = cx * cz;
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
return Rot3( return Rot3(
_cc,- c_s + ssc, s_s + csc, _cc,- c_s + ssc, s_s + csc,
_cs, c_c + sss, -s_c + css, _cs, c_c + sss, -s_c + css,
-sy, sc_, cc_ -sy, sc_, cc_
); );
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
// get components of axis \omega // get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2); double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG #ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz; 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"); if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif #endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c; double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double swx = wx * s, swy = wy * s, swz = wz * s; 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 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 C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz; double C22 = c_1*wwTzz;
return Rot3( return Rot3(
c + C00, -swz + C01, swy + C02, c + C00, -swz + C01, swy + C02,
swz + C01, c + C11, -swx + C12, swz + C01, c + C11, -swx + C12,
-swy + C02, swx + C12, c + C22); -swy + C02, swx + C12, c + C22);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2, Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose(); if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3; if (H2) *H2 = I3;
return *this * R2; return *this * R2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_)); return Rot3(Matrix3(rot_*R2.rot_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const { Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_; if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose())); return Rot3(Matrix3(rot_.transpose()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2, Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_); if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3; if (H2) *H2 = I3;
return Rot3(Matrix3(rot_.transpose()*R2.rot_)); return Rot3(Matrix3(rot_.transpose()*R2.rot_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p, Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1 || H2) { if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_; if (H2) *H2 = rot_;
} }
return Point3(rot_ * p.vector()); return Point3(rot_ * p.vector());
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R) {
static const double PI = boost::math::constants::pi<double>(); static const double PI = boost::math::constants::pi<double>();
const Matrix3& rot = R.rot_; const Matrix3& rot = R.rot_;
// Get trace(R) // Get trace(R)
double tr = rot.trace(); double tr = rot.trace();
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (std::abs(tr+1.0) < 1e-10) { if (std::abs(tr+1.0) < 1e-10) {
if(std::abs(rot(2,2)+1.0) > 1e-10) if(std::abs(rot(2,2)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(2,2) )) * return (PI / sqrt(2.0+2.0*rot(2,2) )) *
Vector3(rot(0,2), rot(1,2), 1.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) else if(std::abs(rot(1,1)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(1,1))) * return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,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 else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (PI / sqrt(2.0+2.0*rot(0,0))) * return (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else { } else {
double magnitude; double magnitude;
double tr_3 = tr-3.0; // always negative double tr_3 = tr-3.0; // always negative
if (tr_3<-1e-7) { if (tr_3<-1e-7) {
double theta = acos((tr-1.0)/2.0); double theta = acos((tr-1.0)/2.0);
magnitude = theta/(2.0*sin(theta)); magnitude = theta/(2.0*sin(theta));
} else { } else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // 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) // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0; magnitude = 0.5 - tr_3*tr_3/12.0;
} }
return magnitude*Vector3( return magnitude*Vector3(
rot(2,1)-rot(1,2), rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0), rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1)); rot(1,0)-rot(0,1));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const { Rot3 Rot3::retractCayley(const Vector& omega) const {
const double x = omega(0), y = omega(1), z = omega(2); const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x * x, y2 = y * y, z2 = z * z; const double x2 = x * x, y2 = y * y, z2 = z * z;
const double xy = x * y, xz = x * z, yz = y * 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; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
return (*this) return (*this)
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, * 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, (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); (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) { if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega); return (*this)*Expmap(omega);
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
return retractCayley(omega); return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega); Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2); return (*this)*Cayley<3>(-Omega/2);
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) { if(mode == Rot3::EXPMAP) {
return Logmap(between(T)); return Logmap(between(T));
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// Mathematica closed form optimization (procrastination?) gone wild: // Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2); 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 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 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 di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i; 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 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 x = (a * f - cd + f) * K;
const double y = (b * f - ce - c) * K; const double y = (b * f - ce - c) * K;
const double z = (fg - di - d) * K; const double z = (fg - di - d) * K;
return -2 * Vector3(x, y, z); return -2 * Vector3(x, y, z);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley // using templated version of Cayley
Eigen::Matrix3d Omega = Cayley<3>(A); Eigen::Matrix3d Omega = Cayley<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const { Matrix3 Rot3::matrix() const {
return rot_; return rot_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::transpose() const { Matrix3 Rot3::transpose() const {
return rot_.transpose(); return rot_.transpose();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); } Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.col(2)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
/* ************************************************************************* */ /* ************************************************************************* */
Quaternion Rot3::toQuaternion() const { Quaternion Rot3::toQuaternion() const {
return Quaternion(rot_); return Quaternion(rot_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam
#endif #endif

102
gtsam/geometry/Sphere2.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011 * @date Feb 02, 2011
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert * @author Frank Dellaert
* @author Alex Trevor
* @brief The Sphere2 class - basically a point on a unit sphere * @brief The Sphere2 class - basically a point on a unit sphere
*/ */
@ -27,6 +28,21 @@ using namespace std;
namespace gtsam { 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) { Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-( // 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 // Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_); Vector p = Point3::Logmap(p_);
@ -106,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const {
// Compute the 3D xi_hat vector // Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); 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; // Avoid nan
// same as putting it onto the unit circle if (xi_hat_norm == 0.0) {
Vector projected = newPoint / newPoint.norm(); 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, if (mode == Sphere2::EXPMAP) {
// we can project x + xi_hat from the tangent space at x to y. Matrix B = basis();
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
double theta = acos(p.transpose() * q);
// Get the basis matrix // the below will be nan if theta == 0.0
Matrix B = basis(); if (p == q)
return (Vector (2) << 0, 0);
// Create the vector forms of p and q (the Point3 of y). else if (p == (Vector)-q)
Vector p = Point3::Logmap(p_); return (Vector (2) << M_PI, 0);
Vector q = Point3::Logmap(y.p_);
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q). Vector result = B.transpose() * result_hat;
double alpha = p.transpose() * q;
assert(alpha != 0.0); return result;
Matrix coeffs = (B.transpose() * q) / alpha; } else if (mode == Sphere2::RENORM) {
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); // Make sure that the angle different between x and y is less than 90. Otherwise,
return result; // 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 * @date Feb 02, 2011
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert * @author Frank Dellaert
* @author Alex Trevor
* @brief Develop a Sphere2 class - basically a point on a unit sphere * @brief Develop a Sphere2 class - basically a point on a unit sphere
*/ */
@ -22,6 +23,10 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.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 // (Cumbersome) forward declaration for random generator
namespace boost { namespace boost {
namespace random { namespace random {
@ -65,6 +70,10 @@ public:
p_ = p_ / p_.norm(); 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 /// Random direction, using boost::uniform_on_sphere
static Sphere2 Random(boost::random::mt19937 & rng); static Sphere2 Random(boost::random::mt19937 & rng);
@ -85,7 +94,11 @@ public:
/// @name Other functionality /// @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; Matrix basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere /// Return skew-symmetric associated with 3D point on unit sphere
@ -98,6 +111,13 @@ public:
return p_; 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 /// Signed, vector-valued error between two directions
Vector error(const Sphere2& q, Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const;
@ -121,11 +141,16 @@ public:
return 2; return 2;
} }
enum CoordinatesMode {
EXPMAP, ///< Use the exponential map to retract
RENORM ///< Retract with vector addtion and renormalize.
};
/// The retract function /// 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 /// 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 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 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() ; 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 ; Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ; Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
Point2 q = K.uncalibrate(p); 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/numericalDerivative.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <sstream>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -95,6 +96,38 @@ TEST (EssentialMatrix, rotate) {
// Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8)); // 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() { int main() {
TestResult tr; TestResult tr;

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

@ -12,75 +12,86 @@
/** /**
* @file testPoint3.cpp * @file testPoint3.cpp
* @brief Unit tests for Point3 class * @brief Unit tests for Point3 class
*/ */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3) GTSAM_CONCEPT_TESTABLE_INST(Point3)
GTSAM_CONCEPT_LIE_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) { TEST(Point3, Lie) {
Point3 p1(1,2,3); Point3 p1(1, 2, 3);
Point3 p2(4,5,6); Point3 p2(4, 5, 6);
Matrix H1, H2; 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), H1));
EXPECT(assert_equal(eye(3), H2)); 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), H1));
EXPECT(assert_equal(eye(3), H2)); EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.retract((Vector(3) << 4., 5., 6.)))); 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((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, arithmetic) TEST( Point3, arithmetic) {
{ CHECK(P * 3 == 3 * P);
CHECK(P*3==3*P); CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
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(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(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,8,6), Point3(1,4,3)*2)); CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal( Point3(2,2,6), 2*Point3(1,1,3))); CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
CHECK(assert_equal( Point3(1,2,3), Point3(2,4,6)/2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, equals) TEST( Point3, equals) {
{
CHECK(P.equals(P)); CHECK(P.equals(P));
Point3 Q; Point3 Q;
CHECK(!P.equals(Q)); CHECK(!P.equals(Q));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, dot) TEST( Point3, dot) {
{ Point3 origin, ones(1, 1, 1);
Point3 origin, ones(1,1,1); CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(origin.dot(Point3(1,1,0)) == 0); CHECK(ones.dot(Point3(1, 1, 0)) == 2);
CHECK(ones.dot(Point3(1,1,0)) == 2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, stream) TEST( Point3, stream) {
{ Point3 p(1, 2, -3);
Point3 p(1,2, -3);
std::ostringstream os; std::ostringstream os;
os << p; os << p;
EXPECT(os.str() == "[1, 2, -3]';"); 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 rot0(Rot2::fromAngle(M_PI/2.0));
Rot2 rot(Rot2::fromAngle(M_PI)); 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); Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }

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

@ -13,6 +13,8 @@
* @file testSphere2.cpp * @file testSphere2.cpp
* @date Feb 03, 2012 * @date Feb 03, 2012
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Tests the Sphere2 class * @brief Tests the Sphere2 class
*/ */
@ -25,6 +27,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/random.hpp> #include <boost/random.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <cmath>
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; 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) { TEST(Sphere2, error) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0)); 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, 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.447214, 0), p.error(q), 1e-5));
EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 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) { TEST(Sphere2, distance) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0)); r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
@ -146,9 +174,20 @@ TEST(Sphere2, retract) {
Vector v(2); Vector v(2);
v << 0.5, 0; v << 0.5, 0;
Sphere2 expected(Point3(1, 0, 0.5)); 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(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) { TEST(Sphere2, localCoordinates_retract) {
size_t numIterations = 10000; size_t numIterations = 10000;
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
Vector_(3, 1.0, 1.0, 1.0); (Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 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++) { for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first). // 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 ) //TEST( Pose2, between )
//{ //{
@ -216,7 +288,7 @@ TEST(Sphere2, localCoordinates_retract) {
// EXPECT(assert_equal(expected,actual1)); // EXPECT(assert_equal(expected,actual1));
// EXPECT(assert_equal(expected,actual2)); // EXPECT(assert_equal(expected,actual2));
// //
// Matrix expectedH1 = Matrix_(3,3, // Matrix expectedH1 = (Matrix(3,3) <<
// 0.0,-1.0,-2.0, // 0.0,-1.0,-2.0,
// 1.0, 0.0,-2.0, // 1.0, 0.0,-2.0,
// 0.0, 0.0,-1.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 // // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); // EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
// //
// Matrix expectedH2 = Matrix_(3,3, // Matrix expectedH2 = (Matrix(3,3) <<
// 1.0, 0.0, 0.0, // 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0, // 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0 // 0.0, 0.0, 1.0
@ -253,6 +325,17 @@ TEST(Sphere2, Random) {
EXPECT(assert_equal(expectedMean,actualMean,0.1)); 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() { int main() {
srand(time(NULL)); srand(time(NULL));

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

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

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

@ -17,23 +17,23 @@
#include <iostream> #include <iostream>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
#define TEST(TITLE,STATEMENT) \ #define TEST(TITLE,STATEMENT) \
gttic_(TITLE); \ gttic_(TITLE); \
for(int i = 0; i < n; i++) \ for(int i = 0; i < n; i++) \
STATEMENT; \ STATEMENT; \
gttoc_(TITLE); gttoc_(TITLE);
int main() int main()
{ {
int n = 5000000; int n = 5000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl; cout << "NOTE: Times are reported for " << n << " calls" << endl;
double norm=sqrt(1.0+16.0+4.0); double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm; 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, T.between(T2))
TEST(between_derivatives, T.between(T2,H1,H2)) TEST(between_derivatives, T.between(T2,H1,H2))
TEST(Logmap, Pose3::Logmap(T.between(T2))) TEST(Logmap, Pose3::Logmap(T.between(T2)))
// Print timings // Print timings
tictoc_print_(); tictoc_print_();
return 0; return 0;
} }

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

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

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

@ -39,7 +39,7 @@ int main()
// create a random direction: // create a random direction:
double norm=sqrt(1.0+16.0+4.0); double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm; 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); 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)) 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, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file BayesNet.h * @file BayesNet.h
* @brief Bayes network * @brief Bayes network
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#pragma once #pragma once
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
namespace gtsam { namespace gtsam {
/** /**
* A BayesNet is a tree of conditionals, stored in elimination order. * 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. * todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping * \nosubgrouping
*/ */
template<class CONDITIONAL> template<class CONDITIONAL>
class BayesNet : public FactorGraph<CONDITIONAL> { class BayesNet : public FactorGraph<CONDITIONAL> {
private: private:
typedef FactorGraph<CONDITIONAL> Base; typedef FactorGraph<CONDITIONAL> Base;
public: public:
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
protected: protected:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Default constructor as an empty BayesNet */ /** Default constructor as an empty BayesNet */
BayesNet() {}; BayesNet() {};
/** Construct from iterator over conditionals */ /** Construct from iterator over conditionals */
template<typename ITERATOR> template<typename ITERATOR>
BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {}
/// @} /// @}
public: public:
/// @name Testable /// @name Testable
/// @{ /// @{
/** print out graph */ /** print out graph */
void print(const std::string& s = "BayesNet", void print(const std::string& s = "BayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; 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, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file BayesTree.h * @file BayesTree.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain * @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert * @author Frank Dellaert
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <string> #include <string>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h> #include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
template<class FACTOR> class FactorGraph; template<class FACTOR> class FactorGraph;
template<class BAYESTREE, class GRAPH> class ClusterTree; template<class BAYESTREE, class GRAPH> class ClusterTree;
/* ************************************************************************* */ /* ************************************************************************* */
/** clique statistics */ /** clique statistics */
struct GTSAM_EXPORT BayesTreeCliqueStats { struct GTSAM_EXPORT BayesTreeCliqueStats {
double avgConditionalSize; double avgConditionalSize;
std::size_t maxConditionalSize; std::size_t maxConditionalSize;
double avgSeparatorSize; double avgSeparatorSize;
std::size_t maxSeparatorSize; std::size_t maxSeparatorSize;
void print(const std::string& s = "") const ; void print(const std::string& s = "") const ;
}; };
/** store all the sizes */ /** store all the sizes */
struct GTSAM_EXPORT BayesTreeCliqueData { struct GTSAM_EXPORT BayesTreeCliqueData {
FastVector<std::size_t> conditionalSizes; FastVector<std::size_t> conditionalSizes;
FastVector<std::size_t> separatorSizes; FastVector<std::size_t> separatorSizes;
BayesTreeCliqueStats getStats() const; BayesTreeCliqueStats getStats() const;
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Bayes tree * Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, * @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. * 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 * @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. * as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
* *
* \addtogroup Multifrontal * \addtogroup Multifrontal
* \nosubgrouping * \nosubgrouping
*/ */
template<class CLIQUE> template<class CLIQUE>
class BayesTree class BayesTree
{ {
protected: protected:
typedef BayesTree<CLIQUE> This; typedef BayesTree<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
public: public:
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
typedef Clique Node; ///< Synonym for Clique (TODO: remove) typedef Clique Node; ///< Synonym for Clique (TODO: remove)
typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove) typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove)
typedef typename CLIQUE::ConditionalType ConditionalType; typedef typename CLIQUE::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional; typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType; typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesNetType> sharedBayesNet; typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef typename CLIQUE::FactorType FactorType; typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<FactorType> sharedFactor; typedef boost::shared_ptr<FactorType> sharedFactor;
typedef typename CLIQUE::FactorGraphType FactorGraphType; typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph; typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate; typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;
/** A convenience class for a list of shared cliques */ /** A convenience class for a list of shared cliques */
typedef FastList<sharedClique> Cliques; typedef FastList<sharedClique> Cliques;
/** Map from keys to Clique */ /** Map from keys to Clique */
typedef ConcurrentMap<Key, sharedClique> Nodes; typedef ConcurrentMap<Key, sharedClique> Nodes;
protected: protected:
/** Map from indices to Clique */ /** Map from indices to Clique */
Nodes nodes_; Nodes nodes_;
/** Root cliques */ /** Root cliques */
FastVector<sharedClique> roots_; FastVector<sharedClique> roots_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
BayesTree() {} BayesTree() {}
/** Copy constructor */ /** Copy constructor */
BayesTree(const This& other); BayesTree(const This& other);
/// @} /// @}
/** Assignment operator */ /** Assignment operator */
This& operator=(const This& other); This& operator=(const This& other);
/// @name Testable /// @name Testable
/// @{ /// @{
/** check equality */ /** check equality */
bool equals(const This& other, double tol = 1e-9) const; bool equals(const This& other, double tol = 1e-9) const;
public: public:
/** print */ /** print */
void print(const std::string& s = "", void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** number of cliques */ /** number of cliques */
size_t size() const; size_t size() const;
/** Check if there are any cliques in the tree */ /** Check if there are any cliques in the tree */
inline bool empty() const { inline bool empty() const {
return nodes_.empty(); return nodes_.empty();
} }
/** return nodes */ /** return nodes */
const Nodes& nodes() const { return nodes_; } const Nodes& nodes() const { return nodes_; }
/** Access node by variable */ /** Access node by variable */
const sharedNode operator[](Key j) const { return nodes_.at(j); } const sharedNode operator[](Key j) const { return nodes_.at(j); }
/** return root cliques */ /** return root cliques */
const FastVector<sharedClique>& roots() const { return roots_; } const FastVector<sharedClique>& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Key j */ /** alternate syntax for matlab: find the clique that contains the variable with Key j */
const sharedClique& clique(Key j) const { const sharedClique& clique(Key j) const {
typename Nodes::const_iterator c = nodes_.find(j); typename Nodes::const_iterator c = nodes_.find(j);
if(c == nodes_.end()) if(c == nodes_.end())
throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree"); throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
else else
return c->second; return c->second;
} }
/** Gather data on all cliques */ /** Gather data on all cliques */
BayesTreeCliqueData getCliqueData() const; BayesTreeCliqueData getCliqueData() const;
/** Collect number of cliques with cached separator marginals */ /** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
/** Return marginal on any variable. Note that this actually returns a conditional, for which a /** 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. * 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 * 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 * systems, this returns a GaussianConditional, which inherits from JacobianFactor and
* GaussianFactor. */ * GaussianFactor. */
sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
/** /**
* return joint on two variables * return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root * 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; sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
/** /**
* return joint on two variables as a BayesNet * return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root * 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; sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
/** /**
* Read only with side effects * Read only with side effects
*/ */
/** saves the Tree to a text file in GraphViz format */ /** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** /**
* Find parent clique of a conditional. It will look at all parents and * Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering. * return the one with the lowest index in the ordering.
*/ */
template<class CONTAINER> template<class CONTAINER>
Key findParentClique(const CONTAINER& parents) const; Key findParentClique(const CONTAINER& parents) const;
/** Remove all nodes */ /** Remove all nodes */
void clear(); void clear();
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */ /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
void deleteCachedShortcuts(); void deleteCachedShortcuts();
/** /**
* Remove path from clique to root and return that path as factors * Remove path from clique to root and return that path as factors
* plus a list of orphaned subtree roots. Used in removeTop below. * plus a list of orphaned subtree roots. Used in removeTop below.
*/ */
void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans); void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans);
/** /**
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * 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. * Factors and orphans are added to the in/out arguments.
*/ */
void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans); void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans);
/** /**
* Remove the requested subtree. */ * Remove the requested subtree. */
Cliques removeSubtree(const sharedClique& subtree); Cliques removeSubtree(const sharedClique& subtree);
/** Insert a new subtree with known parent clique. This function does not check that the /** 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 * 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. */ * structures associated with adding a subtree, such as populating the nodes index. */
void insertRoot(const sharedClique& subtree); void insertRoot(const sharedClique& subtree);
/** add a clique (top down) */ /** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** Add all cliques in this BayesTree to the specified factor graph */ /** Add all cliques in this BayesTree to the specified factor graph */
void addFactorsToGraph(FactorGraph<FactorType>& graph) const; void addFactorsToGraph(FactorGraph<FactorType>& graph) const;
protected: protected:
/** private helper method for saving the Tree to a text file in GraphViz format */ /** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter, void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
int parentnum = 0) const; int parentnum = 0) const;
/** Gather data on a single clique */ /** Gather data on a single clique */
void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const; void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const;
/** remove a clique: warning, can result in a forest */ /** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique); void removeClique(sharedClique clique);
/** Fill the nodes index for a subtree */ /** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree); void fillNodesIndex(const sharedClique& subtree);
// Friend JunctionTree because it directly fills roots and nodes index. // Friend JunctionTree because it directly fills roots and nodes index.
template<class BAYESRTEE, class GRAPH> friend class ClusterTree; template<class BAYESRTEE, class GRAPH> friend class ClusterTree;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_); ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(roots_); ar & BOOST_SERIALIZATION_NVP(roots_);
} }
/// @} /// @}
}; // BayesTree }; // BayesTree
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType
{ {
public: public:
typedef CLIQUE CliqueType; typedef CLIQUE CliqueType;
typedef typename CLIQUE::ConditionalType Base; typedef typename CLIQUE::ConditionalType Base;
boost::shared_ptr<CliqueType> clique; boost::shared_ptr<CliqueType> clique;
BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique) : BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique) :
clique(clique) clique(clique)
{ {
// Store parent keys in our base type factor so that eliminating those parent keys will pull // Store parent keys in our base type factor so that eliminating those parent keys will pull
// this subtree into the elimination. // this subtree into the elimination.
this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
} }
void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const { void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const {
clique->print(s + "stored clique", formatter); clique->print(s + "stored clique", formatter);
} }
}; };
} /// namespace gtsam } /// namespace gtsam

346
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -1,173 +1,173 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file BayesTreeCliqueBase.h * @file BayesTreeCliqueBase.h
* @brief Base class for cliques of a BayesTree * @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert * @author Richard Roberts and Frank Dellaert
*/ */
#pragma once #pragma once
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
template<class CLIQUE> class BayesTree; template<class CLIQUE> class BayesTree;
template<class GRAPH> struct EliminationTraits; template<class GRAPH> struct EliminationTraits;
/** /**
* This is the base class for BayesTree cliques. The default and standard derived type is * 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 * BayesTreeClique, but some algorithms, like iSAM2, use a different clique type in order to store
* extra data along with the clique. * extra data along with the clique.
* *
* This class is templated on the derived class (i.e. the curiously recursive template pattern). * 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 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 * 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. * they were not then we'd need a virtual class.
* *
* @tparam DERIVED The derived clique type. * @tparam DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type. * @tparam CONDITIONAL The conditional type.
* \nosubgrouping */ * \nosubgrouping */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
class BayesTreeCliqueBase class BayesTreeCliqueBase
{ {
private: private:
typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This; typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
typedef DERIVED DerivedType; typedef DERIVED DerivedType;
typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType; typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr; typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr; typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr; typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
public: public:
typedef FACTORGRAPH FactorGraphType; typedef FACTORGRAPH FactorGraphType;
typedef typename EliminationTraitsType::BayesNetType BayesNetType; typedef typename EliminationTraitsType::BayesNetType BayesNetType;
typedef typename BayesNetType::ConditionalType ConditionalType; typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional; typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename FactorGraphType::FactorType FactorType; typedef typename FactorGraphType::FactorType FactorType;
typedef typename FactorGraphType::Eliminate Eliminate; typedef typename FactorGraphType::Eliminate Eliminate;
protected: protected:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Default constructor */ /** Default constructor */
BayesTreeCliqueBase() : problemSize_(1) {} BayesTreeCliqueBase() : problemSize_(1) {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */ /** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {} BayesTreeCliqueBase(const sharedConditional& conditional) : conditional_(conditional), problemSize_(1) {}
/// @} /// @}
/// This stores the Cached separator margnal P(S) /// This stores the Cached separator margnal P(S)
mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_; mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
public: public:
sharedConditional conditional_; sharedConditional conditional_;
derived_weak_ptr parent_; derived_weak_ptr parent_;
FastVector<derived_ptr> children; FastVector<derived_ptr> children;
int problemSize_; int problemSize_;
/// Fill the elimination result produced during elimination. Here this just stores the /// Fill the elimination result produced during elimination. Here this just stores the
/// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique
/// to also cache the remaining factor. /// to also cache the remaining factor.
void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
/// @name Testable /// @name Testable
/// @{ /// @{
/** check equality */ /** check equality */
bool equals(const DERIVED& other, double tol = 1e-9) const; bool equals(const DERIVED& other, double tol = 1e-9) const;
/** print this node */ /** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** Access the conditional */ /** Access the conditional */
const sharedConditional& conditional() const { return conditional_; } const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */ /** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired(); } inline bool isRoot() const { return parent_.expired(); }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */ /** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const; size_t treeSize() const;
/** Collect number of cliques with cached separator marginals */ /** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
/** return a shared_ptr to the parent clique */ /** return a shared_ptr to the parent clique */
derived_ptr parent() const { return parent_.lock(); } derived_ptr parent() const { return parent_.lock(); }
/** Problem size (used for parallel traversal) */ /** Problem size (used for parallel traversal) */
int problemSize() const { return problemSize_; } int problemSize() const { return problemSize_; }
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** return the conditional P(S|Root) on the separator given the root */ /** return the conditional P(S|Root) on the separator given the root */
BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const; BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const;
/** return the marginal P(S) on the separator */ /** return the marginal P(S) on the separator */
FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const; FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
/** return the marginal P(C) of the clique, using marginal caching */ /** return the marginal P(C) of the clique, using marginal caching */
FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const; FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
/** /**
* This deletes the cached shortcuts of all cliques (subtree) below this clique. * This deletes the cached shortcuts of all cliques (subtree) below this clique.
* This is performed when the bayes tree is modified. * This is performed when the bayes tree is modified.
*/ */
void deleteCachedShortcuts(); void deleteCachedShortcuts();
const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const { const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; } return cachedSeparatorMarginal_; }
friend class BayesTree<DerivedType>; friend class BayesTree<DerivedType>;
protected: protected:
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations /// Calculate set \f$ S \setminus B \f$ for shortcut calculations
FastVector<Key> separator_setminus_B(const derived_ptr& B) const; FastVector<Key> separator_setminus_B(const derived_ptr& B) const;
/** Determine variable indices to keep in recursive separator shortcut calculation The factor /** 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 * 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. */ * not in S union B. */
FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */ /** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_); ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_); ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children); ar & BOOST_SERIALIZATION_NVP(children);
} }
/// @} /// @}
}; };
} }

304
gtsam/inference/Conditional.h
View File

@ -1,152 +1,152 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Conditional.h * @file Conditional.h
* @brief Base class for conditional densities * @brief Base class for conditional densities
* @author Frank Dellaert * @author Frank Dellaert
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <boost/range.hpp> #include <boost/range.hpp>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
/** /**
* TODO: Update comments. The following comments are out of date!!! * TODO: Update comments. The following comments are out of date!!!
* *
* Base class for conditional densities, templated on KEY type. This class * Base class for conditional densities, templated on KEY type. This class
* provides storage for the keys involved in a conditional, and iterators and * provides storage for the keys involved in a conditional, and iterators and
* access to the frontal and separator keys. * access to the frontal and separator keys.
* *
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer * 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 * to the associated factor type and shared_ptr type of the derived class. See
* IndexConditional and GaussianConditional for examples. * IndexConditional and GaussianConditional for examples.
* \nosubgrouping * \nosubgrouping
*/ */
template<class FACTOR, class DERIVEDCONDITIONAL> template<class FACTOR, class DERIVEDCONDITIONAL>
class Conditional class Conditional
{ {
protected: protected:
/** The first nrFrontal variables are frontal and the rest are parents. */ /** The first nrFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_; size_t nrFrontals_;
private: private:
/// Typedef to this class /// Typedef to this class
typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This; typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
public: public:
/** View of the frontal keys (call frontals()) */ /** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals; typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
/** View of the separator keys (call parents()) */ /** View of the separator keys (call parents()) */
typedef boost::iterator_range<typename FACTOR::const_iterator> Parents; typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
protected: protected:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Empty Constructor to make serialization possible */ /** Empty Constructor to make serialization possible */
Conditional() : nrFrontals_(0) {} Conditional() : nrFrontals_(0) {}
/** Constructor */ /** Constructor */
Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {} Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/** print with optional formatter */ /** print with optional formatter */
void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const; void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** check equality */ /** check equality */
bool equals(const This& c, double tol = 1e-9) const; bool equals(const This& c, double tol = 1e-9) const;
/// @} /// @}
public: public:
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** return the number of frontals */ /** return the number of frontals */
size_t nrFrontals() const { return nrFrontals_; } size_t nrFrontals() const { return nrFrontals_; }
/** return the number of parents */ /** return the number of parents */
size_t nrParents() const { return asFactor().size() - nrFrontals_; } size_t nrParents() const { return asFactor().size() - nrFrontals_; }
/** Convenience function to get the first frontal key */ /** Convenience function to get the first frontal key */
Key firstFrontalKey() const { Key firstFrontalKey() const {
if(nrFrontals_ > 0) if(nrFrontals_ > 0)
return asFactor().front(); return asFactor().front();
else else
throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys"); throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
} }
/** return a view of the frontal keys */ /** return a view of the frontal keys */
Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); } Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
/** return a view of the parent keys */ /** return a view of the parent keys */
Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); } Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
/** Iterator pointing to first frontal key. */ /** Iterator pointing to first frontal key. */
typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); } typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
/** Iterator pointing past the last frontal key. */ /** Iterator pointing past the last frontal key. */
typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; } typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
/** Iterator pointing to the first parent key. */ /** Iterator pointing to the first parent key. */
typename FACTOR::const_iterator beginParents() const { return endFrontals(); } typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
/** Iterator pointing past the last parent key. */ /** Iterator pointing past the last parent key. */
typename FACTOR::const_iterator endParents() const { return asFactor().end(); } typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** Mutable version of nrFrontals */ /** Mutable version of nrFrontals */
size_t& nrFrontals() { return nrFrontals_; } size_t& nrFrontals() { return nrFrontals_; }
/** Mutable iterator pointing to first frontal key. */ /** Mutable iterator pointing to first frontal key. */
typename FACTOR::iterator beginFrontals() { return asFactor().begin(); } typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
/** Mutable iterator pointing past the last frontal key. */ /** Mutable iterator pointing past the last frontal key. */
typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; } typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
/** Mutable iterator pointing to the first parent key. */ /** Mutable iterator pointing to the first parent key. */
typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; } typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
/** Mutable iterator pointing past the last parent key. */ /** Mutable iterator pointing past the last parent key. */
typename FACTOR::iterator endParents() { return asFactor().end(); } typename FACTOR::iterator endParents() { return asFactor().end(); }
private: private:
// Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type) // 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)); } 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) // 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)); } const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nrFrontals_); ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
} }
/// @} /// @}
}; };
} // gtsam } // gtsam

336
gtsam/inference/EliminationTree.h
View File

@ -1,168 +1,168 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file EliminationTree.h * @file EliminationTree.h
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
* @date Oct 13, 2010 * @date Oct 13, 2010
*/ */
#pragma once #pragma once
#include <utility> #include <utility>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree class EliminationTreeTester; // for unit tests, see testEliminationTree
namespace gtsam { namespace gtsam {
class VariableIndex; class VariableIndex;
class Ordering; class Ordering;
/** /**
* An elimination tree is a data structure used intermediately during * An elimination tree is a data structure used intermediately during
* elimination. In future versions it will be used to save work between * elimination. In future versions it will be used to save work between
* multiple eliminations. * multiple eliminations.
* *
* When a variable is eliminated, a new factor is created by combining that * When a variable is eliminated, a new factor is created by combining that
* variable's neighboring factors. The new combined factor involves the combined * variable's neighboring factors. The new combined factor involves the combined
* factors' involved variables. When the lowest-ordered one of those variables * factors' involved variables. When the lowest-ordered one of those variables
* is eliminated, it consumes that combined factor. In the elimination tree, * 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 * 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 * 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. * because of the implicit sparse structure of the resulting Bayes net.
* *
* This structure is examined even more closely in a JunctionTree, which * This structure is examined even more closely in a JunctionTree, which
* additionally identifies cliques in the chordal Bayes net. * additionally identifies cliques in the chordal Bayes net.
* \nosubgrouping * \nosubgrouping
*/ */
template<class BAYESNET, class GRAPH> template<class BAYESNET, class GRAPH>
class EliminationTree class EliminationTree
{ {
protected: protected:
typedef EliminationTree<BAYESNET, GRAPH> This; ///< This class typedef EliminationTree<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
public: public:
typedef GRAPH FactorGraphType; ///< The factor graph type typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename GRAPH::Eliminate Eliminate; typedef typename GRAPH::Eliminate Eliminate;
struct Node { struct Node {
typedef FastVector<sharedFactor> Factors; typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Node> > Children; typedef FastVector<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root Key key; ///< key associated with root
Factors factors; ///< factors associated with root Factors factors; ///< factors associated with root
Children children; ///< sub-trees Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output, sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const; const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const;
void print(const std::string& str, const KeyFormatter& keyFormatter) const; void print(const std::string& str, const KeyFormatter& keyFormatter) const;
}; };
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
protected: protected:
/** concept check */ /** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
FastVector<sharedNode> roots_; FastVector<sharedNode> roots_;
FastVector<sharedFactor> remainingFactors_; FastVector<sharedFactor> remainingFactors_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** /**
* Build the elimination tree of a factor graph using pre-computed column structure. * 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 factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not * @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&) * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead. * named constructor instead.
* @return The elimination tree * @return The elimination tree
*/ */
EliminationTree(const FactorGraphType& factorGraph, EliminationTree(const FactorGraphType& factorGraph,
const VariableIndex& structure, const Ordering& order); const VariableIndex& structure, const Ordering& order);
/** Build the elimination tree of a factor graph. Note that this has to compute the column /** 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 * structure as a VariableIndex, so if you already have this precomputed, use the other
* constructor instead. * constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree * @param factorGraph The factor graph for which to build the elimination tree
*/ */
EliminationTree(const FactorGraphType& factorGraph, const Ordering& order); EliminationTree(const FactorGraphType& factorGraph, const Ordering& order);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */ * copied, factors are not cloned. */
EliminationTree(const This& other) { *this = other; } EliminationTree(const This& other) { *this = other; }
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
* are copied, factors are not cloned. */ * are copied, factors are not cloned. */
This& operator=(const This& other); This& operator=(const This& other);
/// @} /// @}
public: public:
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** Eliminate the factors to a Bayes net and remaining factor graph /** Eliminate the factors to a Bayes net and remaining factor graph
* @param function The function to use to eliminate, see the namespace functions * @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h * in GaussianFactorGraph.h
* @return The Bayes net and factor graph resulting from elimination * @return The Bayes net and factor graph resulting from elimination
*/ */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminate(Eliminate function) const; eliminate(Eliminate function) const;
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/** Print the tree to cout */ /** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ", void print(const std::string& name = "EliminationTree: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected: protected:
/** Test whether the tree is equal to another */ /** Test whether the tree is equal to another */
bool equals(const This& other, double tol = 1e-9) const; bool equals(const This& other, double tol = 1e-9) const;
/// @} /// @}
public: public:
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** Return the set of roots (one for a tree, multiple for a forest) */ /** Return the set of roots (one for a tree, multiple for a forest) */
const FastVector<sharedNode>& roots() const { return roots_; } const FastVector<sharedNode>& roots() const { return roots_; }
/** Return the remaining factors that are not pulled into elimination */ /** Return the remaining factors that are not pulled into elimination */
const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; } const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
/** Swap the data of this tree with another one, this operation is very fast. */ /** Swap the data of this tree with another one, this operation is very fast. */
void swap(This& other); void swap(This& other);
protected: protected:
/// Protected default constructor /// Protected default constructor
EliminationTree() {} EliminationTree() {}
private: private:
/// Allow access to constructor and add methods for testing purposes /// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeTester; friend class ::EliminationTreeTester;
}; };
} }

116
gtsam/inference/VariableIndex.cpp
View File

@ -1,58 +1,58 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file VariableIndex.cpp * @file VariableIndex.cpp
* @author Richard Roberts * @author Richard Roberts
* @date March 26, 2013 * @date March 26, 2013
*/ */
#include <iostream> #include <iostream>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
/* ************************************************************************* */ /* ************************************************************************* */
bool VariableIndex::equals(const VariableIndex& other, double tol) const { bool VariableIndex::equals(const VariableIndex& other, double tol) const {
return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_ return this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_
&& this->index_ == other.index_; && this->index_ == other.index_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const { void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str; cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
BOOST_FOREACH(KeyMap::value_type key_factors, index_) { BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
cout << "var " << keyFormatter(key_factors.first) << ":"; cout << "var " << keyFormatter(key_factors.first) << ":";
BOOST_FOREACH(const size_t factor, key_factors.second) BOOST_FOREACH(const size_t factor, key_factors.second)
cout << " " << factor; cout << " " << factor;
cout << "\n"; cout << "\n";
} }
cout.flush(); cout.flush();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VariableIndex::outputMetisFormat(ostream& os) const { void VariableIndex::outputMetisFormat(ostream& os) const {
os << size() << " " << nFactors() << "\n"; os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges. // run over variables, which will be hyper-edges.
BOOST_FOREACH(KeyMap::value_type key_factors, index_) { BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
// every variable is a hyper-edge covering its factors // every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, key_factors.second) BOOST_FOREACH(const size_t factor, key_factors.second)
os << (factor+1) << " "; // base 1 os << (factor+1) << " "; // base 1
os << "\n"; os << "\n";
} }
os << flush; os << flush;
} }
} }

360
gtsam/inference/VariableIndex.h
View File

@ -1,180 +1,180 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file VariableIndex.h * @file VariableIndex.h
* @author Richard Roberts * @author Richard Roberts
* @date March 26, 2013 * @date March 26, 2013
*/ */
#pragma once #pragma once
#include <vector> #include <vector>
#include <deque> #include <deque>
#include <stdexcept> #include <stdexcept>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
/** /**
* The VariableIndex class computes and stores the block column structure of a * The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of * factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built * 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 * 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 * that involve each variable. This information is stored as a deque of
* lists of factor indices. * lists of factor indices.
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT VariableIndex { class GTSAM_EXPORT VariableIndex {
public: public:
typedef boost::shared_ptr<VariableIndex> shared_ptr; typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef FastList<size_t> Factors; typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator; typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator; typedef Factors::const_iterator Factor_const_iterator;
protected: protected:
typedef FastMap<Key,Factors> KeyMap; typedef FastMap<Key,Factors> KeyMap;
KeyMap index_; KeyMap index_;
size_t nFactors_; // Number of factors in the original factor graph. size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor. size_t nEntries_; // Sum of involved variable counts of each factor.
public: public:
typedef KeyMap::const_iterator const_iterator; typedef KeyMap::const_iterator const_iterator;
typedef KeyMap::const_iterator iterator; typedef KeyMap::const_iterator iterator;
typedef KeyMap::value_type value_type; typedef KeyMap::value_type value_type;
public: public:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Default constructor, creates an empty VariableIndex */ /** Default constructor, creates an empty VariableIndex */
VariableIndex() : nFactors_(0), nEntries_(0) {} VariableIndex() : nFactors_(0), nEntries_(0) {}
/** /**
* Create a VariableIndex that computes and stores the block column structure * Create a VariableIndex that computes and stores the block column structure
* of a factor graph. * of a factor graph.
*/ */
template<class FG> template<class FG>
VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); } VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** /**
* The number of variable entries. This is one greater than the variable * The number of variable entries. This is one greater than the variable
* with the highest index. * with the highest index.
*/ */
Key size() const { return index_.size(); } Key size() const { return index_.size(); }
/** The number of factors in the original factor graph */ /** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; } size_t nFactors() const { return nFactors_; }
/** The number of nonzero blocks, i.e. the number of variable-factor entries */ /** The number of nonzero blocks, i.e. the number of variable-factor entries */
size_t nEntries() const { return nEntries_; } size_t nEntries() const { return nEntries_; }
/** Access a list of factors by variable */ /** Access a list of factors by variable */
const Factors& operator[](Key variable) const { const Factors& operator[](Key variable) const {
KeyMap::const_iterator item = index_.find(variable); KeyMap::const_iterator item = index_.find(variable);
if(item == index_.end()) if(item == index_.end())
throw std::invalid_argument("Requested non-existent variable from VariableIndex"); throw std::invalid_argument("Requested non-existent variable from VariableIndex");
else else
return item->second; return item->second;
} }
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/** Test for equality (for unit tests and debug assertions). */ /** Test for equality (for unit tests and debug assertions). */
bool equals(const VariableIndex& other, double tol=0.0) const; bool equals(const VariableIndex& other, double tol=0.0) const;
/** Print the variable index (for unit tests and debugging). */ /** Print the variable index (for unit tests and debugging). */
void print(const std::string& str = "VariableIndex: ", void print(const std::string& str = "VariableIndex: ",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** /**
* Output dual hypergraph to Metis file format for use with hmetis * Output dual hypergraph to Metis file format for use with hmetis
* In the dual graph, variables are hyperedges, factors are nodes. * In the dual graph, variables are hyperedges, factors are nodes.
*/ */
void outputMetisFormat(std::ostream& os) const; void outputMetisFormat(std::ostream& os) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** /**
* Augment the variable index with new factors. This can be used when * Augment the variable index with new factors. This can be used when
* solving problems incrementally. * solving problems incrementally.
*/ */
template<class FG> template<class FG>
void augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices = boost::none); 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 * 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 * 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 * graph does not shift the indices of other factors. Also, we keep nFactors_ one greater than
* the highest-numbered factor referenced in a VariableIndex. * the highest-numbered factor referenced in a VariableIndex.
* *
* @param indices The indices of the factors to remove, which must match \c factors * @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 * @param factors The factors being removed, which must symbolically correspond exactly to the
* factors with the specified \c indices that were added. * factors with the specified \c indices that were added.
*/ */
template<typename ITERATOR, class FG> template<typename ITERATOR, class FG>
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
/** Remove unused empty variables (in debug mode verifies they are empty). */ /** Remove unused empty variables (in debug mode verifies they are empty). */
template<typename ITERATOR> template<typename ITERATOR>
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
/** Iterator to the first variable entry */ /** Iterator to the first variable entry */
const_iterator begin() const { return index_.begin(); } const_iterator begin() const { return index_.begin(); }
/** Iterator to the first variable entry */ /** Iterator to the first variable entry */
const_iterator end() const { return index_.end(); } const_iterator end() const { return index_.end(); }
/** Find the iterator for the requested variable entry */ /** Find the iterator for the requested variable entry */
const_iterator find(Key key) const { return index_.find(key); } const_iterator find(Key key) const { return index_.find(key); }
protected: protected:
Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); } Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); }
Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); } Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); }
Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); } Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); }
Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); } Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
/// Internal version of 'at' that asserts existence /// Internal version of 'at' that asserts existence
const Factors& internalAt(Key variable) const { const Factors& internalAt(Key variable) const {
const KeyMap::const_iterator item = index_.find(variable); const KeyMap::const_iterator item = index_.find(variable);
assert(item != index_.end()); assert(item != index_.end());
return item->second; } return item->second; }
/// Internal version of 'at' that asserts existence /// Internal version of 'at' that asserts existence
Factors& internalAt(Key variable) { Factors& internalAt(Key variable) {
const KeyMap::iterator item = index_.find(variable); const KeyMap::iterator item = index_.find(variable);
assert(item != index_.end()); assert(item != index_.end());
return item->second; } return item->second; }
/// @} /// @}
}; };
} }
#include <gtsam/inference/VariableIndex-inl.h> #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) 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, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file JacobianFactor.h * @file JacobianFactor.h
* @author Richard Roberts * @author Richard Roberts
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @date Dec 8, 2010 * @date Dec 8, 2010
*/ */
#pragma once #pragma once
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/VerticalBlockMatrix.h> #include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class HessianFactor; class HessianFactor;
class VariableSlots; class VariableSlots;
class GaussianFactorGraph; class GaussianFactorGraph;
class GaussianConditional; class GaussianConditional;
class HessianFactor; class HessianFactor;
class VectorValues; class VectorValues;
class Ordering; class Ordering;
class JacobianFactor; class JacobianFactor;
GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
/** /**
* A Gaussian factor in the squared-error form. * A Gaussian factor in the squared-error form.
* *
* JacobianFactor implements a * JacobianFactor implements a
* Gaussian, which has quadratic negative log-likelihood * Gaussian, which has quadratic negative log-likelihood
* \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] * \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 * 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 * matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model
* \f$ \Sigma \f$ are stored in this class. * \f$ \Sigma \f$ are stored in this class.
* *
* This factor represents the sum-of-squares error of a \a linear * This factor represents the sum-of-squares error of a \a linear
* measurement function, and is created upon linearization of a NoiseModelFactor, * measurement function, and is created upon linearization of a NoiseModelFactor,
* which in turn is a sum-of-squares factor with a nonlinear measurement function. * 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: * 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 * Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be
* the actual observed measurement, the residual is * the actual observed measurement, the residual is
* \f[ f(x) = h(x) - z . \f] * \f[ f(x) = h(x) - z . \f]
* If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this * 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, then the negative log-likelihood of the Gaussian induced by this
* measurement model is * measurement model is
* \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f] * \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 * Because \f$ h(x) \f$ is linear, we can write it as
* \f[ h(x) = Ax + e \f] * \f[ h(x) = Ax + e \f]
* and thus we have * and thus we have
* \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f] * \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f]
* where \f$ b = z - e \f$. * where \f$ b = z - e \f$.
* *
* This factor can involve an arbitrary number of variables, and in the * 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 * 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- * in the entire factor graph. There are special constructors for 1-, 2-, and 3-
* way JacobianFactors, and additional constructors for creating n-way JacobianFactors. * way JacobianFactors, and additional constructors for creating n-way JacobianFactors.
* The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks, * 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$, * 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$ * as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$
* and the negative log-likelihood represented by this factor would be * 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] * \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 class GTSAM_EXPORT JacobianFactor : public GaussianFactor
{ {
public: public:
typedef JacobianFactor This; ///< Typedef to this class typedef JacobianFactor This; ///< Typedef to this class
typedef GaussianFactor Base; ///< Typedef to base class typedef GaussianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected: protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
public: public:
typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock; typedef VerticalBlockMatrix::constBlock constABlock;
typedef ABlock::ColXpr BVector; typedef ABlock::ColXpr BVector;
typedef constABlock::ConstColXpr constBVector; typedef constABlock::ConstColXpr constBVector;
/** Convert from other GaussianFactor */ /** Convert from other GaussianFactor */
explicit JacobianFactor(const GaussianFactor& gf); explicit JacobianFactor(const GaussianFactor& gf);
/** Copy constructor */ /** Copy constructor */
JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {} JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
explicit JacobianFactor(const HessianFactor& hf); explicit JacobianFactor(const HessianFactor& hf);
/** default constructor for I/O */ /** default constructor for I/O */
JacobianFactor(); JacobianFactor();
/** Construct Null factor */ /** Construct Null factor */
explicit JacobianFactor(const Vector& b_in); explicit JacobianFactor(const Vector& b_in);
/** Construct unary factor */ /** Construct unary factor */
JacobianFactor(Key i1, const Matrix& A1, JacobianFactor(Key i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const Vector& b, const SharedDiagonal& model = SharedDiagonal());
/** Construct binary factor */ /** Construct binary factor */
JacobianFactor(Key i1, const Matrix& A1, JacobianFactor(Key i1, const Matrix& A1,
Key i2, const Matrix& A2, Key i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const Vector& b, const SharedDiagonal& model = SharedDiagonal());
/** Construct ternary factor */ /** Construct ternary factor */
JacobianFactor(Key i1, const Matrix& A1, Key i2, JacobianFactor(Key i1, const Matrix& A1, Key i2,
const Matrix& A2, Key i3, const Matrix& A3, const Matrix& A2, Key i3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const Vector& b, const SharedDiagonal& model = SharedDiagonal());
/** Construct an n-ary factor /** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */ * collection of keys and matrices making up the factor. */
template<typename TERMS> template<typename TERMS>
JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); 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 /** 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 * 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 * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */ * factor. */
template<typename KEYS> template<typename KEYS>
JacobianFactor( JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); 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 * 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 * structure computed for \c graph is already available, providing it will reduce the amount of
* computation performed. */ * computation performed. */
explicit JacobianFactor( explicit JacobianFactor(
const GaussianFactorGraph& graph, const GaussianFactorGraph& graph,
boost::optional<const Ordering&> ordering = boost::none, boost::optional<const Ordering&> ordering = boost::none,
boost::optional<const VariableSlots&> variableSlots = boost::none); boost::optional<const VariableSlots&> variableSlots = boost::none);
/** Virtual destructor */ /** Virtual destructor */
virtual ~JacobianFactor() {} virtual ~JacobianFactor() {}
/** Clone this JacobianFactor */ /** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>( return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<JacobianFactor>(*this)); boost::make_shared<JacobianFactor>(*this));
} }
// Implementing Testable interface // Implementing Testable interface
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ 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) */ 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. /** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an * The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row * additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry * holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The * contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor, * augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix. * which in fact stores an augmented information matrix.
*/ */
virtual Matrix augmentedInformation() const; virtual Matrix augmentedInformation() const;
/** Return the non-augmented information matrix represented by this /** Return the non-augmented information matrix represented by this
* GaussianFactor. * GaussianFactor.
*/ */
virtual Matrix information() const; virtual Matrix information() const;
/** /**
* @brief Returns (dense) A,b pair associated with factor, bakes in the weights * @brief Returns (dense) A,b pair associated with factor, bakes in the weights
*/ */
virtual std::pair<Matrix, Vector> jacobian() const; virtual std::pair<Matrix, Vector> jacobian() const;
/** /**
* @brief Returns (dense) A,b pair associated with factor, does not bake in weights * @brief Returns (dense) A,b pair associated with factor, does not bake in weights
*/ */
std::pair<Matrix, Vector> jacobianUnweighted() const; std::pair<Matrix, Vector> jacobianUnweighted() const;
/** Return (dense) matrix associated with factor. The returned system is an augmented matrix: /** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
* [A b] * [A b]
* weights are baked in */ * weights are baked in */
virtual Matrix augmentedJacobian() const; virtual Matrix augmentedJacobian() const;
/** Return (dense) matrix associated with factor. The returned system is an augmented matrix: /** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
* [A b] * [A b]
* weights are not baked in */ * weights are not baked in */
Matrix augmentedJacobianUnweighted() const; Matrix augmentedJacobianUnweighted() const;
/** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
const VerticalBlockMatrix& matrixObject() const { return Ab_; } const VerticalBlockMatrix& matrixObject() const { return Ab_; }
/** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
VerticalBlockMatrix& matrixObject() { return Ab_; } VerticalBlockMatrix& matrixObject() { return Ab_; }
/** /**
* Construct the corresponding anti-factor to negate information * Construct the corresponding anti-factor to negate information
* stored stored in this factor. * stored stored in this factor.
* @return a HessianFactor with negated Hessian matrices * @return a HessianFactor with negated Hessian matrices
*/ */
virtual GaussianFactor::shared_ptr negate() const; virtual GaussianFactor::shared_ptr negate() const;
/** Check if the factor is empty. TODO: How should this be defined? */ /** Check if the factor is empty. TODO: How should this be defined? */
virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }
/** is noise model constrained ? */ /** is noise model constrained ? */
bool isConstrained() const { return model_->isConstrained(); } bool isConstrained() const { return model_->isConstrained(); }
/** Return the dimension of the variable pointed to by the given key iterator /** 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? * todo: Remove this in favor of keeping track of dimensions with variables?
*/ */
virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }
/** /**
* return the number of rows in the corresponding linear system * return the number of rows in the corresponding linear system
*/ */
size_t rows() const { return Ab_.rows(); } size_t rows() const { return Ab_.rows(); }
/** /**
* return the number of columns in the corresponding linear system * return the number of columns in the corresponding linear system
*/ */
size_t cols() const { return Ab_.cols(); } size_t cols() const { return Ab_.cols(); }
/** get a copy of model */ /** get a copy of model */
const SharedDiagonal& get_model() const { return model_; } const SharedDiagonal& get_model() const { return model_; }
/** get a copy of model (non-const version) */ /** get a copy of model (non-const version) */
SharedDiagonal& get_model() { return model_; } SharedDiagonal& get_model() { return model_; }
/** Get a view of the r.h.s. vector b, not weighted by noise */ /** Get a view of the r.h.s. vector b, not weighted by noise */
const constBVector getb() const { return Ab_(size()).col(0); } 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 */ /** 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()); } constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
/** Get a view of the A matrix, not weighted by noise */ /** Get a view of the A matrix, not weighted by noise */
constABlock getA() const { return Ab_.range(0, size()); } constABlock getA() const { return Ab_.range(0, size()); }
/** Get a view of the r.h.s. vector b (non-const version) */ /** Get a view of the r.h.s. vector b (non-const version) */
BVector getb() { return Ab_(size()).col(0); } 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) */ /** 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()); } ABlock getA(iterator variable) { return Ab_(variable - begin()); }
/** Get a view of the A matrix */ /** Get a view of the A matrix */
ABlock getA() { return Ab_.range(0, size()); } ABlock getA() { return Ab_.range(0, size()); }
/** Return A*x */ /** Return A*x */
Vector operator*(const VectorValues& x) const; Vector operator*(const VectorValues& x) const;
/** x += A'*e. If x is initially missing any values, they are created and assumed to start as /** x += A'*e. If x is initially missing any values, they are created and assumed to start as
* zero vectors. */ * zero vectors. */
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
/// A'*b for Jacobian /// A'*b for Jacobian
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const; JacobianFactor whiten() const;
/** Eliminate the requested variables. */ /** Eliminate the requested variables. */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
eliminate(const Ordering& keys); eliminate(const Ordering& keys);
/** set noiseModel correctly */ /** set noiseModel correctly */
void setModel(bool anyConstrained, const Vector& sigmas); void setModel(bool anyConstrained, const Vector& sigmas);
/** /**
* Densely partially eliminate with QR factorization, this is usually provided as an argument to * Densely partially eliminate with QR factorization, this is usually provided as an argument to
* one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors
* are first factored with Cholesky decomposition to produce JacobianFactors, by calling the * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the
* conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the
* order specified in \c keys. * order specified in \c keys.
* @param factors Factors to combine and eliminate * @param factors Factors to combine and eliminate
* @param keys The variables to eliminate in the order as specified here in \c keys * @param keys The variables to eliminate in the order as specified here in \c keys
* @return The conditional and remaining factor * @return The conditional and remaining factor
* *
* \addtogroup LinearSolving */ * \addtogroup LinearSolving */
friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
/** /**
* splits a pre-factorized factor into a conditional, and changes the current * splits a pre-factorized factor into a conditional, and changes the current
* factor to be the remaining component. Performs same operation as eliminate(), * factor to be the remaining component. Performs same operation as eliminate(),
* but without running QR. * but without running QR.
*/ */
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals); boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
protected: protected:
/// Internal function to fill blocks and set dimensions /// Internal function to fill blocks and set dimensions
template<typename TERMS> template<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(Ab_);
ar & BOOST_SERIALIZATION_NVP(model_); ar & BOOST_SERIALIZATION_NVP(model_);
} }
}; // JacobianFactor }; // JacobianFactor
} // gtsam } // gtsam
#include <gtsam/linear/JacobianFactor-inl.h> #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: /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed by:
/// Dipl.-Inform. Jan Oberl<72>nder (M.Sc.), FZI Research Center for /// Dipl.-Inform. Jan Oberl<72>nder (M.Sc.), FZI Research Center for
/// Information Technology, Karlsruhe, Germany. /// Information Technology, Karlsruhe, Germany.
/// oberlaender@fzi.de /// oberlaender@fzi.de
/// Thanks Jan! /// Thanks Jan!
class GTSAM_EXPORT Cauchy : public Base { class GTSAM_EXPORT Cauchy : public Base {

16
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -23,18 +23,18 @@
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf); JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) { if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
} }
result->push_back(jf); result->push_back(jf);
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */

694
gtsam/linear/VectorValues.cpp
View File

@ -1,347 +1,347 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file VectorValues.cpp * @file VectorValues.cpp
* @brief Implementations for VectorValues * @brief Implementations for VectorValues
* @author Richard Roberts * @author Richard Roberts
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <boost/range/combine.hpp> #include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp> #include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp> #include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
using boost::combine; using boost::combine;
using boost::adaptors::transformed; using boost::adaptors::transformed;
using boost::adaptors::map_values; using boost::adaptors::map_values;
using boost::accumulate; using boost::accumulate;
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues::VectorValues(const VectorValues& first, const VectorValues& second) VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
{ {
// Merge using predicate for comparing first of pair // Merge using predicate for comparing first of pair
merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()), 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))); boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
if(size() != first.size() + second.size()) if(size() != first.size() + second.size())
throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common."); throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues::VectorValues(const Vector& x, const Dims& dims) { VectorValues::VectorValues(const Vector& x, const Dims& dims) {
typedef pair<Key, size_t> Pair; typedef pair<Key, size_t> Pair;
size_t j = 0; size_t j = 0;
BOOST_FOREACH(const Pair& v, dims) { BOOST_FOREACH(const Pair& v, dims) {
Key key; Key key;
size_t n; size_t n;
boost::tie(key, n) = v; boost::tie(key, n) = v;
values_.insert(make_pair(key, sub(x, j, j + n))); values_.insert(make_pair(key, sub(x, j, j + n)));
j += n; j += n;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::Zero(const VectorValues& other) VectorValues VectorValues::Zero(const VectorValues& other)
{ {
VectorValues result; VectorValues result;
BOOST_FOREACH(const KeyValuePair& v, other) BOOST_FOREACH(const KeyValuePair& v, other)
result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size())));
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::update(const VectorValues& values) void VectorValues::update(const VectorValues& values)
{ {
iterator hint = begin(); iterator hint = begin();
BOOST_FOREACH(const KeyValuePair& key_value, values) 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 // Use this trick to find the value using a hint, since we are inserting from another sorted map
size_t oldSize = values_.size(); size_t oldSize = values_.size();
hint = values_.insert(hint, key_value); hint = values_.insert(hint, key_value);
if(values_.size() > oldSize) { if(values_.size() > oldSize) {
values_.unsafe_erase(hint); values_.unsafe_erase(hint);
throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
} else { } else {
hint->second = key_value.second; hint->second = key_value.second;
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::insert(const VectorValues& values) void VectorValues::insert(const VectorValues& values)
{ {
size_t originalSize = size(); size_t originalSize = size();
values_.insert(values.begin(), values.end()); values_.insert(values.begin(), values.end());
if(size() != originalSize + values.size()) 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."); throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::setZero() void VectorValues::setZero()
{ {
BOOST_FOREACH(Vector& v, values_ | map_values) BOOST_FOREACH(Vector& v, values_ | map_values)
v.setZero(); v.setZero();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::print(const string& str, const KeyFormatter& formatter) const { void VectorValues::print(const string& str, const KeyFormatter& formatter) const {
cout << str << ": " << size() << " elements\n"; cout << str << ": " << size() << " elements\n";
BOOST_FOREACH(const value_type& key_value, *this) BOOST_FOREACH(const value_type& key_value, *this)
cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n";
cout.flush(); cout.flush();
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool VectorValues::equals(const VectorValues& x, double tol) const { bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size()) if(this->size() != x.size())
return false; return false;
typedef boost::tuple<value_type, value_type> ValuePair; typedef boost::tuple<value_type, value_type> ValuePair;
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) {
if(values.get<0>().first != values.get<1>().first || if(values.get<0>().first != values.get<1>().first ||
!equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
return false; return false;
} }
return true; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector VectorValues::vector() const Vector VectorValues::vector() const
{ {
// Count dimensions // Count dimensions
DenseIndex totalDim = 0; DenseIndex totalDim = 0;
BOOST_FOREACH(const Vector& v, *this | map_values) BOOST_FOREACH(const Vector& v, *this | map_values)
totalDim += v.size(); totalDim += v.size();
// Copy vectors // Copy vectors
Vector result(totalDim); Vector result(totalDim);
DenseIndex pos = 0; DenseIndex pos = 0;
BOOST_FOREACH(const Vector& v, *this | map_values) { BOOST_FOREACH(const Vector& v, *this | map_values) {
result.segment(pos, v.size()) = v; result.segment(pos, v.size()) = v;
pos += v.size(); pos += v.size();
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector VectorValues::vector(const FastVector<Key>& keys) const Vector VectorValues::vector(const FastVector<Key>& keys) const
{ {
// Count dimensions and collect pointers to avoid double lookups // Count dimensions and collect pointers to avoid double lookups
DenseIndex totalDim = 0; DenseIndex totalDim = 0;
FastVector<const Vector*> items(keys.size()); FastVector<const Vector*> items(keys.size());
for(size_t i = 0; i < keys.size(); ++i) { for(size_t i = 0; i < keys.size(); ++i) {
items[i] = &at(keys[i]); items[i] = &at(keys[i]);
totalDim += items[i]->size(); totalDim += items[i]->size();
} }
// Copy vectors // Copy vectors
Vector result(totalDim); Vector result(totalDim);
DenseIndex pos = 0; DenseIndex pos = 0;
BOOST_FOREACH(const Vector *v, items) { BOOST_FOREACH(const Vector *v, items) {
result.segment(pos, v->size()) = *v; result.segment(pos, v->size()) = *v;
pos += v->size(); pos += v->size();
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector VectorValues::vector(const Dims& keys) const Vector VectorValues::vector(const Dims& keys) const
{ {
// Count dimensions // Count dimensions
DenseIndex totalDim = 0; DenseIndex totalDim = 0;
BOOST_FOREACH(size_t dim, keys | map_values) BOOST_FOREACH(size_t dim, keys | map_values)
totalDim += dim; totalDim += dim;
Vector result(totalDim); Vector result(totalDim);
size_t j = 0; size_t j = 0;
BOOST_FOREACH(const Dims::value_type& it, keys) { BOOST_FOREACH(const Dims::value_type& it, keys) {
result.segment(j,it.second) = at(it.first); result.segment(j,it.second) = at(it.first);
j += it.second; j += it.second;
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::swap(VectorValues& other) { void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_); this->values_.swap(other.values_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal namespace internal
{ {
bool structureCompareOp(const boost::tuple<VectorValues::value_type, bool structureCompareOp(const boost::tuple<VectorValues::value_type,
VectorValues::value_type>& vv) VectorValues::value_type>& vv)
{ {
return vv.get<0>().first == vv.get<1>().first return vv.get<0>().first == vv.get<1>().first
&& vv.get<0>().second.size() == vv.get<1>().second.size(); && vv.get<0>().second.size() == vv.get<1>().second.size();
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool VectorValues::hasSameStructure(const VectorValues other) const bool VectorValues::hasSameStructure(const VectorValues other) const
{ {
return accumulate(combine(*this, other) return accumulate(combine(*this, other)
| transformed(internal::structureCompareOp), true, logical_and<bool>()); | transformed(internal::structureCompareOp), true, logical_and<bool>());
} }
/* ************************************************************************* */ /* ************************************************************************* */
double VectorValues::dot(const VectorValues& v) const double VectorValues::dot(const VectorValues& v) const
{ {
if(this->size() != v.size()) if(this->size() != v.size())
throw invalid_argument("VectorValues::dot called with a VectorValues of different structure"); throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
double result = 0.0; double result = 0.0;
typedef boost::tuple<value_type, value_type> ValuePair; typedef boost::tuple<value_type, value_type> ValuePair;
using boost::adaptors::map_values; using boost::adaptors::map_values;
BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) {
assert_throw(values.get<0>().first == values.get<1>().first, assert_throw(values.get<0>().first == values.get<1>().first,
invalid_argument("VectorValues::dot called with a VectorValues of different structure")); invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
invalid_argument("VectorValues::dot called with a VectorValues of different structure")); invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
result += values.get<0>().second.dot(values.get<1>().second); result += values.get<0>().second.dot(values.get<1>().second);
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double VectorValues::norm() const { double VectorValues::norm() const {
return std::sqrt(this->squaredNorm()); return std::sqrt(this->squaredNorm());
} }
/* ************************************************************************* */ /* ************************************************************************* */
double VectorValues::squaredNorm() const { double VectorValues::squaredNorm() const {
double sumSquares = 0.0; double sumSquares = 0.0;
using boost::adaptors::map_values; using boost::adaptors::map_values;
BOOST_FOREACH(const Vector& v, *this | map_values) BOOST_FOREACH(const Vector& v, *this | map_values)
sumSquares += v.squaredNorm(); sumSquares += v.squaredNorm();
return sumSquares; return sumSquares;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::operator+(const VectorValues& c) const VectorValues VectorValues::operator+(const VectorValues& c) const
{ {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes"); throw invalid_argument("VectorValues::operator+ called with different vector sizes");
assert_throw(hasSameStructure(c), assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator+ called with different vector sizes")); invalid_argument("VectorValues::operator+ called with different vector sizes"));
VectorValues result; VectorValues result;
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) 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)); result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second));
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::add(const VectorValues& c) const VectorValues VectorValues::add(const VectorValues& c) const
{ {
return *this + c; return *this + c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues& VectorValues::operator+=(const VectorValues& c) VectorValues& VectorValues::operator+=(const VectorValues& c)
{ {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes"); throw invalid_argument("VectorValues::operator+= called with different vector sizes");
assert_throw(hasSameStructure(c), assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator+= called with different vector sizes")); invalid_argument("VectorValues::operator+= called with different vector sizes"));
iterator j1 = begin(); iterator j1 = begin();
const_iterator j2 = c.begin(); const_iterator j2 = c.begin();
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
for(; j1 != end(); ++j1, ++j2) for(; j1 != end(); ++j1, ++j2)
j1->second += j2->second; j1->second += j2->second;
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues& VectorValues::addInPlace(const VectorValues& c) VectorValues& VectorValues::addInPlace(const VectorValues& c)
{ {
return *this += c; return *this += c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues& VectorValues::addInPlace_(const VectorValues& c) VectorValues& VectorValues::addInPlace_(const VectorValues& c)
{ {
for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) { for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
pair<VectorValues::iterator, bool> xi = tryInsert(j2->first, Vector()); pair<VectorValues::iterator, bool> xi = tryInsert(j2->first, Vector());
if(xi.second) if(xi.second)
xi.first->second = j2->second; xi.first->second = j2->second;
else else
xi.first->second += j2->second; xi.first->second += j2->second;
} }
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::operator-(const VectorValues& c) const VectorValues VectorValues::operator-(const VectorValues& c) const
{ {
if(this->size() != c.size()) if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes"); throw invalid_argument("VectorValues::operator- called with different vector sizes");
assert_throw(hasSameStructure(c), assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator- called with different vector sizes")); invalid_argument("VectorValues::operator- called with different vector sizes"));
VectorValues result; VectorValues result;
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) 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)); result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second));
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::subtract(const VectorValues& c) const VectorValues VectorValues::subtract(const VectorValues& c) const
{ {
return *this - c; return *this - c;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues operator*(const double a, const VectorValues &v) VectorValues operator*(const double a, const VectorValues &v)
{ {
VectorValues result; VectorValues result;
BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v)
result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second));
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::scale(const double a) const VectorValues VectorValues::scale(const double a) const
{ {
return a * *this; return a * *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues& VectorValues::operator*=(double alpha) VectorValues& VectorValues::operator*=(double alpha)
{ {
BOOST_FOREACH(Vector& v, *this | map_values) BOOST_FOREACH(Vector& v, *this | map_values)
v *= alpha; v *= alpha;
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues& VectorValues::scaleInPlace(double alpha) VectorValues& VectorValues::scaleInPlace(double alpha)
{ {
return *this *= alpha; return *this *= alpha;
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // \namespace gtsam

768
gtsam/linear/VectorValues.h
View File

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

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

@ -24,7 +24,7 @@ const double tol = 1e-5;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testSampler, basic) { 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); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
char seed = 'A'; char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); 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) // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15); Matrix F(15,15);
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, 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, 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, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
@ -274,6 +274,7 @@ namespace gtsam {
// Update preintegrated measurements // Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
// deltaPij += deltaVij * deltaT;
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr; deltaRij = deltaRij * Rincr;
@ -341,8 +342,11 @@ namespace gtsam {
public: public:
/** Shorthand for a smart pointer to a factor */ /** 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 */ /** Default constructor - only use for serialization */
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} 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; // 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; //// 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: public:
/** Shorthand for a smart pointer to a factor */ /** 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; typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */ /** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} 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 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) 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;

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

@ -167,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Bias 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point // Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // 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)); // 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)); // 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) 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{ //{
// // Linearization point // // 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)); // 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)); // 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)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
// //
// // Pre-integrator // // 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) 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; 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. // and the key/index needs to be reset to 0, the first key in the next iteration.
assert(marginal->nrFrontals() == 1); assert(marginal->nrFrontals() == 1);
assert(marginal->nrParents() == 0); assert(marginal->nrParents() == 0);
newPrior = boost::make_shared<JacobianFactor>( newPrior = boost::make_shared<JacobianFactor>(
marginal->keys().front(), marginal->keys().front(),
marginal->getA(marginal->begin()), marginal->getA(marginal->begin()),
marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
marginal->get_model()); marginal->get_model());
return x; 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. // created above.
if(!clique->solnPointers_.empty()) if(!clique->solnPointers_.empty())
{ {
GaussianConditional& c = *clique->conditional(); GaussianConditional& c = *clique->conditional();
// Solve matrix // Solve matrix
Vector xS; Vector xS;
{ {
// Count dimensions of vector // Count dimensions of vector
DenseIndex dim = 0; DenseIndex dim = 0;
FastVector<VectorValues::const_iterator> parentPointers; FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents()); parentPointers.reserve(clique->conditional()->nrParents());
BOOST_FOREACH(Key parent, clique->conditional()->parents()) { BOOST_FOREACH(Key parent, clique->conditional()->parents()) {
parentPointers.push_back(clique->solnPointers_.at(parent)); parentPointers.push_back(clique->solnPointers_.at(parent));
dim += parentPointers.back()->second.size(); dim += parentPointers.back()->second.size();
} }
// Fill parent vector // Fill parent vector
xS.resize(dim); xS.resize(dim);
DenseIndex vectorPos = 0; DenseIndex vectorPos = 0;
BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) {
const Vector& parentVector = parentPointer->second; const Vector& parentVector = parentPointer->second;
xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
vectorPos += parentVector.size(); vectorPos += parentVector.size();
} }
} }
xS = c.getb() - c.get_S() * xS; xS = c.getb() - c.get_S() * xS;
Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = c.get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); if(soln.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
// Insert solution into a VectorValues // Insert solution into a VectorValues
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;
for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal)); clique->solnPointers_.at(*frontal)->second = soln.segment(vectorPosition, c.getDim(frontal));
vectorPosition += c.getDim(frontal); vectorPosition += c.getDim(frontal);
} }
} }
else else

8
gtsam/nonlinear/ISAM2.h
View File

@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params {
* entries would be added with: * entries would be added with:
* \code * \code
FastMap<char,Vector> thresholds; 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['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['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds; params.relinearizeThreshold = thresholds;
\endcode \endcode
*/ */
@ -552,8 +552,8 @@ public:
* If provided, 'deletedFactorsIndices' will be augmented with the factor graph * If provided, 'deletedFactorsIndices' will be augmented with the factor graph
* indices of any factor that was removed during the 'marginalizeLeaves' call * indices of any factor that was removed during the 'marginalizeLeaves' call
*/ */
void marginalizeLeaves(const FastList<Key>& leafKeys, void marginalizeLeaves(const FastList<Key>& leafKeys,
boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none, boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none); boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none);
/** Access the current linearization point */ /** 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); return graph_.linearize(state_.values);
} }
/* ************************************************************************* */
void LevenbergMarquardtOptimizer::increaseLambda(){
state_.lambda *= params_.lambdaFactor;
}
/* ************************************************************************* */
void LevenbergMarquardtOptimizer::decreaseLambda(){
state_.lambda /= params_.lambdaFactor;
}
/* ************************************************************************* */ /* ************************************************************************* */
void LevenbergMarquardtOptimizer::iterate() { void LevenbergMarquardtOptimizer::iterate() {
@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() {
if (error <= state_.error) { if (error <= state_.error) {
state_.values.swap(newValues); state_.values.swap(newValues);
state_.error = error; state_.error = error;
state_.lambda /= params_.lambdaFactor; decreaseLambda();
break; break;
} else { } else {
// Either we're not cautious, or the same lambda was worse than the current error. // 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; cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
break; break;
} else { } else {
state_.lambda *= params_.lambdaFactor; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl;
increaseLambda();
} }
} }
} catch (IndeterminantLinearSystemException& e) { } catch (IndeterminantLinearSystemException& e) {
@ -172,7 +185,7 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
break; break;
} else { } else {
state_.lambda *= params_.lambdaFactor; increaseLambda();
} }
} }
// Frank asks: why would we do that? // Frank asks: why would we do that?

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -174,6 +174,12 @@ public:
return state_.lambda; 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 /// Access the current number of inner iterations
int getInnerIterations() const { int getInnerIterations() const {
return state_.totalNumberInnerIterations; return state_.totalNumberInnerIterations;

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

@ -19,7 +19,7 @@ using namespace std;
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; 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; const double tol = 1e-5;
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; 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 ) TEST( Values, equals1 )
{ {
Values expected; 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); expected.insert(key1,v);
Values actual; Values actual;
actual.insert(key1,v); actual.insert(key1,v);
@ -76,8 +77,9 @@ TEST( Values, equals1 )
TEST( Values, equals2 ) TEST( Values, equals2 )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 5.0, 6.0, 8.0); LieVector v2((Vector(3) << 5.0, 6.0, 8.0));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2)); CHECK(!cfg1.equals(cfg2));
@ -88,8 +90,9 @@ TEST( Values, equals2 )
TEST( Values, equals_nan ) TEST( Values, equals_nan )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, inf, inf, inf); LieVector v2((Vector(3) << inf, inf, inf));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2)); CHECK(!cfg1.equals(cfg2));
@ -100,10 +103,11 @@ TEST( Values, equals_nan )
TEST( Values, insert_good ) TEST( Values, insert_good )
{ {
Values cfg1, cfg2, expected; Values cfg1, cfg2, expected;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 8.0, 9.0, 1.0); LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3(3, 2.0, 4.0, 3.0); LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4(3, 8.0, 3.0, 7.0); LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
cfg2.insert(key3, v4); cfg2.insert(key3, v4);
@ -121,10 +125,11 @@ TEST( Values, insert_good )
TEST( Values, insert_bad ) TEST( Values, insert_bad )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 8.0, 9.0, 1.0); LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3(3, 2.0, 4.0, 3.0); LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4(3, 8.0, 3.0, 7.0); LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
cfg2.insert(key2, v3); cfg2.insert(key2, v3);
@ -137,8 +142,8 @@ TEST( Values, insert_bad )
TEST( Values, update_element ) TEST( Values, update_element )
{ {
Values cfg; Values cfg;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 8.0, 9.0, 1.0); LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
cfg.insert(key1, v1); cfg.insert(key1, v1);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
@ -181,8 +186,8 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero) //TEST(Values, dim_zero)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector(2, 2.0, 3.0)); // config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim()); // LONGS_EQUAL(5, config0.dim());
// //
// VectorValues expected; // VectorValues expected;
@ -195,16 +200,16 @@ TEST(Values, basic_functions)
TEST(Values, expmap_a) TEST(Values, expmap_a)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2)) (key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -213,15 +218,15 @@ TEST(Values, expmap_a)
TEST(Values, expmap_b) TEST(Values, expmap_b)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -230,16 +235,16 @@ TEST(Values, expmap_b)
//TEST(Values, expmap_c) //TEST(Values, expmap_c)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); // config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
// //
// Vector increment = LieVector(6, // Vector increment = LieVector(6,
// 1.0, 1.1, 1.2, // 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5); // 1.3, 1.4, 1.5);
// //
// Values expected; // Values expected;
// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); // expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); // expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
// //
// CHECK(assert_equal(expected, config0.retract(increment))); // CHECK(assert_equal(expected, config0.retract(increment)));
//} //}
@ -248,8 +253,8 @@ TEST(Values, expmap_b)
TEST(Values, expmap_d) TEST(Values, expmap_d)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
//config0.print("config0"); //config0.print("config0");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
@ -266,8 +271,8 @@ TEST(Values, expmap_d)
TEST(Values, localCoordinates) TEST(Values, localCoordinates)
{ {
Values valuesA; Values valuesA;
valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues expDelta = pair_list_of<Key, Vector> VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3)) (key1, (Vector(3) << 0.1, 0.2, 0.3))
@ -314,17 +319,17 @@ TEST(Values, exists_)
TEST(Values, update) TEST(Values, update)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(1, 1.)); config0.insert(key1, LieVector((Vector(1) << 1.)));
config0.insert(key2, LieVector(1, 2.)); config0.insert(key2, LieVector((Vector(1) << 2.)));
Values superset; Values superset;
superset.insert(key1, LieVector(1, -1.)); superset.insert(key1, LieVector((Vector(1) << -1.)));
superset.insert(key2, LieVector(1, -2.)); superset.insert(key2, LieVector((Vector(1) << -2.)));
config0.update(superset); config0.update(superset);
Values expected; Values expected;
expected.insert(key1, LieVector(1, -1.)); expected.insert(key1, LieVector((Vector(1) << -1.)));
expected.insert(key2, LieVector(1, -2.)); expected.insert(key2, LieVector((Vector(1) << -2.)));
CHECK(assert_equal(expected,config0)); 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)); Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_); 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 (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22); 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 { } else {
hx = pose.range(point, H1, H2); hx = pose.range(point, H1, H2);
} }
return Vector_(1, hx - measured_); return (Vector(1) << hx - measured_);
} }
/** return the 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() 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); 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() 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); 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){ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
// CHECKS // Store poses or cameras in SfM_data
Values valuesPoses = values.filter<Pose3>(); Values valuesPoses = values.filter<Pose3>();
if( valuesPoses.size() != data.number_cameras()){ if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
<<") and values (#cameras " << valuesPoses.size() << ")!!" << endl; Key poseKey = symbol('x',i);
return false; 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>(); Values valuesPoints = values.filter<Point3>();
if( valuesPoints.size() != data.number_tracks()){ if( valuesPoints.size() != data.number_tracks()){
cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl; <<") 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;
} }
for (size_t j = 0; j < data.number_tracks(); j++){ // for each point 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); return writeBAL(filename, data);
} }
} // \namespace gtsam } // \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) * while camera poses and values are read from Values)
* @param filename The name of the BAL file to write * @param filename The name of the BAL file to write
* @param data SfM structure where the data is stored * @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 * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); 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); EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
// Check evaluation // Check evaluation
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = camera2.project(P1); const Point2 pi = SimpleCamera::project_to_camera(P2);
Point2 reprojectionError(pi - pB(i)); Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
@ -269,6 +269,21 @@ TEST (EssentialMatrixFactor3, minimization) {
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, LieScalar(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); 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 } // namespace example1

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

@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); 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 ; 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()); 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 * Create vector initialized to a constant value
* @param value 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 ) { TEST( testFixedVector, conversions ) {
double data1[] = {1.0, 2.0, 3.0}; 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); TestVector3 fv1(v1), fv2(data1);
Vector actFv2(fv2); Vector actFv2(fv2);
@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testFixedVector, variable_constructor ) { 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(1.0, act(0), tol);
EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); EXPECT_DOUBLES_EQUAL(2.0, act(1), tol);
EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); EXPECT_DOUBLES_EQUAL(3.0, act(2), tol);
@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testFixedVector, equals ) { 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); TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)),
TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.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, vec1, tol));
EXPECT(assert_equal(vec1, vec2, tol)); EXPECT(assert_equal(vec1, vec2, tol));
@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testFixedVector, static_constructors ) { TEST( testFixedVector, static_constructors ) {
TestVector3 actZero = TestVector3::zero(); 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)); EXPECT(assert_equal(expZero, actZero, tol));
TestVector3 actOnes = TestVector3::ones(); 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)); EXPECT(assert_equal(expOnes, actOnes, tol));
TestVector3 actRepeat = TestVector3::repeat(2.3); 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)); EXPECT(assert_equal(expRepeat, actRepeat, tol));
TestVector3 actBasis = TestVector3::basis(1); 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)); EXPECT(assert_equal(expBasis, actBasis, tol));
TestVector3 actDelta = TestVector3::delta(1, 2.3); 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)); 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 { 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)); elevation_.localCoordinates(x.elevation_)(0));
} }

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -166,7 +166,7 @@ public:
gtsam::Point3 ray = pw - pt; gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude double theta = atan2(ray.y(), ray.x()); // longitude
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); 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)); 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 // extend line by random dist and angle to get BC
double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len); double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
double tABC = randomAngle().theta(); 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 // extend from B to find C
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); 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 // calculate angle to change by
Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta()); 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 // create a segment to use for intersection checking
// find the closest intersection // 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); Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); 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); LieScalar inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv)); EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); 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)); LieScalar inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); 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)); LieScalar inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); 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.)); LieScalar inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i
TEST( InvDepthFactor, Dproject_pose) 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); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark) 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); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth) 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); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(InvDepthFactor, backproject) 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); LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);
@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2) TEST(InvDepthFactor, backproject2)
{ {
// backwards facing camera // 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); LieScalar inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); 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); 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(x1, x1.retract(zero(4)), tol));
EXPECT(assert_equal(x2, x2.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(x2, x1.retract(delta12), tol));
EXPECT(assert_equal(x1, x2.retract(delta21), 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 ); 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; omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g // Calculating g
@ -551,7 +551,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature 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 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) ) ); 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 // Calculate rho
@ -560,7 +560,7 @@ public:
double rho_E = -Vn/(Rm + height); double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height); double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(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){ 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; " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx(); return gtsam::ones(2) * 2.0 * K_->fx();
} }
return gtsam::Vector_(1, 0.0); return (gtsam::Vector(1) << 0.0);
} }
/** return the measurement */ /** return the measurement */

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -96,7 +96,7 @@ public:
<< std::endl; << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx(); 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 /// Evaluate error h(x)-z and optionally derivatives

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -99,7 +99,7 @@ public:
<< std::endl; << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx(); 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 /// 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 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(); 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 */ /** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : 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); assert(model->dim() == 1);
this->fillH(); 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 = zeros(1, 3);
(*H2)(0, 2) = -1.0; (*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; Mechanization_bRn2 mech;
KalmanFilter::State state; KalmanFilter::State state;
// boost::tie(mech,state) = ahrs.initialize(g_e); // 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; // double dt = 2;
// Rot3 expected; // Rot3 expected;
// Mechanization_bRn2 mech2 = mech.integrate(u,dt); // 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); 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)); 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; imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); 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; Pose3 actualPose2;
LieVector actualVel2; LieVector actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, 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 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40); LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2(3, 0.51, -0.48, 0.43); LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); 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 Pose1(Rot3(), Point3(2.0,1.0,3.0));
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), 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 Vel1((Vector(3) << 0.0, 0.0, 0.0));
LieVector Vel2(3,0.0,0.0,0.0); LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); 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, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); 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, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2); 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; imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; 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); 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)); 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; imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); 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; Pose3 actualPose2;
LieVector actualVel2; LieVector actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, 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 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40); LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2(3, 0.51, -0.48, 0.43); LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); 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 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)); 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 Vel1((Vector(3) << 0.0,0.0,0.0));
LieVector Vel2(3,0.0,0.0,0.0); LieVector Vel2((Vector(3) << 0.0,0.0,0.0));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); 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, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); 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, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2); 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; imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; 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); Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); 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 // initialize inverse depth with "incorrect" depth of 1/4
// in reality this is 1/5, but initial depth is guessed // in reality this is 1/5, but initial depth is guessed
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);

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