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

Merge remote-tracking branch 'origin/develop' into feature/SmartFactors3 

Conflicts:
	gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
release/4.3a0
dellaert 2015-03-04 16:02:25 -08:00
parent 2f2cc078dc 432a2e24cd
commit f28b083c14
35 changed files with 1061 additions and 404 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -532,6 +532,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSimilarity3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

1
.settings/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/org.eclipse.cdt.codan.core.prefs

6
gtsam.h
View File

@ -156,7 +156,7 @@ virtual class Value {
size_t dim() const;
};
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/LieScalar_Deprecated.h>
class LieScalar {
// Standard constructors
LieScalar();
@ -185,7 +185,7 @@ class LieScalar {
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieVector_Deprecated.h>
class LieVector {
// Standard constructors
LieVector();
@ -217,7 +217,7 @@ class LieVector {
void serialize() const;
};
#include <gtsam/base/LieMatrix.h>
#include <gtsam/base/LieMatrix_Deprecated.h>
class LieMatrix {
// Standard constructors
LieMatrix();

9
gtsam/base/FastSet.h
View File

@ -19,15 +19,14 @@
#pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#include <set>
#include <iostream>
#include <string>
#include <cmath>
#include <boost/mpl/has_xxx.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
#include <set>
#include <iostream>
#include <string>
#include <cmath>
BOOST_MPL_HAS_XXX_TRAIT_DEF(print)

2
gtsam/base/GenericValue.h
View File

@ -26,7 +26,7 @@
#include <boost/pool/pool_alloc.hpp>
#include <cmath>
#include <iostream>
#include <iosfwd>
#include <typeinfo> // operator typeid
namespace gtsam {

2
gtsam/base/LieMatrix.cpp
View File

@ -15,7 +15,7 @@
* @author Richard Roberts and Alex Cunningham
*/
#include <gtsam/base/LieMatrix.h>
#include <gtsam/base/LieMatrix_Deprecated.h>
namespace gtsam {

137
gtsam/base/LieMatrix.h
View File

@ -11,147 +11,16 @@
/**
* @file LieMatrix.h
* @brief A wrapper around Matrix providing Lie compatibility
* @author Richard Roberts and Alex Cunningham
* @brief External deprecation warning, see LieMatrix_Deprecated.h for details
* @author Paul Drews
*/
#pragma once
#include <cstdarg>
#ifdef _MSC_VER
#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
#else
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
* @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieMatrix : public Matrix {
/// @name Constructors
/// @{
enum { dimension = Eigen::Dynamic };
/** default constructor - only for serialize */
LieMatrix() {}
/** initialize from a normal matrix */
LieMatrix(const Matrix& v) : Matrix(v) {}
template <class M>
LieMatrix(const M& v) : Matrix(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int M, int N>
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
#endif
/** constructor with size and initial data, row order ! */
LieMatrix(size_t m, size_t n, const double* const data) :
Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
/// @}
/// @name Testable interface
/// @{
/** print @param s optional string naming the object */
GTSAM_EXPORT void print(const std::string& name="") const;
/** equality up to tolerance */
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
}
/// @}
/// @name Standard Interface
/// @{
/** get the underlying matrix */
inline Matrix matrix() const {
return static_cast<Matrix>(*this);
}
/// @}
/// @name Group
/// @{
LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
LieMatrix between(const LieMatrix& q) { return q-(*this);}
LieMatrix inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieMatrix& p) {return p.vector();}
static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return size(); }
/** Convert to vector, is done row-wise - TODO why? */
inline Vector vector() const {
Vector result(size());
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
return result;
}
/** identity - NOTE: no known size at compile time - so zero length */
inline static LieMatrix identity() {
throw std::runtime_error("LieMatrix::identity(): Don't use this function");
return LieMatrix();
}
/// @}
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Matrix",
boost::serialization::base_object<Matrix>(*this));
}
};
template<>
struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> {
// Override Retract, as the default version does not know how to initialize
static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
}
};
} // \namespace gtsam
#include "gtsam/base/LieMatrix_Deprecated.h"

151
gtsam/base/LieMatrix_Deprecated.h Normal file
View File

@ -0,0 +1,151 @@
/* ----------------------------------------------------------------------------
* 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 LieMatrix.h
* @brief A wrapper around Matrix providing Lie compatibility
* @author Richard Roberts and Alex Cunningham
*/
#pragma once
#include <cstdarg>
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
* @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieMatrix : public Matrix {
/// @name Constructors
/// @{
enum { dimension = Eigen::Dynamic };
/** default constructor - only for serialize */
LieMatrix() {}
/** initialize from a normal matrix */
LieMatrix(const Matrix& v) : Matrix(v) {}
template <class M>
LieMatrix(const M& v) : Matrix(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int M, int N>
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
#endif
/** constructor with size and initial data, row order ! */
LieMatrix(size_t m, size_t n, const double* const data) :
Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
/// @}
/// @name Testable interface
/// @{
/** print @param s optional string naming the object */
GTSAM_EXPORT void print(const std::string& name="") const;
/** equality up to tolerance */
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
}
/// @}
/// @name Standard Interface
/// @{
/** get the underlying matrix */
inline Matrix matrix() const {
return static_cast<Matrix>(*this);
}
/// @}
/// @name Group
/// @{
LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
LieMatrix between(const LieMatrix& q) { return q-(*this);}
LieMatrix inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieMatrix& p) {return p.vector();}
static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return size(); }
/** Convert to vector, is done row-wise - TODO why? */
inline Vector vector() const {
Vector result(size());
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
return result;
}
/** identity - NOTE: no known size at compile time - so zero length */
inline static LieMatrix identity() {
throw std::runtime_error("LieMatrix::identity(): Don't use this function");
return LieMatrix();
}
/// @}
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Matrix",
boost::serialization::base_object<Matrix>(*this));
}
};
template<>
struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> {
// Override Retract, as the default version does not know how to initialize
static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
}
};
} // \namespace gtsam

2
gtsam/base/LieScalar.cpp
View File

@ -8,7 +8,7 @@
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/LieScalar_Deprecated.h>
namespace gtsam {
void LieScalar::print(const std::string& name) const {

69
gtsam/base/LieScalar.h
View File

@ -11,7 +11,7 @@
/**
* @file LieScalar.h
* @brief A wrapper around scalar providing Lie compatibility
* @brief External deprecation warning, see LieScalar_Deprecated.h for details
* @author Kai Ni
*/
@ -23,69 +23,4 @@
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
#include <gtsam/dllexport.h>
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
/**
* @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct GTSAM_EXPORT LieScalar {
enum { dimension = 1 };
/** default constructor */
LieScalar() : d_(0.0) {}
/** wrap a double */
/*explicit*/ LieScalar(double d) : d_(d) {}
/** access the underlying value */
double value() const { return d_; }
/** Automatic conversion to underlying value */
operator double() const { return d_; }
/** convert vector */
Vector1 vector() const { Vector1 v; v<<d_; return v; }
/// @name Testable
/// @{
void print(const std::string& name="") const;
bool equals(const LieScalar& expected, double tol=1e-5) const {
return fabs(expected.d_ - d_) <= tol;
}
/// @}
/// @name Group
/// @{
static LieScalar identity() { return LieScalar(0);}
LieScalar compose(const LieScalar& q) { return (*this)+q;}
LieScalar between(const LieScalar& q) { return q-(*this);}
LieScalar inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
size_t dim() const { return 1; }
Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}
/// @}
/// @name Lie Group
/// @{
static Vector1 Logmap(const LieScalar& p) { return p.vector();}
static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}
/// @}
private:
double d_;
};
template<>
struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};
} // \namespace gtsam
#include <gtsam/base/LieScalar_Deprecated.h>

85
gtsam/base/LieScalar_Deprecated.h Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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 LieScalar.h
* @brief A wrapper around scalar providing Lie compatibility
* @author Kai Ni
*/
#pragma once
#include <gtsam/dllexport.h>
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
/**
* @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct GTSAM_EXPORT LieScalar {
enum { dimension = 1 };
/** default constructor */
LieScalar() : d_(0.0) {}
/** wrap a double */
/*explicit*/ LieScalar(double d) : d_(d) {}
/** access the underlying value */
double value() const { return d_; }
/** Automatic conversion to underlying value */
operator double() const { return d_; }
/** convert vector */
Vector1 vector() const { Vector1 v; v<<d_; return v; }
/// @name Testable
/// @{
void print(const std::string& name="") const;
bool equals(const LieScalar& expected, double tol=1e-5) const {
return fabs(expected.d_ - d_) <= tol;
}
/// @}
/// @name Group
/// @{
static LieScalar identity() { return LieScalar(0);}
LieScalar compose(const LieScalar& q) { return (*this)+q;}
LieScalar between(const LieScalar& q) { return q-(*this);}
LieScalar inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
size_t dim() const { return 1; }
Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}
/// @}
/// @name Lie Group
/// @{
static Vector1 Logmap(const LieScalar& p) { return p.vector();}
static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}
/// @}
private:
double d_;
};
template<>
struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};
} // \namespace gtsam

2
gtsam/base/LieVector.cpp
View File

@ -15,8 +15,8 @@
* @author Alex Cunningham
*/
#include <gtsam/base/LieVector_Deprecated.h>
#include <cstdarg>
#include <gtsam/base/LieVector.h>
using namespace std;

102
gtsam/base/LieVector.h
View File

@ -11,8 +11,8 @@
/**
* @file LieVector.h
* @brief A wrapper around vector providing Lie compatibility
* @author Alex Cunningham
* @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details.
* @author Paul Drews
*/
#pragma once
@ -23,100 +23,4 @@
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
/**
* @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieVector : public Vector {
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */
LieVector() {}
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
template <class V>
LieVector(const V& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int N>
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
#endif
/** wrap a double */
LieVector(double d) : Vector((Vector(1) << d).finished()) {}
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);
/// @name Testable
/// @{
GTSAM_EXPORT void print(const std::string& name="") const;
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
/// @}
/// @name Group
/// @{
LieVector compose(const LieVector& q) { return (*this)+q;}
LieVector between(const LieVector& q) { return q-(*this);}
LieVector inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieVector& q) { return between(q).vector();}
LieVector retract(const Vector& v) {return compose(LieVector(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieVector& p) {return p.vector();}
static LieVector Expmap(const Vector& v) { return LieVector(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** get the underlying vector */
Vector vector() const {
return static_cast<Vector>(*this);
}
/** Returns dimensionality of the tangent space */
size_t dim() const { return this->size(); }
/** identity - NOTE: no known size at compile time - so zero length */
static LieVector identity() {
throw std::runtime_error("LieVector::identity(): Don't use this function");
return LieVector();
}
/// @}
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this));
}
};
template<>
struct traits<LieVector> : public internal::VectorSpace<LieVector> {};
} // \namespace gtsam
#include <gtsam/base/LieVector_Deprecated.h>

116
gtsam/base/LieVector_Deprecated.h Normal file
View File

@ -0,0 +1,116 @@
/* ----------------------------------------------------------------------------
* 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 LieVector.h
* @brief A wrapper around vector providing Lie compatibility
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
/**
* @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieVector : public Vector {
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */
LieVector() {}
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
template <class V>
LieVector(const V& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int N>
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
#endif
/** wrap a double */
LieVector(double d) : Vector((Vector(1) << d).finished()) {}
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);
/// @name Testable
/// @{
GTSAM_EXPORT void print(const std::string& name="") const;
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
/// @}
/// @name Group
/// @{
LieVector compose(const LieVector& q) { return (*this)+q;}
LieVector between(const LieVector& q) { return q-(*this);}
LieVector inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieVector& q) { return between(q).vector();}
LieVector retract(const Vector& v) {return compose(LieVector(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieVector& p) {return p.vector();}
static LieVector Expmap(const Vector& v) { return LieVector(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** get the underlying vector */
Vector vector() const {
return static_cast<Vector>(*this);
}
/** Returns dimensionality of the tangent space */
size_t dim() const { return this->size(); }
/** identity - NOTE: no known size at compile time - so zero length */
static LieVector identity() {
throw std::runtime_error("LieVector::identity(): Don't use this function");
return LieVector();
}
/// @}
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this));
}
};
template<>
struct traits<LieVector> : public internal::VectorSpace<LieVector> {};
} // \namespace gtsam

22
gtsam/base/Matrix.cpp
View File

@ -20,6 +20,8 @@
#include <gtsam/base/timing.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h>
#include <gtsam/3rdparty/Eigen/Eigen/SVD>
#include <gtsam/3rdparty/Eigen/Eigen/LU>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
}
/* ************************************************************************* */
//3 argument call
void print(const Matrix& A, const string &s, ostream& stream) {
size_t m = A.rows(), n = A.cols();
@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) {
stream << "];" << endl;
}
/* ************************************************************************* */
//1 or 2 argument call
void print(const Matrix& A, const string &s){
print(A, s, cout);
}
/* ************************************************************************* */
void save(const Matrix& A, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app);
@ -697,6 +706,19 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
return ss.str();
}
void inplace_QR(Matrix& A){
size_t rows = A.rows();
size_t cols = A.cols();
size_t size = std::min(rows,cols);
typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
HCoeffsType hCoeffs(size);
RowVectorType temp(cols);
Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
zeroBelowDiagonal(A);
}
} // namespace gtsam

32
gtsam/base/Matrix.h
View File

@ -21,11 +21,14 @@
// \callgraph
#pragma once
#include <gtsam/base/Vector.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtsam/3rdparty/Eigen/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Eigen/Cholesky>
#include <gtsam/3rdparty/Eigen/Eigen/LU>
#include <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
/**
* Matrix is a typedef in the gtsam namespace
@ -198,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
}
/**
* print a matrix
* print without optional string, must specify cout yourself
*/
GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout);
GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
/**
* print with optional string to cout
*/
GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
/**
* save a matrix to file, which can be loaded by matlab
@ -367,21 +375,7 @@ GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
* @param A is the input matrix, and is the output
* @param clear_below_diagonal enables zeroing out below diagonal
*/
template <class MATRIX>
void inplace_QR(MATRIX& A) {
size_t rows = A.rows();
size_t cols = A.cols();
size_t size = std::min(rows,cols);
typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
HCoeffsType hCoeffs(size);
RowVectorType temp(cols);
Eigen::internal::householder_qr_inplace_blocked<MATRIX, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
zeroBelowDiagonal(A);
}
void inplace_QR(Matrix& A);
/**
* Imperative algorithm for in-place full elimination with

5
gtsam/base/Value.h
View File

@ -18,9 +18,10 @@
#pragma once
#include <memory>
#include <boost/serialization/assume_abstract.hpp>
#include <gtsam/base/Vector.h>
#include <boost/serialization/assume_abstract.hpp>
#include <memory>
namespace gtsam {

19
gtsam/base/Vector.cpp
View File

@ -16,20 +16,18 @@
* @author Frank Dellaert
*/
#include <gtsam/base/Vector.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <stdexcept>
#include <cstdarg>
#include <limits>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cmath>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <cstdio>
#include <gtsam/base/Vector.h>
#include <vector>
using namespace std;
@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) {
}
/* ************************************************************************* */
//3 argument call
void print(const Vector& v, const string& s, ostream& stream) {
size_t n = v.size();
@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) {
stream << "];" << endl;
}
/* ************************************************************************* */
//1 or 2 argument call
void print(const Vector& v, const string& s) {
print(v, s, cout);
}
/* ************************************************************************* */
void save(const Vector& v, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app);

18
gtsam/base/Vector.h
View File

@ -18,13 +18,12 @@
// \callgraph
#pragma once
#include <list>
#include <vector>
#include <iostream>
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam/3rdparty/Eigen/Eigen/Core>
#include <iosfwd>
#include <list>
namespace gtsam {
@ -97,9 +96,14 @@ GTSAM_EXPORT bool zero(const Vector& v);
inline size_t dim(const Vector& v) { return v.size(); }
/**
* print with optional string
* print without optional string, must specify cout yourself
*/
GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout);
GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
/**
* print with optional string to cout
*/
GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
/**
* save a vector to file, which can be loaded by matlab

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

@ -15,10 +15,10 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieMatrix.h>
using namespace gtsam;

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

@ -15,10 +15,10 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieScalar.h>
using namespace gtsam;

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

@ -15,10 +15,10 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/LieVector.h>
using namespace gtsam;

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

@ -15,8 +15,8 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/TestableAssertions.h>
using namespace gtsam;

3
gtsam/base/types.cpp
View File

@ -17,10 +17,9 @@
* @addtogroup base
*/
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h>
#include <gtsam/inference/Symbol.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {

8
gtsam/base/types.h
View File

@ -21,18 +21,18 @@
#include <gtsam/dllexport.h>
#include <gtsam/config.h>
#include <cstddef>
#include <string>
#include <iostream>
#include <boost/function/function1.hpp>
#include <boost/range/concepts.hpp>
#include <boost/optional.hpp>
#include <cstddef>
#include <string>
#include <ostream>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#include <iostream>
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP

2
gtsam/geometry/EssentialMatrix.h
View File

@ -10,7 +10,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>
#include <iosfwd>
namespace gtsam {

6
gtsam/nonlinear/Values.h
View File

@ -26,14 +26,9 @@
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h>
#include <boost/optional.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/function.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
@ -43,7 +38,6 @@
#pragma GCC diagnostic pop
#endif
#include <boost/ptr_container/serialize_ptr_map.hpp>
#include <boost/iterator_adaptors.hpp>
#include <string>
#include <utility>

107
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -232,8 +232,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK2));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Camera>(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@ -252,8 +256,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
// VectorValues delta = GFG->optimize();
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Camera>(x3).print("Camera after optimization: ");
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-8));
if (isDebugTest)
tictoc_print_();
@ -442,8 +444,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
-0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
-0.0313952598), Point3(0.1, -0.1, 1.9)),
values.at<Camera>(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@ -459,8 +466,6 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Camera>(x3).print("Camera after optimization: ");
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-7));
if (isDebugTest)
tictoc_print_();
@ -819,8 +824,14 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
-0.130455917),
Point3(0.0897734171, -0.110201006, 0.901022872)),
values.at<Camera>(x3).pose()));
boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
@ -900,8 +911,14 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
values.insert(x2, Camera(pose2 * noise_pose, sharedK2));
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose3 * noise_pose * noise_pose, sharedK2));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814,
-0.572308662, -0.324093872, 0.639358349, -0.521617766,
-0.564921063),
Point3(0.145118171, -0.252907438, 0.819956033)),
values.at<Camera>(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@ -917,12 +934,17 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Camera>(x3).print("Camera after optimization: ");
cout
<< "TEST COMMENTED: rotation only version of smart factors has been deprecated "
<< endl;
// EXPECT(assert_equal(pose_above,result.at<Camera>(x3)));
EXPECT(
assert_equal(
Pose3(
Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814,
-0.572308662, -0.324093872, 0.639358349, -0.521617766,
-0.564921063),
Point3(0.145118171, -0.252907438, 0.819956033)),
result.at<Camera>(x3).pose()));
if (isDebugTest)
tictoc_print_();
}
@ -990,8 +1012,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
-0.130455917),
Point3(0.0897734171, -0.110201006, 0.901022872)),
values.at<Camera>(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@ -1007,12 +1035,17 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Camera>(x3).print("Camera after optimization: ");
cout
<< "TEST COMMENTED: rotation only version of smart factors has been deprecated "
<< endl;
// EXPECT(assert_equal(pose_above,result.at<Camera>(x3)));
EXPECT(
assert_equal(
Pose3(
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
-0.130455917),
Point3(0.0897734171, -0.110201006, 0.901022872)),
result.at<Camera>(x3).pose()));
if (isDebugTest)
tictoc_print_();
}
@ -1236,9 +1269,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Camera>(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@ -1252,9 +1288,6 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
gttoc_(SmartProjectionPoseFactor);
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Camera>(x3).print("Camera after optimization: ");
EXPECT(assert_equal(cam3, result.at<Camera>(x3), 1e-6));
if (isDebugTest)
tictoc_print_();
@ -1342,8 +1375,14 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
if (isDebugTest)
values.at<Camera>(x3).print("Camera before optimization: ");
EXPECT(
assert_equal(
Pose3(
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
-0.130455917),
Point3(0.0897734171, -0.110201006, 0.901022872)),
values.at<Camera>(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@ -1358,13 +1397,17 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
gttoc_(SmartProjectionPoseFactor);
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Camera>(x3).print("Camera after optimization: ");
cout
<< "TEST COMMENTED: rotation only version of smart factors has been deprecated "
<< endl;
// EXPECT(assert_equal(pose_above,result.at<Camera>(x3)));
EXPECT(
assert_equal(
Pose3(
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
-0.130455917),
Point3(0.0897734171, -0.110201006, 0.901022872)),
values.at<Camera>(x3).pose()));
if (isDebugTest)
tictoc_print_();
}

124
gtsam_unstable/geometry/Similarity3.cpp Normal file
View File

@ -0,0 +1,124 @@
/* ----------------------------------------------------------------------------
* 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 Similarity3.cpp
* @brief Implementation of Similarity3 transform
* @author Paul Drews
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
R_(R), t_(t), s_(s) {
}
Similarity3::Similarity3() :
R_(), t_(), s_(1){
}
Similarity3::Similarity3(double s) :
s_ (s) {
}
Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) :
R_ (R), t_ (t), s_ (s) {
}
Similarity3::operator Pose3() const {
return Pose3(R_, s_*t_);
}
Similarity3 Similarity3::identity() {
return Similarity3(); }
//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
// return Vector7();
//}
//
//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
// return Similarity3();
//}
bool Similarity3::operator==(const Similarity3& other) const {
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
}
bool Similarity3::equals(const Similarity3& sim, double tol) const {
return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
&& s_ < (sim.s_+tol) && s_ > (sim.s_-tol);
}
Similarity3::Translation Similarity3::transform_from(const Translation& p) const {
return R_ * (s_ * p) + t_;
}
Matrix7 Similarity3::AdjointMap() const{
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
Matrix3 A = s_ * skewSymmetric(t) * R;
Matrix7 adj;
adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 1, 6>::Zero(), 1;
return adj;
}
inline Similarity3::Translation Similarity3::operator*(const Translation& p) const {
return transform_from(p);
}
Similarity3 Similarity3::inverse() const {
Rotation Rt = R_.inverse();
Translation sRt = R_.inverse() * (-s_ * t_);
return Similarity3(Rt, sRt, 1.0/s_);
}
Similarity3 Similarity3::operator*(const Similarity3& T) const {
return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
}
void Similarity3::print(const std::string& s) const {
std::cout << std::endl;
std::cout << s;
rotation().print("R:\n");
translation().print("t: ");
std::cout << "s: " << scale() << std::endl;
}
Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) {
// Will retracting or localCoordinating R work if R is not a unit rotation?
// Also, how do we actually get s out? Seems like we need to store it somewhere.
Rotation r; //Create a zero rotation to do our retraction.
return Similarity3( //
r.retract(v.head<3>()), // retract rotation using v[0,1,2]
Translation(v.segment<3>(3)), // Retract the translation
1.0 + v[6]); //finally, update scale using v[6]
}
Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) {
Rotation r; //Create a zero rotation to do the retraction
Vector7 v;
v.head<3>() = r.localCoordinates(other.R_);
v.segment<3>(3) = other.t_.vector();
//v.segment<3>(3) = translation().localCoordinates(other.translation());
v[6] = other.s_ - 1.0;
return v;
}
}

150
gtsam_unstable/geometry/Similarity3.h Normal file
View File

@ -0,0 +1,150 @@
/* ----------------------------------------------------------------------------
* 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 Similarity3.h
* @brief Implementation of Similarity3 transform
* @author Paul Drews
*/
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
// Forward declarations
class Pose3;
/**
* 3D similarity transform
*/
class Similarity3: public LieGroup<Similarity3, 7> {
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
private:
Rotation R_;
Translation t_;
double s_;
public:
/// @name Constructors
/// @{
Similarity3();
/// Construct pure scaling
Similarity3(double s);
/// Construct from GTSAM types
Similarity3(const Rotation& R, const Translation& t, double s);
/// Construct from Eigen types
Similarity3(const Matrix3& R, const Vector3& t, double s);
/// @}
/// @name Testable
/// @{
/// Compare with tolerance
bool equals(const Similarity3& sim, double tol) const;
/// Compare with standard tolerance
bool operator==(const Similarity3& other) const;
/// Print with optional string
void print(const std::string& s) const;
/// @}
/// @name Group
/// @{
/// Return an identity transform
static Similarity3 identity();
/// Return the inverse
Similarity3 inverse() const;
Translation transform_from(const Translation& p) const;
/** syntactic sugar for transform_from */
inline Translation operator*(const Translation& p) const;
Similarity3 operator*(const Similarity3& T) const;
/// @}
/// @name Standard interface
/// @{
/// Return a GTSAM rotation
const Rotation& rotation() const {
return R_;
};
/// Return a GTSAM translation
const Translation& translation() const {
return t_;
};
/// Return the scale
double scale() const {
return s_;
};
/// Convert to a rigid body pose
operator Pose3() const;
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() {
return 7;
};
/// Dimensionality of tangent space = 7 DOF
inline size_t dim() const {
return 7;
};
/// @}
/// @name Chart
/// @{
/// Update Similarity transform via 7-dim vector in tangent space
struct ChartAtOrigin {
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none);
/// 7-dimensional vector v in tangent space that makes other = this->retract(v)
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none);
};
/// Project from one tangent space to another
Matrix7 AdjointMap() const;
/// @}
/// @name Stubs
/// @{
/// Not currently implemented, required because this is a lie group
static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
using LieGroup<Similarity3, 7>::inverse; // version with derivative
};
template<>
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
}

256
gtsam_unstable/geometry/tests/testSimilarity3.cpp Normal file
View File

@ -0,0 +1,256 @@
/* ----------------------------------------------------------------------------
* 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 testSimilarity3.cpp
* @brief Unit tests for Similarity3 class
* @author Paul Drews
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace std;
using symbol_shorthand::X;
GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
static Point3 P(0.2,0.7,-2);
static Rot3 R = Rot3::rodriguez(0.3,0,0);
static Similarity3 T(R,Point3(3.5,-8.2,4.2),1);
static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1);
static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1);
//******************************************************************************
TEST(Similarity3, Constructors) {
Similarity3 test;
}
//******************************************************************************
TEST(Similarity3, Getters) {
Similarity3 test;
EXPECT(assert_equal(Rot3(), test.rotation()));
EXPECT(assert_equal(Point3(), test.translation()));
EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9);
}
//******************************************************************************
TEST(Similarity3, Getters2) {
Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation()));
EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
}
TEST(Similarity3, AdjointMap) {
Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Matrix7 result;
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000,
6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000,
-2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000,
0, 0, 0, -0.2248, -0.3502, -0.9093, 0,
0, 0, 0, 0.9024, -0.4269, -0.0587, 0,
0, 0, 0, -0.3676, -0.8337, 0.4120, 0,
0, 0, 0, 0, 0, 0, 1.0000;
EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
}
TEST(Similarity3, inverse) {
Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Matrix3 Re;
Re << -0.2248, 0.9024, -0.3676,
-0.3502, -0.4269, -0.8337,
-0.9093, -0.0587, 0.4120;
Vector3 te(-9.8472, 59.7640, 10.2125);
Similarity3 expected(Re, te, 1.0/7.0);
EXPECT(assert_equal(expected, test.inverse(), 1e-3));
}
TEST(Similarity3, multiplication) {
Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11);
Matrix3 re;
re << 0.0688, 0.9863, -0.1496,
-0.5665, -0.0848, -0.8197,
-0.8211, 0.1412, 0.5530;
Vector3 te(-13.6797, 3.2441, -5.7794);
Similarity3 expected(re, te, 77);
EXPECT(assert_equal(expected, test1*test2, 1e-2));
}
//******************************************************************************
TEST(Similarity3, Manifold) {
EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
Vector z = Vector7::Zero();
Similarity3 sim;
EXPECT(sim.retract(z) == sim);
Vector7 v = Vector7::Zero();
v(6) = 2;
Similarity3 sim2;
EXPECT(sim2.retract(z) == sim2);
EXPECT(assert_equal(z, sim2.localCoordinates(sim)));
Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
Vector v3(7);
v3 << 0, 0, 0, 1, 2, 3, 0;
EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
Vector vlocal = sim.localCoordinates(other);
EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
Rot3 R = Rot3::rodriguez(0.3,0,0);
Vector vlocal2 = sim.localCoordinates(other2);
EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2));
// TODO add unit tests for retract and localCoordinates
}
/* ************************************************************************* */
TEST( Similarity3, retract_first_order)
{
Similarity3 id;
Vector v = zero(7);
v(0) = 0.3;
EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2;
EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2));
}
/* ************************************************************************* */
TEST(Similarity3, localCoordinates_first_order)
{
Vector d12 = repeat(7,0.1);
d12(6) = 1.0;
Similarity3 t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
/* ************************************************************************* */
TEST(Similarity3, manifold_first_order)
{
Similarity3 t1 = T;
Similarity3 t2 = T3;
Similarity3 origin;
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
}
TEST(Similarity3, Optimization) {
Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
Symbol key('x',1);
PriorFactor<Similarity3> factor(key, prior, model);
NonlinearFactorGraph graph;
graph.push_back(factor);
Values initial;
initial.insert<Similarity3>(key, Similarity3());
Values result;
LevenbergMarquardtParams params;
params.setVerbosityLM("TRYCONFIG");
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
}
TEST(Similarity3, Optimization2) {
Similarity3 prior = Similarity3();
Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
Similarity3 loop = Similarity3(1.42);
//prior.print("Goal Transform");
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01);
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
PriorFactor<Similarity3> factor(X(1), prior, model);
BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise);
BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2);
NonlinearFactorGraph graph;
graph.push_back(factor);
graph.push_back(b1);
graph.push_back(b2);
graph.push_back(b3);
graph.push_back(b4);
graph.push_back(lc);
//graph.print("Full Graph\n");
Values initial;
initial.insert<Similarity3>(X(1), Similarity3());
initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
initial.insert<Similarity3>(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
//initial.print("Initial Estimate\n");
Values result;
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
//result.print("Optimized Estimate\n");
Pose3 p1, p2, p3, p4, p5;
p1 = Pose3(result.at<Similarity3>(X(1)));
p2 = Pose3(result.at<Similarity3>(X(2)));
p3 = Pose3(result.at<Similarity3>(X(3)));
p4 = Pose3(result.at<Similarity3>(X(4)));
p5 = Pose3(result.at<Similarity3>(X(5)));
//p1.print("Pose1");
//p2.print("Pose2");
//p3.print("Pose3");
//p4.print("Pose4");
//p5.print("Pose5");
Similarity3 expected(0.7);
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

2
gtsam_unstable/gtsam_unstable.h
View File

@ -477,7 +477,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
};
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
virtual class VelocityConstraint3 : gtsam::NonlinearFactor {

5
gtsam_unstable/slam/serialization.cpp
View File

@ -5,6 +5,8 @@
* @author Alex Cunningham
*/
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/slam/serialization.h>
#include <gtsam/base/serialization.h>
@ -22,9 +24,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieMatrix.h>
//#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3DS2.h>

3
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -20,9 +20,8 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieVector_Deprecated.h>
using namespace std;
using namespace gtsam;

3
tests/testGaussianISAM2.cpp
View File

@ -23,10 +23,9 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/LieScalar.h>
#include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/base/LieScalar_Deprecated.h>
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp>
namespace br { using namespace boost::adaptors; using namespace boost::range; }