Re-factored scalar traits and fixed Matrix traits (bug in RowMajor, compile issues for dynamic types)
dellaert
parent
27156ec8c1
commit
74c588aee9
|
|
@ -24,12 +24,8 @@ namespace internal {
|
|||
template<class Class, int N>
|
||||
struct VectorSpaceImpl {
|
||||
|
||||
typedef vector_space_tag structure_category;
|
||||
|
||||
/// @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 Between(const Class& v1, const Class& v2) { return v2-v1;}
|
||||
static Class Inverse(const Class& m) { return -m;}
|
||||
|
|
@ -54,7 +50,7 @@ struct VectorSpaceImpl {
|
|||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||
if (H1) *H1 = 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
|
||||
/// @{
|
||||
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 Between(const Class& v1, const Class& v2) { return v2-v1;}
|
||||
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;
|
||||
|
||||
/// @name Group
|
||||
/// @{
|
||||
typedef additive_group_tag group_flavor;
|
||||
static Class Identity() { return Class::identity();}
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
enum { dimension = Class::dimension};
|
||||
|
|
@ -192,96 +192,62 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
|
|||
/// To use this for your gtsam type, define:
|
||||
/// template<> struct traits<Type> : public ScalarTraits<Type> { };
|
||||
template<typename Scalar>
|
||||
struct ScalarTraits {
|
||||
struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
|
||||
|
||||
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
|
||||
/// @{
|
||||
typedef additive_group_tag group_flavor;
|
||||
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;
|
||||
enum { dimension = 1 };
|
||||
typedef Eigen::Matrix<double, 1, 1> TangentVector;
|
||||
typedef OptionalJacobian<1, 1> ChartJacobian;
|
||||
|
||||
// For 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 fabs(v1 - v2) < tol;
|
||||
}
|
||||
|
||||
static TangentVector Local(Scalar origin, Scalar other) {
|
||||
static TangentVector Local(Scalar origin, Scalar other,
|
||||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||
if (H1) (*H1)[0] = -1.0;
|
||||
if (H2) (*H2)[0] = 1.0;
|
||||
TangentVector result;
|
||||
result(0) = other - origin;
|
||||
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,
|
||||
ChartJacobian Horigin, ChartJacobian Hv = 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) {
|
||||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||
if (H1) (*H1)[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,
|
||||
ChartJacobian H2 = boost::none) {
|
||||
if (H1) (*H1)[0] = -1.0;
|
||||
if (H2) (*H2)[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;
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
|
||||
if (H) (*H)[0] = 1.0;
|
||||
return Local(0, m);
|
||||
}
|
||||
|
||||
static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) {
|
||||
if (Hv)
|
||||
(*Hv)[0] = 1.0;
|
||||
static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
|
||||
if (H) (*H)[0] = 1.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> {
|
||||
};
|
||||
|
||||
// traits for any double Eigen matrix
|
||||
// traits for any fixed double Eigen matrix
|
||||
template<int M, int N, int Options, int MaxRows, int MaxCols>
|
||||
struct traits_x<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
|
||||
// BOOST_STATIC_ASSERT_MSG(
|
||||
// M != Eigen::Dynamic && N != Eigen::Dynamic,
|
||||
// "These traits are only valid on fixed-size types.");
|
||||
struct traits_x<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
|
||||
internal::VectorSpaceImpl<
|
||||
Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
|
||||
|
||||
// Typedefs required by all manifold types.
|
||||
typedef vector_space_tag structure_category;
|
||||
enum {
|
||||
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;
|
||||
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
static Eigen::Matrix<double, dimension, dimension> Eye(
|
||||
const ManifoldType& m) {
|
||||
static Jacobian Eye(const Dynamic& m) {
|
||||
int dim = GetDimension(m);
|
||||
return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
|
||||
}
|
||||
|
||||
// For Testable
|
||||
static void Print(const ManifoldType& m, const std::string& str = "") {
|
||||
gtsam::print(Eigen::MatrixXd(m), str);
|
||||
}
|
||||
static bool Equals(const ManifoldType& v1, const ManifoldType& v2,
|
||||
double tol = 1e-8) {
|
||||
return equal_with_abs_tol(v1, v2, tol);
|
||||
static TangentVector Local(const Dynamic& m, const Dynamic& other, //
|
||||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||
if (H1) *H1 = -Eye(m);
|
||||
if (H2) *H2 = Eye(m);
|
||||
TangentVector v(GetDimension(m));
|
||||
Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
|
||||
return v;
|
||||
}
|
||||
|
||||
static TangentVector Local(const ManifoldType& origin,
|
||||
const ManifoldType& other, ChartJacobian Horigin = boost::none,
|
||||
ChartJacobian Hother = boost::none) {
|
||||
if (Horigin) *Horigin = -Eye(origin);
|
||||
if (Hother) *Hother = Eye(origin);
|
||||
TangentVector result(GetDimension(origin));
|
||||
Eigen::Map<Eigen::Matrix<double, M, N> >(result.data(), origin.rows(),
|
||||
origin.cols()) = other - origin;
|
||||
static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
|
||||
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
|
||||
if (H1) *H1 = Eye(m);
|
||||
if (H2) *H2 = Eye(m);
|
||||
return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
|
||||
}
|
||||
/// @}
|
||||
|
||||
/// @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;
|
||||
}
|
||||
|
||||
static ManifoldType Retract(const ManifoldType& origin,
|
||||
const TangentVector& v, ChartJacobian Horigin = boost::none,
|
||||
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 Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
|
||||
throw std::runtime_error("Expmap not defined for dynamic types");
|
||||
}
|
||||
|
||||
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) {
|
||||
if (H1) *H1 = Eye(v1);
|
||||
if (H2) *H2 = Eye(v1);
|
||||
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) {
|
||||
if (H1) *H1 = -Eye(v1);
|
||||
if (H2) *H2 = Eye(v1);
|
||||
return v2 - v1;
|
||||
}
|
||||
/// @}
|
||||
|
||||
static ManifoldType Inverse(const ManifoldType& m, ChartJacobian H =
|
||||
boost::none) {
|
||||
if (H) *H = -Eye(m);
|
||||
return -m;
|
||||
}
|
||||
};
|
||||
|
||||
static ManifoldType Identity() {
|
||||
//FIXME: this won't work for dynamic matrices, but where to get the size???
|
||||
return ManifoldType::Zero();
|
||||
}
|
||||
} // \ internal
|
||||
|
||||
static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm =
|
||||
boost::none) {
|
||||
if (Hm)
|
||||
*Hm = Eye(m);
|
||||
TangentVector result(GetDimension(m));
|
||||
Eigen::Map<Eigen::Matrix<double, M, N> >(result.data()) = m;
|
||||
return result;
|
||||
}
|
||||
// traits for fully dynamic matrix
|
||||
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> {
|
||||
};
|
||||
|
||||
//FIXME: this also does not work for dynamic matrices
|
||||
static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv =
|
||||
boost::none) {
|
||||
ManifoldType m;
|
||||
m.setZero();
|
||||
if (Hv)
|
||||
*Hv = Eye(m);
|
||||
return m + Eigen::Map<Eigen::Matrix<double, M, N> >(v.data());
|
||||
}
|
||||
// traits for dynamic column 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> {
|
||||
};
|
||||
|
||||
// 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
|
||||
|
|
|
|||
|
|
@ -16,12 +16,14 @@
|
|||
* @author Carlos Nieto
|
||||
**/
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/VectorSpace.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -30,7 +32,7 @@ static double inf = std::numeric_limits<double>::infinity();
|
|||
static const double tol = 1e-9;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, constructor_data )
|
||||
TEST(Matrix, constructor_data )
|
||||
{
|
||||
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 B(2, 2);
|
||||
|
|
@ -74,7 +76,7 @@ namespace {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, special_comma_initializer)
|
||||
TEST(Matrix, special_comma_initializer)
|
||||
{
|
||||
Matrix expected(2,2);
|
||||
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();
|
||||
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 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 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;
|
||||
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 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.,
|
||||
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);
|
||||
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);
|
||||
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.,
|
||||
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);
|
||||
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,
|
||||
1.0).finished();
|
||||
|
|
@ -302,7 +304,7 @@ TEST( matrix, insert_sub )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, diagMatrices )
|
||||
TEST(Matrix, diagMatrices )
|
||||
{
|
||||
std::vector<Matrix> Hs;
|
||||
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) <<
|
||||
1.1, 2.3, 4.2, 7.6,
|
||||
-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);
|
||||
A(0, 0) = 1.;
|
||||
|
|
@ -384,7 +386,7 @@ TEST( matrix, scale_columns )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, scale_rows )
|
||||
TEST(Matrix, scale_rows )
|
||||
{
|
||||
Matrix A(3, 4);
|
||||
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);
|
||||
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;
|
||||
Matrix3 actual = skewSymmetric(wx,wy,wz);
|
||||
|
|
@ -476,7 +478,7 @@ TEST( matrix, skewSymmetric )
|
|||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, equal )
|
||||
TEST(Matrix, equal )
|
||||
{
|
||||
Matrix A(4, 4);
|
||||
A(0, 0) = -1;
|
||||
|
|
@ -506,7 +508,7 @@ TEST( matrix, equal )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, equal_nan )
|
||||
TEST(Matrix, equal_nan )
|
||||
{
|
||||
Matrix A(4, 4);
|
||||
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 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 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 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 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);
|
||||
A(0, 0) = -1;
|
||||
|
|
@ -593,7 +595,7 @@ TEST( matrix, multiplication )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, scalar_matrix_multiplication )
|
||||
TEST(Matrix, scalar_matrix_multiplication )
|
||||
{
|
||||
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);
|
||||
|
||||
|
|
@ -627,7 +629,7 @@ TEST( matrix, matrix_vector_multiplication )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, nrRowsAndnrCols )
|
||||
TEST(Matrix, nrRowsAndnrCols )
|
||||
{
|
||||
Matrix A(3, 6);
|
||||
LONGS_EQUAL( A.rows() , 3 );
|
||||
|
|
@ -635,7 +637,7 @@ TEST( matrix, nrRowsAndnrCols )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, scalar_divide )
|
||||
TEST(Matrix, scalar_divide )
|
||||
{
|
||||
Matrix A(2, 2);
|
||||
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) <<
|
||||
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);
|
||||
A(0, 0) = 1;
|
||||
|
|
@ -754,7 +756,7 @@ TEST( matrix, inverse )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, inverse2 )
|
||||
TEST(Matrix, inverse2 )
|
||||
{
|
||||
Matrix A(3, 3);
|
||||
A(0, 0) = 0;
|
||||
|
|
@ -784,7 +786,7 @@ TEST( matrix, inverse2 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, backsubtitution )
|
||||
TEST(Matrix, backsubtitution )
|
||||
{
|
||||
// TEST ONE 2x2 matrix U1*x=b1
|
||||
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
|
||||
|
||||
|
|
@ -838,7 +840,7 @@ TEST( matrix, householder )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, householder_colMajor )
|
||||
TEST(Matrix, householder_colMajor )
|
||||
{
|
||||
// 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
|
||||
|
||||
|
|
@ -898,7 +900,7 @@ TEST( matrix, eigen_QR )
|
|||
// unit test for qr factorization (and hence householder)
|
||||
// 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,
|
||||
|
|
@ -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,
|
||||
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 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();
|
||||
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
|
||||
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,
|
||||
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.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished();
|
||||
}
|
||||
TEST( matrix, LLt )
|
||||
TEST(Matrix, LLt )
|
||||
{
|
||||
EQUALITY(cholesky::expected, LLt(cholesky::M));
|
||||
}
|
||||
TEST( matrix, RtR )
|
||||
TEST(Matrix, RtR )
|
||||
{
|
||||
EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
|
||||
}
|
||||
|
||||
TEST( matrix, cholesky_inverse )
|
||||
TEST(Matrix, cholesky_inverse )
|
||||
{
|
||||
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();
|
||||
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();
|
||||
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 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 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 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.);
|
||||
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);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd2 )
|
||||
TEST(Matrix, svd2 )
|
||||
{
|
||||
Matrix U, V;
|
||||
Vector s;
|
||||
|
|
@ -1139,7 +1141,7 @@ TEST( matrix, svd2 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd3 )
|
||||
TEST(Matrix, svd3 )
|
||||
{
|
||||
Matrix U, V;
|
||||
Vector s;
|
||||
|
|
@ -1167,7 +1169,7 @@ TEST( matrix, svd3 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, svd4 )
|
||||
TEST(Matrix, svd4 )
|
||||
{
|
||||
Matrix U, V;
|
||||
Vector s;
|
||||
|
|
@ -1209,7 +1211,7 @@ TEST( matrix, svd4 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, DLT )
|
||||
TEST(Matrix, DLT )
|
||||
{
|
||||
Matrix A = (Matrix(8, 9) <<
|
||||
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));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
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() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -15,10 +15,12 @@
|
|||
* @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 <boost/tuple/tuple.hpp>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -40,7 +42,7 @@ namespace {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, special_comma_initializer)
|
||||
TEST(Vector, special_comma_initializer)
|
||||
{
|
||||
Vector expected(3);
|
||||
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;
|
||||
double data[] = {10,20};
|
||||
|
|
@ -78,14 +80,14 @@ TEST( TestVector, copy )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, zero1 )
|
||||
TEST(Vector, zero1 )
|
||||
{
|
||||
Vector v = Vector::Zero(2);
|
||||
EXPECT(zero(v));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, zero2 )
|
||||
TEST(Vector, zero2 )
|
||||
{
|
||||
Vector a = 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 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 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 b(2); b(0) = -10; b(1) = -20;
|
||||
|
|
@ -118,7 +120,7 @@ TEST( TestVector, negate )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, sub )
|
||||
TEST(Vector, sub )
|
||||
{
|
||||
Vector a(6);
|
||||
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),
|
||||
small = ones(3);
|
||||
|
|
@ -148,7 +150,7 @@ TEST( TestVector, subInsert )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, householder )
|
||||
TEST(Vector, householder )
|
||||
{
|
||||
Vector x(4);
|
||||
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);
|
||||
for(int i = 0; i < 2; i++)
|
||||
|
|
@ -187,7 +189,7 @@ TEST( TestVector, concatVectors)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, weightedPseudoinverse )
|
||||
TEST(Vector, weightedPseudoinverse )
|
||||
{
|
||||
// column from a matrix
|
||||
Vector x(2);
|
||||
|
|
@ -213,7 +215,7 @@ TEST( TestVector, weightedPseudoinverse )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( TestVector, weightedPseudoinverse_constraint )
|
||||
TEST(Vector, weightedPseudoinverse_constraint )
|
||||
{
|
||||
// column from a matrix
|
||||
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 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 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 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 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 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),
|
||||
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);
|
||||
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 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 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 v2 = Vector3(0.1, -4.1, 0.0);
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -22,8 +22,8 @@
|
|||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/concepts.h>
|
||||
#include <gtsam/base/VectorSpace.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
#undef CHECK
|
||||
|
|
@ -39,34 +39,49 @@ using namespace gtsam;
|
|||
// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// is_manifold
|
||||
TEST(Manifold, IsManifold) {
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Manifold, SomeManifoldsGTSAM) {
|
||||
//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>));
|
||||
|
||||
// dynamic not working yet
|
||||
//BOOST_CONCEPT_ASSERT((IsManifold<Vector>));
|
||||
//BOOST_CONCEPT_ASSERT((IsManifold<Matrix>));
|
||||
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Cal3_S2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Cal3Bundler>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
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
|
||||
TEST(Manifold, _dimension) {
|
||||
//using namespace traits;
|
||||
EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension);
|
||||
EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::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
|
||||
TEST(Manifold, DefaultChart) {
|
||||
|
||||
|
|
@ -134,35 +149,10 @@ TEST(Manifold, DefaultChart) {
|
|||
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() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue