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

Re-factored scalar traits and fixed Matrix traits (bug in RowMajor, compile issues for dynamic types)

release/4.3a0
dellaert 2014-12-26 16:11:53 +01:00
parent 27156ec8c1
commit 74c588aee9
4 changed files with 328 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

320
gtsam/base/VectorSpace.h
View File

@ -24,12 +24,8 @@ namespace internal {
template<class Class, int N> template<class Class, int N>
struct VectorSpaceImpl { struct VectorSpaceImpl {
typedef vector_space_tag structure_category;
/// @name Group /// @name Group
/// @{ /// @{
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity();}
static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
static Class Between(const Class& v1, const Class& v2) { return v2-v1;} static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
static Class Inverse(const Class& m) { return -m;} static Class Inverse(const Class& m) { return -m;}
@ -54,7 +50,7 @@ struct VectorSpaceImpl {
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Jacobian::Identity(); if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity();
return origin + Class(v); return origin + (Class)v;
} }
/// @} /// @}
@ -100,8 +96,6 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @name Group /// @name Group
/// @{ /// @{
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity();}
static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
static Class Between(const Class& v1, const Class& v2) { return v2-v1;} static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
static Class Inverse(const Class& m) { return -m;} static Class Inverse(const Class& m) { return -m;}
@ -180,6 +174,12 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
typedef vector_space_tag structure_category; typedef vector_space_tag structure_category;
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity();}
/// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{
enum { dimension = Class::dimension}; enum { dimension = Class::dimension};
@ -192,96 +192,62 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
/// To use this for your gtsam type, define: /// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public ScalarTraits<Type> { }; /// template<> struct traits<Type> : public ScalarTraits<Type> { };
template<typename Scalar> template<typename Scalar>
struct ScalarTraits { struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
typedef vector_space_tag structure_category; typedef vector_space_tag structure_category;
/// @name Testable
/// @{
static void Print(Scalar m, const std::string& str = "") {
gtsam::print(m, str);
}
static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
return std::abs(v1 - v2) < tol;
}
/// @}
/// @name Group /// @name Group
/// @{ /// @{
typedef additive_group_tag group_flavor; typedef additive_group_tag group_flavor;
static Scalar Identity() { return 0;} static Scalar Identity() { return 0;}
static Scalar Compose(const Scalar& v1, const Scalar& v2) { return v1+v2;}
static Scalar Between(const Scalar& v1, const Scalar& v2) { return v2-v1;}
static Scalar Inverse(const Scalar& m) { return -m;}
/// @} /// @}
// Typedefs required by all manifold types. /// @name Manifold
/// @{
typedef Scalar ManifoldType; typedef Scalar ManifoldType;
enum { dimension = 1 }; enum { dimension = 1 };
typedef Eigen::Matrix<double, 1, 1> TangentVector; typedef Eigen::Matrix<double, 1, 1> TangentVector;
typedef OptionalJacobian<1, 1> ChartJacobian; typedef OptionalJacobian<1, 1> ChartJacobian;
// For Testable static TangentVector Local(Scalar origin, Scalar other,
static void Print(Scalar m, const std::string& str = "") { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
gtsam::print(m, str); if (H1) (*H1)[0] = -1.0;
} if (H2) (*H2)[0] = 1.0;
static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
return fabs(v1 - v2) < tol;
}
static TangentVector Local(Scalar origin, Scalar other) {
TangentVector result; TangentVector result;
result(0) = other - origin; result(0) = other - origin;
return result; return result;
} }
static Scalar Retract(Scalar origin, const TangentVector& v) {
return origin + v[0];
}
static TangentVector Local(Scalar origin, Scalar other, ChartJacobian Horigin,
ChartJacobian Hother = boost::none) {
if (Horigin) (*Horigin)[0] = -1.0;
if (Hother) (*Hother)[0] = 1.0;
return Local(origin, other);
}
static Scalar Retract(Scalar origin, const TangentVector& v, static Scalar Retract(Scalar origin, const TangentVector& v,
ChartJacobian Horigin, ChartJacobian Hv = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (Horigin) (*Horigin)[0] = 1.0;
if (Hv) (*Hv)[0] = 1.0;
return origin + v[0];
}
static int GetDimension(Scalar m) {return 1;}
static Scalar Compose(Scalar v1, Scalar v2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = 1.0; if (H1) (*H1)[0] = 1.0;
if (H2) (*H2)[0] = 1.0; if (H2) (*H2)[0] = 1.0;
return v1 + v2; return origin + v[0];
} }
/// @}
static Scalar Between(Scalar v1, Scalar v2, ChartJacobian H1, /// @name Lie Group
ChartJacobian H2 = boost::none) { /// @{
if (H1) (*H1)[0] = -1.0; static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
if (H2) (*H2)[0] = 1.0; if (H) (*H)[0] = 1.0;
return v2 - v1;
}
static Scalar Inverse(Scalar m, ChartJacobian H) {
if (H) (*H)[0] = -1;
return -m;
}
static TangentVector Logmap(Scalar m) {
return Local(0, m);
}
static Scalar Expmap(const TangentVector& v) {
return v[0];
}
static TangentVector Logmap(Scalar m, ChartJacobian Hm) {
if (Hm)
(*Hm)[0] = 1.0;
return Local(0, m); return Local(0, m);
} }
static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) { static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
if (Hv) if (H) (*H)[0] = 1.0;
(*Hv)[0] = 1.0;
return v[0]; return v[0];
} }
/// @}
}; };
@ -295,108 +261,190 @@ template<> struct traits_x<double> : public internal::ScalarTraits<double> {
template<> struct traits_x<float> : public internal::ScalarTraits<float> { template<> struct traits_x<float> : public internal::ScalarTraits<float> {
}; };
// traits for any double Eigen matrix // traits for any fixed double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols> template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits_x<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > { struct traits_x<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
// BOOST_STATIC_ASSERT_MSG( internal::VectorSpaceImpl<
// M != Eigen::Dynamic && N != Eigen::Dynamic, Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
// "These traits are only valid on fixed-size types.");
// Typedefs required by all manifold types.
typedef vector_space_tag structure_category; typedef vector_space_tag structure_category;
enum { typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
dimension = (
M == Eigen::Dynamic ? Eigen::Dynamic :
(N == Eigen::Dynamic ? Eigen::Dynamic : M * N))
};
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> ManifoldType;
static int GetDimension(const ManifoldType& m) { /// @name Testable
/// @{
static void Print(const Fixed& m, const std::string& str = "") {
gtsam::print(Eigen::MatrixXd(m), str);
}
static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) {
return equal_with_abs_tol(v1, v2, tol);
}
/// @}
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Fixed Identity() { return Fixed::Zero();}
/// @}
/// @name Manifold
/// @{
enum { dimension = M*N};
typedef Fixed ManifoldType;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(Fixed origin, Fixed other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1) = -Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = other - origin;
return result;
}
static Fixed Retract(Fixed origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1) = Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
return origin + Eigen::Map<const Fixed>(v.data());
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
if (H) *H = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = m;
return result;
}
static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
Fixed m;
m.setZero();
if (H) *H = Jacobian::Identity();
return m + Eigen::Map<const Fixed>(v.data());
}
/// @}
};
namespace internal {
// traits for dynamic Eigen matrices
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct DynamicTraits {
typedef vector_space_tag structure_category;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Dynamic;
/// @name Testable
/// @{
static void Print(const Dynamic& m, const std::string& str = "") {
gtsam::print(Eigen::MatrixXd(m), str);
}
static bool Equals(const Dynamic& v1, const Dynamic& v2,
double tol = 1e-8) {
return equal_with_abs_tol(v1, v2, tol);
}
/// @}
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Dynamic Identity() {
throw std::runtime_error("Identity not defined for dynamic types");
}
/// @}
/// @name Manifold
/// @{
enum { dimension = Eigen::Dynamic };
typedef Eigen::VectorXd TangentVector;
typedef Eigen::MatrixXd Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Dynamic ManifoldType;
static int GetDimension(const Dynamic& m) {
return m.rows() * m.cols(); return m.rows() * m.cols();
} }
static Eigen::Matrix<double, dimension, dimension> Eye( static Jacobian Eye(const Dynamic& m) {
const ManifoldType& m) {
int dim = GetDimension(m); int dim = GetDimension(m);
return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim); return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
} }
// For Testable static TangentVector Local(const Dynamic& m, const Dynamic& other, //
static void Print(const ManifoldType& m, const std::string& str = "") { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
gtsam::print(Eigen::MatrixXd(m), str); if (H1) *H1 = -Eye(m);
} if (H2) *H2 = Eye(m);
static bool Equals(const ManifoldType& v1, const ManifoldType& v2, TangentVector v(GetDimension(m));
double tol = 1e-8) { Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
return equal_with_abs_tol(v1, v2, tol); return v;
} }
static TangentVector Local(const ManifoldType& origin, static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
const ManifoldType& other, ChartJacobian Horigin = boost::none, ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
ChartJacobian Hother = boost::none) { if (H1) *H1 = Eye(m);
if (Horigin) *Horigin = -Eye(origin); if (H2) *H2 = Eye(m);
if (Hother) *Hother = Eye(origin); return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
TangentVector result(GetDimension(origin)); }
Eigen::Map<Eigen::Matrix<double, M, N> >(result.data(), origin.rows(), /// @}
origin.cols()) = other - origin;
/// @name Lie Group
/// @{
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) {
if (H) *H = Eye(m);
TangentVector result(GetDimension(m));
Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
return result; return result;
} }
static ManifoldType Retract(const ManifoldType& origin, static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
const TangentVector& v, ChartJacobian Horigin = boost::none, throw std::runtime_error("Expmap not defined for dynamic types");
ChartJacobian Hv = boost::none) {
if (Horigin) *Horigin = Eye(origin);
if (Hv) *Hv = Eye(origin);
return origin
+ Eigen::Map<const Eigen::Matrix<double, M, N> >(v.data(),
origin.rows(), origin.cols());
} }
static ManifoldType Compose(const ManifoldType& v1, const ManifoldType& v2, static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) {
if (H) *H = -Eye(m);
return -m;
}
static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(v1); if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v1); if (H2) *H2 = Eye(v1);
return v1 + v2; return v1 + v2;
} }
static ManifoldType Between(const ManifoldType& v1, const ManifoldType& v2, static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = -Eye(v1); if (H1) *H1 = -Eye(v1);
if (H2) *H2 = Eye(v1); if (H2) *H2 = Eye(v1);
return v2 - v1; return v2 - v1;
} }
/// @}
static ManifoldType Inverse(const ManifoldType& m, ChartJacobian H = };
boost::none) {
if (H) *H = -Eye(m);
return -m;
}
static ManifoldType Identity() { } // \ internal
//FIXME: this won't work for dynamic matrices, but where to get the size???
return ManifoldType::Zero();
}
static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = // traits for fully dynamic matrix
boost::none) { template<int Options, int MaxRows, int MaxCols>
if (Hm) struct traits_x<Eigen::Matrix<double, -1, -1, Options, MaxRows, MaxCols> > :
*Hm = Eye(m); public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> {
TangentVector result(GetDimension(m)); };
Eigen::Map<Eigen::Matrix<double, M, N> >(result.data()) = m;
return result;
}
//FIXME: this also does not work for dynamic matrices // traits for dynamic column vector
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = template<int Options, int MaxRows, int MaxCols>
boost::none) { struct traits_x<Eigen::Matrix<double, -1, 1, Options, MaxRows, MaxCols> > :
ManifoldType m; public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> {
m.setZero(); };
if (Hv)
*Hv = Eye(m);
return m + Eigen::Map<Eigen::Matrix<double, M, N> >(v.data());
}
// traits for dynamic row vector
template<int Options, int MaxRows, int MaxCols>
struct traits_x<Eigen::Matrix<double, 1, -1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> {
}; };
/// Vector Space concept /// Vector Space concept

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

@ -16,12 +16,14 @@
* @author Carlos Nieto * @author Carlos Nieto
**/ **/
#include <iostream> #include <gtsam/base/Matrix.h>
#include <sstream> #include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/base/Matrix.h> #include <iostream>
#include <sstream>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -30,7 +32,7 @@ static double inf = std::numeric_limits<double>::infinity();
static const double tol = 1e-9; static const double tol = 1e-9;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, constructor_data ) TEST(Matrix, constructor_data )
{ {
Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished(); Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished();
@ -44,7 +46,7 @@ TEST( matrix, constructor_data )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, Matrix_ ) TEST(Matrix, Matrix_ )
{ {
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished(); Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished();
Matrix B(2, 2); Matrix B(2, 2);
@ -74,7 +76,7 @@ namespace {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, special_comma_initializer) TEST(Matrix, special_comma_initializer)
{ {
Matrix expected(2,2); Matrix expected(2,2);
expected(0,0) = 1; expected(0,0) = 1;
@ -103,7 +105,7 @@ TEST( matrix, special_comma_initializer)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, col_major ) TEST(Matrix, col_major )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
const double * const a = &A(0, 0); const double * const a = &A(0, 0);
@ -114,7 +116,7 @@ TEST( matrix, col_major )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, collect1 ) TEST(Matrix, collect1 )
{ {
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
@ -131,7 +133,7 @@ TEST( matrix, collect1 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, collect2 ) TEST(Matrix, collect2 )
{ {
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
@ -151,7 +153,7 @@ TEST( matrix, collect2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, collect3 ) TEST(Matrix, collect3 )
{ {
Matrix A, B; Matrix A, B;
A = eye(2, 3); A = eye(2, 3);
@ -168,7 +170,7 @@ TEST( matrix, collect3 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, stack ) TEST(Matrix, stack )
{ {
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
@ -191,7 +193,7 @@ TEST( matrix, stack )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, column ) TEST(Matrix, column )
{ {
Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
@ -210,7 +212,7 @@ TEST( matrix, column )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, insert_column ) TEST(Matrix, insert_column )
{ {
Matrix big = zeros(5, 6); Matrix big = zeros(5, 6);
Vector col = ones(5); Vector col = ones(5);
@ -229,7 +231,7 @@ TEST( matrix, insert_column )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, insert_subcolumn ) TEST(Matrix, insert_subcolumn )
{ {
Matrix big = zeros(5, 6); Matrix big = zeros(5, 6);
Vector col1 = ones(2); Vector col1 = ones(2);
@ -252,7 +254,7 @@ TEST( matrix, insert_subcolumn )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, row ) TEST(Matrix, row )
{ {
Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
@ -271,7 +273,7 @@ TEST( matrix, row )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, zeros ) TEST(Matrix, zeros )
{ {
Matrix A(2, 3); Matrix A(2, 3);
A(0, 0) = 0; A(0, 0) = 0;
@ -287,7 +289,7 @@ TEST( matrix, zeros )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, insert_sub ) TEST(Matrix, insert_sub )
{ {
Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
1.0).finished(); 1.0).finished();
@ -302,7 +304,7 @@ TEST( matrix, insert_sub )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, diagMatrices ) TEST(Matrix, diagMatrices )
{ {
std::vector<Matrix> Hs; std::vector<Matrix> Hs;
Hs.push_back(ones(3,3)); Hs.push_back(ones(3,3));
@ -326,7 +328,7 @@ TEST( matrix, diagMatrices )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, stream_read ) { TEST(Matrix, stream_read ) {
Matrix expected = (Matrix(3,4) << Matrix expected = (Matrix(3,4) <<
1.1, 2.3, 4.2, 7.6, 1.1, 2.3, 4.2, 7.6,
-0.3, -8e-2, 5.1, 9.0, -0.3, -8e-2, 5.1, 9.0,
@ -346,7 +348,7 @@ TEST( matrix, stream_read ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, scale_columns ) TEST(Matrix, scale_columns )
{ {
Matrix A(3, 4); Matrix A(3, 4);
A(0, 0) = 1.; A(0, 0) = 1.;
@ -384,7 +386,7 @@ TEST( matrix, scale_columns )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, scale_rows ) TEST(Matrix, scale_rows )
{ {
Matrix A(3, 4); Matrix A(3, 4);
A(0, 0) = 1.; A(0, 0) = 1.;
@ -422,7 +424,7 @@ TEST( matrix, scale_rows )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, scale_rows_mask ) TEST(Matrix, scale_rows_mask )
{ {
Matrix A(3, 4); Matrix A(3, 4);
A(0, 0) = 1.; A(0, 0) = 1.;
@ -460,7 +462,7 @@ TEST( matrix, scale_rows_mask )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, skewSymmetric ) TEST(Matrix, skewSymmetric )
{ {
double wx = 1, wy = 2, wz = 3; double wx = 1, wy = 2, wz = 3;
Matrix3 actual = skewSymmetric(wx,wy,wz); Matrix3 actual = skewSymmetric(wx,wy,wz);
@ -476,7 +478,7 @@ TEST( matrix, skewSymmetric )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, equal ) TEST(Matrix, equal )
{ {
Matrix A(4, 4); Matrix A(4, 4);
A(0, 0) = -1; A(0, 0) = -1;
@ -506,7 +508,7 @@ TEST( matrix, equal )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, equal_nan ) TEST(Matrix, equal_nan )
{ {
Matrix A(4, 4); Matrix A(4, 4);
A(0, 0) = -1; A(0, 0) = -1;
@ -535,7 +537,7 @@ TEST( matrix, equal_nan )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, addition ) TEST(Matrix, addition )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -544,7 +546,7 @@ TEST( matrix, addition )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, addition_in_place ) TEST(Matrix, addition_in_place )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -554,7 +556,7 @@ TEST( matrix, addition_in_place )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, subtraction ) TEST(Matrix, subtraction )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -563,7 +565,7 @@ TEST( matrix, subtraction )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, subtraction_in_place ) TEST(Matrix, subtraction_in_place )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -573,7 +575,7 @@ TEST( matrix, subtraction_in_place )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, multiplication ) TEST(Matrix, multiplication )
{ {
Matrix A(2, 2); Matrix A(2, 2);
A(0, 0) = -1; A(0, 0) = -1;
@ -593,7 +595,7 @@ TEST( matrix, multiplication )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, scalar_matrix_multiplication ) TEST(Matrix, scalar_matrix_multiplication )
{ {
Vector result(2); Vector result(2);
@ -613,7 +615,7 @@ TEST( matrix, scalar_matrix_multiplication )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, matrix_vector_multiplication ) TEST(Matrix, matrix_vector_multiplication )
{ {
Vector result(2); Vector result(2);
@ -627,7 +629,7 @@ TEST( matrix, matrix_vector_multiplication )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, nrRowsAndnrCols ) TEST(Matrix, nrRowsAndnrCols )
{ {
Matrix A(3, 6); Matrix A(3, 6);
LONGS_EQUAL( A.rows() , 3 ); LONGS_EQUAL( A.rows() , 3 );
@ -635,7 +637,7 @@ TEST( matrix, nrRowsAndnrCols )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, scalar_divide ) TEST(Matrix, scalar_divide )
{ {
Matrix A(2, 2); Matrix A(2, 2);
A(0, 0) = 10; A(0, 0) = 10;
@ -653,7 +655,7 @@ TEST( matrix, scalar_divide )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, zero_below_diagonal ) { TEST(Matrix, zero_below_diagonal ) {
Matrix A1 = (Matrix(3, 4) << Matrix A1 = (Matrix(3, 4) <<
1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0,
1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0,
@ -708,7 +710,7 @@ TEST( matrix, zero_below_diagonal ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, inverse ) TEST(Matrix, inverse )
{ {
Matrix A(3, 3); Matrix A(3, 3);
A(0, 0) = 1; A(0, 0) = 1;
@ -754,7 +756,7 @@ TEST( matrix, inverse )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, inverse2 ) TEST(Matrix, inverse2 )
{ {
Matrix A(3, 3); Matrix A(3, 3);
A(0, 0) = 0; A(0, 0) = 0;
@ -784,7 +786,7 @@ TEST( matrix, inverse2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, backsubtitution ) TEST(Matrix, backsubtitution )
{ {
// TEST ONE 2x2 matrix U1*x=b1 // TEST ONE 2x2 matrix U1*x=b1
Vector expected1 = Vector2(3.6250, -0.75); Vector expected1 = Vector2(3.6250, -0.75);
@ -809,7 +811,7 @@ TEST( matrix, backsubtitution )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, householder ) TEST(Matrix, householder )
{ {
// check in-place householder, with v vectors below diagonal // check in-place householder, with v vectors below diagonal
@ -838,7 +840,7 @@ TEST( matrix, householder )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, householder_colMajor ) TEST(Matrix, householder_colMajor )
{ {
// check in-place householder, with v vectors below diagonal // check in-place householder, with v vectors below diagonal
@ -867,7 +869,7 @@ TEST( matrix, householder_colMajor )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, eigen_QR ) TEST(Matrix, eigen_QR )
{ {
// use standard Eigen function to yield a non-in-place QR factorization // use standard Eigen function to yield a non-in-place QR factorization
@ -898,7 +900,7 @@ TEST( matrix, eigen_QR )
// unit test for qr factorization (and hence householder) // unit test for qr factorization (and hence householder)
// This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs // This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, qr ) TEST(Matrix, qr )
{ {
Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
@ -921,7 +923,7 @@ TEST( matrix, qr )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, sub ) TEST(Matrix, sub )
{ {
Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
0, 00, 10, 0, 0, 0, -10).finished(); 0, 00, 10, 0, 0, 0, -10).finished();
@ -933,7 +935,7 @@ TEST( matrix, sub )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, trans ) TEST(Matrix, trans )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
@ -941,7 +943,7 @@ TEST( matrix, trans )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, col_major_access ) TEST(Matrix, col_major_access )
{ {
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
const double* a = &A(0, 0); const double* a = &A(0, 0);
@ -949,7 +951,7 @@ TEST( matrix, col_major_access )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, weighted_elimination ) TEST(Matrix, weighted_elimination )
{ {
// create a matrix to eliminate // create a matrix to eliminate
Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
@ -984,7 +986,7 @@ TEST( matrix, weighted_elimination )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, inverse_square_root ) TEST(Matrix, inverse_square_root )
{ {
Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
0.0, 0.0, 0.0, 0.01).finished(); 0.0, 0.0, 0.0, 0.01).finished();
@ -1036,22 +1038,22 @@ Matrix expected = (Matrix(5, 5) <<
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished(); 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished();
} }
TEST( matrix, LLt ) TEST(Matrix, LLt )
{ {
EQUALITY(cholesky::expected, LLt(cholesky::M)); EQUALITY(cholesky::expected, LLt(cholesky::M));
} }
TEST( matrix, RtR ) TEST(Matrix, RtR )
{ {
EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
} }
TEST( matrix, cholesky_inverse ) TEST(Matrix, cholesky_inverse )
{ {
EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, multiplyAdd ) TEST(Matrix, multiplyAdd )
{ {
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
@ -1062,7 +1064,7 @@ TEST( matrix, multiplyAdd )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, transposeMultiplyAdd ) TEST(Matrix, transposeMultiplyAdd )
{ {
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
@ -1073,7 +1075,7 @@ TEST( matrix, transposeMultiplyAdd )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, linear_dependent ) TEST(Matrix, linear_dependent )
{ {
Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
@ -1081,7 +1083,7 @@ TEST( matrix, linear_dependent )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, linear_dependent2 ) TEST(Matrix, linear_dependent2 )
{ {
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
@ -1089,7 +1091,7 @@ TEST( matrix, linear_dependent2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, linear_dependent3 ) TEST(Matrix, linear_dependent3 )
{ {
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished();
@ -1097,7 +1099,7 @@ TEST( matrix, linear_dependent3 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, svd1 ) TEST(Matrix, svd1 )
{ {
Vector v = Vector3(2., 1., 0.); Vector v = Vector3(2., 1., 0.);
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
@ -1116,7 +1118,7 @@ static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished();
static Matrix sampleAt = trans(sampleA); static Matrix sampleAt = trans(sampleA);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, svd2 ) TEST(Matrix, svd2 )
{ {
Matrix U, V; Matrix U, V;
Vector s; Vector s;
@ -1139,7 +1141,7 @@ TEST( matrix, svd2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, svd3 ) TEST(Matrix, svd3 )
{ {
Matrix U, V; Matrix U, V;
Vector s; Vector s;
@ -1167,7 +1169,7 @@ TEST( matrix, svd3 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, svd4 ) TEST(Matrix, svd4 )
{ {
Matrix U, V; Matrix U, V;
Vector s; Vector s;
@ -1209,7 +1211,7 @@ TEST( matrix, svd4 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, DLT ) TEST(Matrix, DLT )
{ {
Matrix A = (Matrix(8, 9) << Matrix A = (Matrix(8, 9) <<
0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11,
@ -1231,6 +1233,18 @@ TEST( matrix, DLT )
EXPECT(assert_equal(expected, actual, 1e-4)); EXPECT(assert_equal(expected, actual, 1e-4));
} }
//******************************************************************************
TEST(Matrix , IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>));
typedef Eigen::Matrix<double,2,3,Eigen::RowMajor> RowMajor;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowMajor>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

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

@ -15,10 +15,12 @@
* @author Frank Dellaert * @author Frank Dellaert
**/ **/
#include <iostream> #include <gtsam/base/Vector.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <gtsam/base/Vector.h> #include <iostream>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -40,7 +42,7 @@ namespace {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, special_comma_initializer) TEST(Vector, special_comma_initializer)
{ {
Vector expected(3); Vector expected(3);
expected(0) = 1; expected(0) = 1;
@ -68,7 +70,7 @@ TEST( TestVector, special_comma_initializer)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, copy ) TEST(Vector, copy )
{ {
Vector a(2); a(0) = 10; a(1) = 20; Vector a(2); a(0) = 10; a(1) = 20;
double data[] = {10,20}; double data[] = {10,20};
@ -78,14 +80,14 @@ TEST( TestVector, copy )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, zero1 ) TEST(Vector, zero1 )
{ {
Vector v = Vector::Zero(2); Vector v = Vector::Zero(2);
EXPECT(zero(v)); EXPECT(zero(v));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, zero2 ) TEST(Vector, zero2 )
{ {
Vector a = zero(2); Vector a = zero(2);
Vector b = Vector::Zero(2); Vector b = Vector::Zero(2);
@ -94,7 +96,7 @@ TEST( TestVector, zero2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, scalar_multiply ) TEST(Vector, scalar_multiply )
{ {
Vector a(2); a(0) = 10; a(1) = 20; Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = 1; b(1) = 2; Vector b(2); b(0) = 1; b(1) = 2;
@ -102,7 +104,7 @@ TEST( TestVector, scalar_multiply )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, scalar_divide ) TEST(Vector, scalar_divide )
{ {
Vector a(2); a(0) = 10; a(1) = 20; Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = 1; b(1) = 2; Vector b(2); b(0) = 1; b(1) = 2;
@ -110,7 +112,7 @@ TEST( TestVector, scalar_divide )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, negate ) TEST(Vector, negate )
{ {
Vector a(2); a(0) = 10; a(1) = 20; Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = -10; b(1) = -20; Vector b(2); b(0) = -10; b(1) = -20;
@ -118,7 +120,7 @@ TEST( TestVector, negate )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, sub ) TEST(Vector, sub )
{ {
Vector a(6); Vector a(6);
a(0) = 10; a(1) = 20; a(2) = 3; a(0) = 10; a(1) = 20; a(2) = 3;
@ -134,7 +136,7 @@ TEST( TestVector, sub )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, subInsert ) TEST(Vector, subInsert )
{ {
Vector big = zero(6), Vector big = zero(6),
small = ones(3); small = ones(3);
@ -148,7 +150,7 @@ TEST( TestVector, subInsert )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, householder ) TEST(Vector, householder )
{ {
Vector x(4); Vector x(4);
x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1; x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1;
@ -163,7 +165,7 @@ TEST( TestVector, householder )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, concatVectors) TEST(Vector, concatVectors)
{ {
Vector A(2); Vector A(2);
for(int i = 0; i < 2; i++) for(int i = 0; i < 2; i++)
@ -187,7 +189,7 @@ TEST( TestVector, concatVectors)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse ) TEST(Vector, weightedPseudoinverse )
{ {
// column from a matrix // column from a matrix
Vector x(2); Vector x(2);
@ -213,7 +215,7 @@ TEST( TestVector, weightedPseudoinverse )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_constraint ) TEST(Vector, weightedPseudoinverse_constraint )
{ {
// column from a matrix // column from a matrix
Vector x(2); Vector x(2);
@ -238,7 +240,7 @@ TEST( TestVector, weightedPseudoinverse_constraint )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_nan ) TEST(Vector, weightedPseudoinverse_nan )
{ {
Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
@ -252,7 +254,7 @@ TEST( TestVector, weightedPseudoinverse_nan )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, ediv ) TEST(Vector, ediv )
{ {
Vector a = Vector3(10., 20., 30.); Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0); Vector b = Vector3(2.0, 5.0, 6.0);
@ -263,7 +265,7 @@ TEST( TestVector, ediv )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, dot ) TEST(Vector, dot )
{ {
Vector a = Vector3(10., 20., 30.); Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0); Vector b = Vector3(2.0, 5.0, 6.0);
@ -271,7 +273,7 @@ TEST( TestVector, dot )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, axpy ) TEST(Vector, axpy )
{ {
Vector x = Vector3(10., 20., 30.); Vector x = Vector3(10., 20., 30.);
Vector y0 = Vector3(2.0, 5.0, 6.0); Vector y0 = Vector3(2.0, 5.0, 6.0);
@ -284,7 +286,7 @@ TEST( TestVector, axpy )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, equals ) TEST(Vector, equals )
{ {
Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()).finished(); //testing nan Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()).finished(); //testing nan
Vector v2 = (Vector(1) << 1.0).finished(); Vector v2 = (Vector(1) << 1.0).finished();
@ -293,7 +295,7 @@ TEST( TestVector, equals )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, greater_than ) TEST(Vector, greater_than )
{ {
Vector v1 = Vector3(1.0, 2.0, 3.0), Vector v1 = Vector3(1.0, 2.0, 3.0),
v2 = zero(3); v2 = zero(3);
@ -302,14 +304,14 @@ TEST( TestVector, greater_than )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, reciprocal ) TEST(Vector, reciprocal )
{ {
Vector v = Vector3(1.0, 2.0, 4.0); Vector v = Vector3(1.0, 2.0, 4.0);
EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, linear_dependent ) TEST(Vector, linear_dependent )
{ {
Vector v1 = Vector3(1.0, 2.0, 3.0); Vector v1 = Vector3(1.0, 2.0, 3.0);
Vector v2 = Vector3(-2.0, -4.0, -6.0); Vector v2 = Vector3(-2.0, -4.0, -6.0);
@ -317,7 +319,7 @@ TEST( TestVector, linear_dependent )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, linear_dependent2 ) TEST(Vector, linear_dependent2 )
{ {
Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v1 = Vector3(0.0, 2.0, 0.0);
Vector v2 = Vector3(0.0, -4.0, 0.0); Vector v2 = Vector3(0.0, -4.0, 0.0);
@ -325,13 +327,21 @@ TEST( TestVector, linear_dependent2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, linear_dependent3 ) TEST(Vector, linear_dependent3 )
{ {
Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v1 = Vector3(0.0, 2.0, 0.0);
Vector v2 = Vector3(0.1, -4.1, 0.0); Vector v2 = Vector3(0.1, -4.1, 0.0);
EXPECT(!linear_dependent(v1, v2)); EXPECT(!linear_dependent(v1, v2));
} }
//******************************************************************************
TEST(Vector, IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

80
tests/testManifold.cpp
View File

@ -22,8 +22,8 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/VectorSpace.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#undef CHECK #undef CHECK
@ -39,34 +39,49 @@ using namespace gtsam;
// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler> Camera; typedef PinholeCamera<Cal3Bundler> Camera;
/* ************************************************************************* */ //******************************************************************************
// is_manifold TEST(Manifold, SomeManifoldsGTSAM) {
TEST(Manifold, IsManifold) {
//BOOST_CONCEPT_ASSERT((IsManifold<int>)); // integer is not a manifold //BOOST_CONCEPT_ASSERT((IsManifold<int>)); // integer is not a manifold
BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
BOOST_CONCEPT_ASSERT((IsManifold<Matrix24>));
BOOST_CONCEPT_ASSERT((IsManifold<double>));
BOOST_CONCEPT_ASSERT((IsManifold<Camera>)); BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
BOOST_CONCEPT_ASSERT((IsManifold<Cal3_S2>));
// dynamic not working yet BOOST_CONCEPT_ASSERT((IsManifold<Cal3Bundler>));
//BOOST_CONCEPT_ASSERT((IsManifold<Vector>)); BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
//BOOST_CONCEPT_ASSERT((IsManifold<Matrix>));
} }
/* ************************************************************************* */ //******************************************************************************
TEST(Manifold, SomeLieGroupsGTSAM) {
BOOST_CONCEPT_ASSERT((IsLieGroup<Rot2>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Pose2>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Pose3>));
}
//******************************************************************************
TEST(Manifold, SomeVectorSpacesGTSAM) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<double>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<float>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>));
}
//******************************************************************************
// dimension // dimension
TEST(Manifold, _dimension) { TEST(Manifold, _dimension) {
//using namespace traits; //using namespace traits;
EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension); EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension);
EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::dimension); EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::dimension);
EXPECT_LONGS_EQUAL(1, traits_x<double>::dimension); EXPECT_LONGS_EQUAL(1, traits_x<double>::dimension);
// EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Vector>::dimension);
// EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Matrix>::dimension);
} }
/* ************************************************************************* */ //******************************************************************************
TEST(Manifold, Identity) {
EXPECT_DOUBLES_EQUAL(0.0, traits_x<double>::Identity(), 0.0);
EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits_x<Matrix24>::Identity())));
EXPECT(assert_equal(Pose3(), traits_x<Pose3>::Identity()));
EXPECT(assert_equal(Point2(), traits_x<Point2>::Identity()));
}
//******************************************************************************
// charts // charts
TEST(Manifold, DefaultChart) { TEST(Manifold, DefaultChart) {
@ -134,35 +149,10 @@ TEST(Manifold, DefaultChart) {
EXPECT(assert_equal(zero(3), traits_x<Rot3>::Local(R, R))); EXPECT(assert_equal(zero(3), traits_x<Rot3>::Local(R, R)));
} }
/* ************************************************************************* */ //******************************************************************************
// zero
//TEST(Manifold, _zero) {
// EXPECT(assert_equal(Pose3(), traits::zero<Pose3>::value()));
// Cal3Bundler cal(0, 0, 0);
// EXPECT(assert_equal(cal, traits::zero<Cal3Bundler>::value()));
// EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero<Camera>::value()));
// EXPECT(assert_equal(Point2(), traits::zero<Point2>::value()));
// EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero<Matrix24>::value())));
// EXPECT_DOUBLES_EQUAL(0.0, traits::zero<double>::value(), 0.0);
//}
//
/* ************************************************************************* */
// identity
TEST(Manifold, _identity) {
EXPECT(assert_equal(Pose3(), traits_x<Pose3>::Identity()));
// BOOST_CONCEPT_ASSERT((IsLieGroup<Cal3Bundler>));
// EXPECT(assert_equal(Cal3Bundler(), traits_x<Cal3Bundler>::Identity()));
// BOOST_CONCEPT_ASSERT((IsLieGroup<Camera>));
// EXPECT(assert_equal(Camera(), traits_x<Camera>::Identity())); // not a lie group
EXPECT(assert_equal(Point2(), traits_x<Point2>::Identity()));
EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits_x<Matrix24>::Identity())));
EXPECT_DOUBLES_EQUAL(0.0, traits_x<double>::Identity(), 0.0);
}
/* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ //******************************************************************************