GTSAM_CONCEPT_ASSERT to replace BOOST equivalent.
parent
a38d76bcef
commit
eb37866e92
|
@ -166,7 +166,7 @@ Concept Checks
|
|||
|
||||
Boost provides a nice way to check whether a given type satisfies a concept. For example, the following
|
||||
|
||||
BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>)
|
||||
|
||||
asserts that Point2 indeed is a model for the VectorSpace concept.
|
||||
|
||||
|
|
|
@ -51,7 +51,7 @@ template<typename VALUE>
|
|||
class FastSet: public std::set<VALUE, std::less<VALUE>,
|
||||
typename internal::FastDefaultAllocator<VALUE>::type> {
|
||||
|
||||
BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
|
||||
GTSAM_CONCEPT_ASSERT(IsTestable<VALUE>);
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
@ -52,8 +52,8 @@ public:
|
|||
typedef typename traits<G>::group_flavor flavor_tag;
|
||||
//typedef typename traits<G>::identity::value_type identity_value_type;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsGroup) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
GTSAM_CONCEPT_USAGE(IsGroup) {
|
||||
static_assert(
|
||||
(std::is_base_of<group_tag, structure_category_tag>::value),
|
||||
"This type's structure_category trait does not assert it as a group (or derived)");
|
||||
e = traits<G>::Identity();
|
||||
|
@ -82,7 +82,7 @@ private:
|
|||
|
||||
/// Check invariants
|
||||
template<typename G>
|
||||
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
|
||||
GTSAM_CONCEPT_REQUIRES(IsGroup<G>,bool) //
|
||||
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
|
||||
G e = traits<G>::Identity();
|
||||
return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol)
|
||||
|
@ -128,7 +128,7 @@ struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
|
|||
|
||||
/// compose multiple times
|
||||
template<typename G>
|
||||
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
|
||||
GTSAM_CONCEPT_REQUIRES(IsGroup<G>,G) //
|
||||
compose_pow(const G& g, size_t n) {
|
||||
if (n == 0) return traits<G>::Identity();
|
||||
else if (n == 1) return g;
|
||||
|
@ -139,8 +139,8 @@ compose_pow(const G& g, size_t n) {
|
|||
/// Assumes nothing except group structure and Testable from G and H
|
||||
template<typename G, typename H>
|
||||
class DirectProduct: public std::pair<G, H> {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<G>));
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<H>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<G>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsGroup<H>);
|
||||
|
||||
public:
|
||||
/// Default constructor yields identity
|
||||
|
@ -170,8 +170,8 @@ struct traits<DirectProduct<G, H> > :
|
|||
/// Assumes existence of three additive operators for both groups
|
||||
template<typename G, typename H>
|
||||
class DirectSum: public std::pair<G, H> {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<G>); // TODO(frank): check additive
|
||||
GTSAM_CONCEPT_ASSERT2(IsGroup<H>); // TODO(frank): check additive
|
||||
|
||||
const G& g() const { return this->first; }
|
||||
const H& h() const { return this->second;}
|
||||
|
|
|
@ -264,8 +264,8 @@ public:
|
|||
typedef typename traits<T>::TangentVector TangentVector;
|
||||
typedef typename traits<T>::ChartJacobian ChartJacobian;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsLieGroup) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
GTSAM_CONCEPT_USAGE(IsLieGroup) {
|
||||
static_assert(
|
||||
(std::is_base_of<lie_group_tag, structure_category_tag>::value),
|
||||
"This type's trait does not assert it is a Lie group (or derived)");
|
||||
|
||||
|
|
|
@ -22,12 +22,7 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
|
||||
#ifdef GTSAM_USE_BOOST_FEATURES
|
||||
#include <boost/concept_check.hpp>
|
||||
#include <boost/concept/requires.hpp>
|
||||
#include <boost/type_traits/is_base_of.hpp>
|
||||
#endif
|
||||
#include <gtsam/base/concepts.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -66,7 +61,7 @@ struct HasManifoldPrereqs {
|
|||
Eigen::Matrix<double, dim, 1> v;
|
||||
OptionalJacobian<dim, dim> Hp, Hq, Hv;
|
||||
|
||||
BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
|
||||
GTSAM_CONCEPT_USAGE(HasManifoldPrereqs) {
|
||||
v = p.localCoordinates(q);
|
||||
q = p.retract(v);
|
||||
}
|
||||
|
@ -97,7 +92,7 @@ template<class Class>
|
|||
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
|
||||
|
||||
// Check that Class has the necessary machinery
|
||||
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
|
||||
GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
|
||||
|
||||
// Dimension of the manifold
|
||||
enum { dimension = Class::dimension };
|
||||
|
@ -125,7 +120,7 @@ template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {}
|
|||
|
||||
/// Check invariants for Manifold type
|
||||
template<typename T>
|
||||
BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) //
|
||||
GTSAM_CONCEPT_REQUIRES(IsTestable<T>, bool) //
|
||||
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
|
||||
typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
|
||||
typename traits<T>::TangentVector v = traits<T>::Local(a,b);
|
||||
|
@ -144,11 +139,11 @@ public:
|
|||
typedef typename traits<T>::ManifoldType ManifoldType;
|
||||
typedef typename traits<T>::TangentVector TangentVector;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsManifold) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
GTSAM_CONCEPT_USAGE(IsManifold) {
|
||||
static_assert(
|
||||
(std::is_base_of<manifold_tag, structure_category_tag>::value),
|
||||
"This type's structure_category trait does not assert it as a manifold (or derived)");
|
||||
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
|
||||
static_assert(TangentVector::SizeAtCompileTime == dim);
|
||||
|
||||
// make sure Chart methods are defined
|
||||
v = traits<T>::Local(p, q);
|
||||
|
@ -166,7 +161,7 @@ template<typename T>
|
|||
struct FixedDimension {
|
||||
typedef const int value_type;
|
||||
static const int value = traits<T>::dimension;
|
||||
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
|
||||
static_assert(value != Eigen::Dynamic,
|
||||
"FixedDimension instantiated for dymanically-sized type.");
|
||||
};
|
||||
} // \ namespace gtsam
|
||||
|
|
|
@ -27,8 +27,8 @@ namespace gtsam {
|
|||
/// Assumes Lie group structure for G and H
|
||||
template<typename G, typename H>
|
||||
class ProductLieGroup: public std::pair<G, H> {
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsLieGroup<G>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsLieGroup<H>);
|
||||
typedef std::pair<G, H> Base;
|
||||
|
||||
protected:
|
||||
|
|
|
@ -33,9 +33,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#ifdef GTSAM_USE_BOOST_FEATURES
|
||||
#include <boost/concept_check.hpp>
|
||||
#endif
|
||||
#include <gtsam/base/concepts.h>
|
||||
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
|
@ -63,7 +61,7 @@ namespace gtsam {
|
|||
bool r1,r2;
|
||||
public:
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsTestable) {
|
||||
GTSAM_CONCEPT_USAGE(IsTestable) {
|
||||
// check print function, with optional string
|
||||
traits<T>::Print(t, std::string());
|
||||
traits<T>::Print(t);
|
||||
|
@ -136,7 +134,7 @@ namespace gtsam {
|
|||
template<typename T>
|
||||
struct HasTestablePrereqs {
|
||||
|
||||
BOOST_CONCEPT_USAGE(HasTestablePrereqs) {
|
||||
GTSAM_CONCEPT_USAGE(HasTestablePrereqs) {
|
||||
t->print(str);
|
||||
b = t->equals(*s,tol);
|
||||
}
|
||||
|
@ -154,7 +152,7 @@ namespace gtsam {
|
|||
struct Testable {
|
||||
|
||||
// Check that T has the necessary methods
|
||||
BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>));
|
||||
GTSAM_CONCEPT_ASSERT(HasTestablePrereqs<T>);
|
||||
|
||||
static void Print(const T& m, const std::string& str = "") {
|
||||
m.print(str);
|
||||
|
@ -173,7 +171,7 @@ namespace gtsam {
|
|||
*
|
||||
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
|
||||
* the gtsam namespace to be more easily enforced as testable
|
||||
* @deprecated please use BOOST_CONCEPT_ASSERT and
|
||||
* @deprecated please use GTSAM_CONCEPT_ASSERT and
|
||||
*/
|
||||
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
|
||||
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;
|
||||
|
|
|
@ -168,7 +168,7 @@ struct HasVectorSpacePrereqs {
|
|||
Class p, q;
|
||||
Vector v;
|
||||
|
||||
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
|
||||
GTSAM_CONCEPT_USAGE(HasVectorSpacePrereqs) {
|
||||
p = Class::Identity(); // identity
|
||||
q = p + p; // addition
|
||||
q = p - p; // subtraction
|
||||
|
@ -185,7 +185,7 @@ template<class Class>
|
|||
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
|
||||
|
||||
// Check that Class has the necessary machinery
|
||||
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
|
||||
GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
|
||||
|
||||
typedef vector_space_tag structure_category;
|
||||
|
||||
|
@ -472,8 +472,8 @@ public:
|
|||
|
||||
typedef typename traits<T>::structure_category structure_category_tag;
|
||||
|
||||
BOOST_CONCEPT_USAGE(IsVectorSpace) {
|
||||
BOOST_STATIC_ASSERT_MSG(
|
||||
GTSAM_CONCEPT_USAGE(IsVectorSpace) {
|
||||
static_assert(
|
||||
(std::is_base_of<vector_space_tag, structure_category_tag>::value),
|
||||
"This type's trait does not assert it as a vector space (or derived)");
|
||||
r = p + q;
|
||||
|
|
|
@ -39,7 +39,7 @@ void testDefaultChart(TestResult& result_,
|
|||
|
||||
// First, check the basic chart concept. This checks that the interface is satisfied.
|
||||
// The rest of the function is even more detailed, checking the correctness of the chart.
|
||||
BOOST_CONCEPT_ASSERT((ChartConcept<Chart>));
|
||||
GTSAM_CONCEPT_ASSERT(ChartConcept<Chart>);
|
||||
|
||||
T other = value;
|
||||
|
||||
|
|
|
@ -8,28 +8,22 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
// This is a helper to ease the transition to the new traits defined in this file.
|
||||
// Uncomment this if you want all methods that are tagged as not implemented
|
||||
// to cause compilation errors.
|
||||
#ifdef COMPILE_ERROR_NOT_IMPLEMENTED
|
||||
|
||||
#ifdef GTSAM_USE_BOOST_FEATURES
|
||||
#include <boost/static_assert.hpp>
|
||||
#include <boost/concept_check.hpp>
|
||||
#define GTSAM_CONCEPT_USAGE(concept) BOOST_CONCEPT_USAGE((concept))
|
||||
#define GTSAM_CONCEPT_ASSERT(concept) BOOST_CONCEPT_ASSERT((concept))
|
||||
#define GTSAM_CONCEPT_ASSERT1(concept) BOOST_CONCEPT_ASSERT((concept))
|
||||
#define GTSAM_CONCEPT_ASSERT2(concept) BOOST_CONCEPT_ASSERT((concept))
|
||||
#define GTSAM_CONCEPT_ASSERT3(concept) BOOST_CONCEPT_ASSERT((concept))
|
||||
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) BOOST_CONCEPT_REQUIRES((concept), (return_type))
|
||||
#else
|
||||
// These do something sensible:
|
||||
#define GTSAM_CONCEPT_USAGE(concept) void check##concept()
|
||||
#define GTSAM_CONCEPT_ASSERT(concept) concept checkConcept [[maybe_unused]];
|
||||
#define GTSAM_CONCEPT_ASSERT1(concept) concept checkConcept1 [[maybe_unused]];
|
||||
#define GTSAM_CONCEPT_ASSERT2(concept) concept checkConcept2 [[maybe_unused]];
|
||||
#define GTSAM_CONCEPT_ASSERT3(concept) concept checkConcept3 [[maybe_unused]];
|
||||
// This one just ignores concept for now:
|
||||
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type
|
||||
#endif
|
||||
|
||||
#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \
|
||||
"This method is required by the new concepts framework but has not been implemented yet.");
|
||||
|
||||
#else
|
||||
|
||||
#include <exception>
|
||||
#define CONCEPT_NOT_IMPLEMENTED \
|
||||
throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet.");
|
||||
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template <typename T> struct traits;
|
||||
|
||||
}
|
||||
|
|
|
@ -80,7 +80,7 @@ using namespace gtsam;
|
|||
typedef Symmetric<2> S2;
|
||||
TEST(Group, S2) {
|
||||
S2 e, s1 = S2::Transposition(0, 1);
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<S2>));
|
||||
GTSAM_CONCEPT_ASSERT(IsGroup<S2>);
|
||||
EXPECT(check_group_invariants(e, s1));
|
||||
}
|
||||
|
||||
|
@ -88,7 +88,7 @@ TEST(Group, S2) {
|
|||
typedef Symmetric<3> S3;
|
||||
TEST(Group, S3) {
|
||||
S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<S3>));
|
||||
GTSAM_CONCEPT_ASSERT(IsGroup<S3>);
|
||||
EXPECT(check_group_invariants(e, s1));
|
||||
EXPECT(assert_equal(s1, s1 * e));
|
||||
EXPECT(assert_equal(s1, e * s1));
|
||||
|
@ -127,7 +127,7 @@ struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
|
|||
TEST(Group, Dih6) {
|
||||
Dih6 e, g(S2::Transposition(0, 1),
|
||||
S3::Transposition(0, 1) * S3::Transposition(1, 2));
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
|
||||
GTSAM_CONCEPT_ASSERT(IsGroup<Dih6>);
|
||||
EXPECT(check_group_invariants(e, g));
|
||||
EXPECT(assert_equal(e, compose_pow(g, 0)));
|
||||
EXPECT(assert_equal(g, compose_pow(g, 1)));
|
||||
|
|
|
@ -1146,17 +1146,30 @@ TEST(Matrix, DLT )
|
|||
}
|
||||
|
||||
//******************************************************************************
|
||||
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>));
|
||||
TEST(Matrix, Matrix24IsVectorSpace) {
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
|
||||
}
|
||||
|
||||
TEST(Matrix, RowMajorIsVectorSpace) {
|
||||
typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
|
||||
}
|
||||
|
||||
TEST(Matrix, MatrixIsVectorSpace) {
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix>);
|
||||
}
|
||||
|
||||
TEST(Matrix, VectorIsVectorSpace) {
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector>);
|
||||
}
|
||||
|
||||
TEST(Matrix, RowVectorIsVectorSpace) {
|
||||
typedef Eigen::Matrix<double, 1, -1> RowVector;
|
||||
GTSAM_CONCEPT_ASSERT1(IsVectorSpace<RowVector>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsVectorSpace<Vector5>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Matrix, AbsoluteError) {
|
||||
double a = 2000, b = 1997, tol = 1e-1;
|
||||
bool isEqual;
|
||||
|
|
|
@ -266,11 +266,14 @@ TEST(Vector, linear_dependent3 )
|
|||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Vector, IsVectorSpace) {
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
|
||||
TEST(Vector, VectorIsVectorSpace) {
|
||||
GTSAM_CONCEPT_ASSERT1(IsVectorSpace<Vector5>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsVectorSpace<Vector>);
|
||||
}
|
||||
|
||||
TEST(Vector, RowVectorIsVectorSpace) {
|
||||
typedef Eigen::Matrix<double,1,-1> RowVector;
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -34,7 +34,7 @@ BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1);
|
|||
|
||||
//******************************************************************************
|
||||
TEST(BearingRange2D, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange2D>));
|
||||
GTSAM_CONCEPT_ASSERT(IsManifold<BearingRange2D>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -46,7 +46,7 @@ TEST(BearingRange, 2D) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(BearingRange3D, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange3D>));
|
||||
GTSAM_CONCEPT_ASSERT(IsManifold<BearingRange3D>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -27,7 +27,7 @@ typedef Cyclic<2> Z2;
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Cyclic, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Z3>));
|
||||
GTSAM_CONCEPT_ASSERT(IsGroup<Z3>);
|
||||
EXPECT_LONGS_EQUAL(0, traits<Z3>::Identity());
|
||||
}
|
||||
|
||||
|
@ -107,8 +107,8 @@ struct traits<K4> : internal::AdditiveGroupTraits<K4> {
|
|||
TEST(Cyclic , DirectSum) {
|
||||
// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
|
||||
// smallest non-cyclic group called the Klein four-group:
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<K4>));
|
||||
BOOST_CONCEPT_ASSERT((IsTestable<K4>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<K4>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsTestable<K4>);
|
||||
|
||||
// Refer to http://en.wikipedia.org/wiki/Klein_four-group
|
||||
K4 e(0,0), a(0, 1), b(1, 0), c(1, 1);
|
||||
|
|
|
@ -34,9 +34,9 @@ TEST(Point2 , Constructor) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Double , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<double>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<double>));
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<double>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<double>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<double>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsVectorSpace<double>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
@ -48,9 +48,9 @@ TEST(Double , Invariants) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Point2 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Point2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Point2>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Point2>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsVectorSpace<Point2>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -34,9 +34,9 @@ TEST(Point3 , Constructor) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Point3 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Point3>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Point3>));
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point3>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Point3>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Point3>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsVectorSpace<Point3>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -35,9 +35,9 @@ GTSAM_CONCEPT_LIE_INST(Pose2)
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Pose2 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Pose2 >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Pose2 >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Pose2 >));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Pose2 >);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Pose2 >);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<Pose2 >);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -29,9 +29,9 @@ typedef traits<Q>::ChartJacobian QuaternionJacobian;
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Quaternion , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Quaternion >);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Quaternion >);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<Quaternion >);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -38,9 +38,9 @@ static double error = 1e-9, epsilon = 0.001;
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Rot3 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Rot3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Rot3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3 >));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Rot3 >);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Rot3 >);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<Rot3 >);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -36,9 +36,9 @@ TEST(SO3, Identity) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SO3>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<SO3>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<SO3>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<SO3>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -42,9 +42,9 @@ TEST(SO4, Identity) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<SO4>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<SO4>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<SO4>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -84,9 +84,9 @@ TEST(SOn, SO5) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(SOn, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SOn>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SOn>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<SOn>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<SOn>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<SOn>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -35,9 +35,9 @@ static const double s = 4;
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Similarity2, Concepts) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Similarity2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Similarity2>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Similarity2>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<Similarity2>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -54,9 +54,9 @@ const double degree = M_PI / 180;
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Concepts) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Similarity3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity3 >));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Similarity3 >);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Similarity3 >);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<Similarity3 >);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
@ -31,9 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
|
|||
|
||||
//******************************************************************************
|
||||
TEST(StereoPoint2 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<StereoPoint2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<StereoPoint2>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<StereoPoint2>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<StereoPoint2 >);
|
||||
GTSAM_CONCEPT_ASSERT3(IsVectorSpace<StereoPoint2>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -218,7 +218,7 @@ protected:
|
|||
template <typename T>
|
||||
class ScalarMultiplyExpression : public Expression<T> {
|
||||
// Check that T is a vector space
|
||||
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
|
||||
GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
|
||||
|
||||
public:
|
||||
explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
|
||||
|
@ -231,7 +231,7 @@ class ScalarMultiplyExpression : public Expression<T> {
|
|||
template <typename T>
|
||||
class BinarySumExpression : public Expression<T> {
|
||||
// Check that T is a vector space
|
||||
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
|
||||
GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
|
||||
|
||||
public:
|
||||
explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);
|
||||
|
|
|
@ -42,7 +42,7 @@ namespace gtsam {
|
|||
*/
|
||||
template <typename T>
|
||||
class ExpressionFactor : public NoiseModelFactor {
|
||||
BOOST_CONCEPT_ASSERT((IsTestable<T>));
|
||||
GTSAM_CONCEPT_ASSERT(IsTestable<T>);
|
||||
|
||||
protected:
|
||||
|
||||
|
|
|
@ -44,8 +44,8 @@ namespace gtsam {
|
|||
template <class VALUE>
|
||||
class ExtendedKalmanFilter {
|
||||
// Check that VALUE type is a testable Manifold
|
||||
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsTestable<VALUE>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<VALUE>);
|
||||
|
||||
public:
|
||||
typedef std::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
|
||||
|
|
|
@ -38,7 +38,7 @@ T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
|
|||
// right now only word sized types are supported.
|
||||
// Easy to extend if needed,
|
||||
// by somehow inferring the unsigned integer of same size
|
||||
BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
|
||||
static_assert(sizeof(T) == sizeof(size_t));
|
||||
size_t & uiValue = reinterpret_cast<size_t &>(value);
|
||||
size_t misAlignment = uiValue % requiredAlignment;
|
||||
if (misAlignment) {
|
||||
|
@ -559,7 +559,7 @@ public:
|
|||
template <class T>
|
||||
class ScalarMultiplyNode : public ExpressionNode<T> {
|
||||
// Check that T is a vector space
|
||||
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
|
||||
GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
|
||||
|
||||
double scalar_;
|
||||
std::shared_ptr<ExpressionNode<T> > expression_;
|
||||
|
|
|
@ -35,7 +35,7 @@ namespace gtsam {
|
|||
|
||||
template <class T> class BinaryMeasurement : public Factor {
|
||||
// Check that T type is testable
|
||||
BOOST_CONCEPT_ASSERT((IsTestable<T>));
|
||||
GTSAM_CONCEPT_ASSERT(IsTestable<T>);
|
||||
|
||||
public:
|
||||
// shorthand for a smart pointer to a measurement
|
||||
|
|
|
@ -40,8 +40,8 @@ namespace gtsam {
|
|||
class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
|
||||
|
||||
// Check that VALUE type is a testable Lie group
|
||||
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<VALUE>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsTestable<VALUE>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsLieGroup<VALUE>);
|
||||
|
||||
public:
|
||||
|
||||
|
|
|
@ -47,9 +47,9 @@ template<> struct traits<Product> : internal::LieGroupTraits<Product> {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Lie, ProductLieGroup) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Product>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Product>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<Product>));
|
||||
GTSAM_CONCEPT_ASSERT1(IsGroup<Product>);
|
||||
GTSAM_CONCEPT_ASSERT2(IsManifold<Product>);
|
||||
GTSAM_CONCEPT_ASSERT3(IsLieGroup<Product>);
|
||||
Product pair1;
|
||||
Vector5 d;
|
||||
d << 1, 2, 0.1, 0.2, 0.3;
|
||||
|
|
|
@ -37,27 +37,27 @@ typedef PinholeCamera<Cal3Bundler> Camera;
|
|||
|
||||
//******************************************************************************
|
||||
TEST(Manifold, SomeManifoldsGTSAM) {
|
||||
//BOOST_CONCEPT_ASSERT((IsManifold<int>)); // integer is not a manifold
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Cal3_S2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Cal3Bundler>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
|
||||
//GTSAM_CONCEPT_ASSERT(IsManifold<int>); // integer is not a manifold
|
||||
GTSAM_CONCEPT_ASSERT(IsManifold<Camera>);
|
||||
GTSAM_CONCEPT_ASSERT(IsManifold<Cal3_S2>);
|
||||
GTSAM_CONCEPT_ASSERT(IsManifold<Cal3Bundler>);
|
||||
GTSAM_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>));
|
||||
GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot2>);
|
||||
GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose2>);
|
||||
GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot3>);
|
||||
GTSAM_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>));
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<double>);
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<float>);
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>);
|
||||
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
Loading…
Reference in New Issue