From bc094951ed674a8f7884aacedd0288814c8c6cfc Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Tue, 28 Oct 2014 00:57:44 +0100 Subject: [PATCH] all values in Values container are now a ChartValue > ChartValues are GenericValues and a Chart, which defaults to DefaultChart had to make charts functional (ie no storage of the chart origin) so that they could be zero sized base class otherwise there would have been a double of the memory for values (ones for the value, and once for the chart origin, which default to the same) most tests work, execept for serialization based stuff, and const filtering of values. --- gtsam/base/ChartValue.h | 149 +++++++++++++++++++++++ gtsam/base/GenericValue.h | 125 +------------------ gtsam/base/Manifold.h | 136 +++++++++++++-------- gtsam/base/Value.h | 9 +- gtsam/base/numericalDerivative.h | 16 +-- gtsam/geometry/CalibratedCamera.h | 21 +++- gtsam/geometry/tests/testRot3.cpp | 2 +- gtsam/nonlinear/Values-inl.h | 50 ++++++-- gtsam/nonlinear/Values.h | 34 ++++-- gtsam/nonlinear/tests/testValues.cpp | 9 ++ gtsam_unstable/nonlinear/AdaptAutoDiff.h | 4 +- tests/testManifold.cpp | 49 ++++---- 12 files changed, 375 insertions(+), 229 deletions(-) create mode 100644 gtsam/base/ChartValue.h diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h new file mode 100644 index 000000000..52ae1650a --- /dev/null +++ b/gtsam/base/ChartValue.h @@ -0,0 +1,149 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file ChartValue.h + * @date Jan 26, 2012 + * @author Michael Bosse, Abel Gawel, Renaud Dube + * based on DrivedValue.h by Duy Nguyen Ta + */ + +#pragma once + +#include +#include +#include + +////////////////// +// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR +#include + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +#ifdef ERROR +#undef ERROR +#endif +////////////////// + + +namespace gtsam { + +// ChartValue is derived from GenericValue and CHART so that CHART can be zero sized (as in DefaultChart) +// if the CHART is a member variable then it won't ever be zero sized. +template > +class ChartValue : public GenericValue, public CHART { + BOOST_CONCEPT_ASSERT((ChartConcept)); + public: + typedef T type; + typedef CHART Chart; + + public: + ChartValue() : GenericValue(T()) {} + ChartValue(const T& value) : GenericValue(value) {} + template + ChartValue(const T& value, C chart_initializer) : GenericValue(value), CHART(chart_initializer) {} + + virtual ~ChartValue() {} + + /** + * Create a duplicate object returned as a pointer to the generic Value interface. + * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. + * The result must be deleted with Value::deallocate_, not with the 'delete' operator. + */ + virtual Value* clone_() const { + void *place = boost::singleton_pool::malloc(); + ChartValue* ptr = new(place) ChartValue(*this); // calls copy constructor to fill in + return ptr; + } + + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~ChartValue(); // Virtual destructor cleans up the derived object + boost::singleton_pool::free((void*)this); // Release memory from pool + } + + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } + + /// just use the equals_ defined in GenericValue + // virtual bool equals_(const Value& p, double tol = 1e-9) const { + // } + + /// Chart Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class using the retract trait function + const T retractResult = Chart::retract(GenericValue::value(), delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = boost::singleton_pool::malloc(); + Value* resultAsValue = new(resultAsValuePlace) ChartValue(retractResult, static_cast(*this)); + + // Return the pointer to the Value base class + return resultAsValue; + } + + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { + // Cast the base class Value pointer to a templated generic class pointer + const GenericValue& genericValue2 = static_cast&>(value2); + + // Return the result of calling localCoordinates trait on the derived class + return Chart::local(GenericValue::value(), genericValue2.value()); + } + + virtual size_t dim() const { + return Chart::getDimension(GenericValue::value()); // need functional form here since the dimension may be dynamic + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const ChartValue& derivedRhs = static_cast(rhs); + + // Do the assignment and return the result + *this = ChartValue(derivedRhs); // calls copy constructor + return *this; + } + +protected: + // implicit assignment operator for (const ChartValue& rhs) works fine here + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + // DerivedValue& operator=(const DerivedValue& rhs) { + // // Nothing to do, do not call base class assignment operator + // return *this; + // } + +private: + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { }; +}; + +template +const Chart& Value::getChart() const { +// define Value::cast here since now ChartValue has been declared + return dynamic_cast(*this); + } + + +} /* namespace gtsam */ diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 43f606a17..291bdd8f9 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -19,25 +19,6 @@ #pragma once #include -#include - -////////////////// -// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR -#include - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif - -#ifdef ERROR -#undef ERROR -#endif -////////////////// - namespace gtsam { @@ -49,142 +30,44 @@ template return a.equals(b,tol); } -// trait to compute the local coordinates of other with respect to origin -template -Vector localCoordinates(const T& origin, const T& other) { - return origin.localCoordinates(other); -} - -template -T retract(const T& origin, const Vector& delta) { - return origin.retract(delta); -} - template void print(const T& obj, const std::string& str) { obj.print(str); } -template -size_t getDimension(const T& obj) { - return obj.dim(); -} } template class GenericValue : public Value { public: - typedef T ValueType; - typedef GenericValue This; + typedef T type; protected: T value_; public: - GenericValue() {} GenericValue(const T& value) : value_(value) {} - T& value() { return value_; } const T& value() const { return value_; } + T& value() { return value_; } virtual ~GenericValue() {} - /** - * Create a duplicate object returned as a pointer to the generic Value interface. - * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. - */ - virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - This* ptr = new(place) This(*this); - return ptr; - } - - /** - * Destroy and deallocate this object, only if it was originally allocated using clone_(). - */ - virtual void deallocate_() const { - this->~GenericValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*)this); // Release memory from pool - } - - /** - * Clone this value (normal clone on the heap, delete with 'delete' operator) - */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(*this); - } - /// equals implementing generic Value interface virtual bool equals_(const Value& p, double tol = 1e-9) const { // Cast the base class Value pointer to a templated generic class pointer - const This& genericValue2 = static_cast(p); + const GenericValue& genericValue2 = static_cast(p); // Return the result of using the equals traits for the derived class return traits::equals(this->value_, genericValue2.value_, tol); - - } - - /// Generic Value interface version of retract - virtual Value* retract_(const Vector& delta) const { - // Call retract on the derived class using the retract trait function - const T retractResult = traits::retract(value_,delta); - - // Create a Value pointer copy of the result - void* resultAsValuePlace = boost::singleton_pool::malloc(); - Value* resultAsValue = new(resultAsValuePlace) This(retractResult); - - // Return the pointer to the Value base class - return resultAsValue; - } - - /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { - // Cast the base class Value pointer to a templated generic class pointer - const This& genericValue2 = static_cast(value2); - - // Return the result of calling localCoordinates trait on the derived class - return traits::localCoordinates(value_,genericValue2.value_); } virtual void print(const std::string& str) const { traits::print(value_,str); } - virtual size_t dim() const { - return traits::getDimension(value_); // need functional form here since the dimension may be dynamic - } - - /// Assignment operator - virtual Value& operator=(const Value& rhs) { - // Cast the base class Value pointer to a derived class pointer - const This& derivedRhs = static_cast(rhs); - - // Do the assignment and return the result - this->value_ = derivedRhs.value_; - return *this; - } - - /// Conversion to the derived class - operator const T& () const { - return value_; - } - - /// Conversion to the derived class - operator T& () { - return value_; - } - protected: - /// Assignment operator, protected because only the Value or DERIVED - /// assignment operators should be used. -// DerivedValue& operator=(const DerivedValue& rhs) { -// // Nothing to do, do not call base class assignment operator -// return *this; -// } + /// Assignment operator for this class not needed since GenricValue is an abstract class -private: - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { }; }; // define Value::cast here since now GenericValue has been declared diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 63390ec1f..c5b0268aa 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -67,12 +67,13 @@ struct is_manifold: public std::false_type { }; // dimension, can return Eigen::Dynamic (-1) if not known at compile time +typedef std::integral_constant Dynamic; template -struct dimension; +struct dimension : public Dynamic {}; //default to dynamic /** * zero::value is intended to be the origin of a canonical coordinate system - * with canonical(t) == DefaultChart(zero::value).apply(t) + * with canonical(t) == DefaultChart::local(zero::value, t) * Below we provide the group identity as zero *in case* it is a group */ template struct zero: public identity { @@ -112,8 +113,6 @@ struct is_manifold > : public std::true_typ // TODO: Could be more sophisticated using Eigen traits and SFINAE? -typedef std::integral_constant Dynamic; - template struct dimension > : public Dynamic { }; @@ -141,62 +140,102 @@ struct zero > : public std::integral_consta } }; +template struct is_chart : public std::false_type {}; + } // \ namespace traits // Chart is a map from T -> vector, retract is its inverse template struct DefaultChart { - BOOST_STATIC_ASSERT(traits::is_manifold::value); + //BOOST_STATIC_ASSERT(traits::is_manifold::value); + typedef T type; typedef Eigen::Matrix::value, 1> vector; - T t_; - DefaultChart(const T& t) : - t_(t) { + + static vector local(const T& origin, const T& other) { + return origin.localCoordinates(other); } - vector apply(const T& other) { - return t_.localCoordinates(other); + static T retract(const T& origin, const vector& d) { + return origin.retract(d); } - T retract(const vector& d) { - return t_.retract(d); + static int getDimension(const T& origin) { + return origin.dim(); } }; +namespace traits { +template struct is_chart > : public std::true_type {}; +} + +template +struct ChartConcept { + public: + typedef typename C::type type; + typedef typename C::vector vector; + + BOOST_CONCEPT_USAGE(ChartConcept) { + // is_chart trait should be true + BOOST_STATIC_ASSERT((traits::is_chart::value)); + + /** + * Returns Retraction update of val_ + */ + type retract_ret = C::retract(val_,vec_); + + /** + * Returns local coordinates of another object + */ + vec_ = C::local(val_,retract_ret); + + // a way to get the dimension that is compatible with dynamically sized types + dim_ = C::getDimension(val_); + } + + private: + type val_; + vector vec_; + int dim_; +}; + /** - * Canonical::value is a chart around zero::value + * CanonicalChart > is a chart around zero::value + * Canonical is CanonicalChart > * An example is Canonical */ -template struct Canonical { - typedef T type; - typedef typename DefaultChart::vector vector; - DefaultChart chart; - Canonical() : - chart(traits::zero::value()) { - } +template struct CanonicalChart { + BOOST_CONCEPT_ASSERT((ChartConcept)); + + typedef C Chart; + typedef typename Chart::type type; + typedef typename Chart::vector vector; + // Convert t of type T into canonical coordinates - vector apply(const T& t) { - return chart.apply(t); + vector local(const type& t) { + return Chart::local(traits::zero::value(), t); } // Convert back from canonical coordinates to T - T retract(const vector& v) { - return chart.retract(v); + type retract(const vector& v) { + return Chart::retract(traits::zero::value(), v); } }; +template struct Canonical : public CanonicalChart > {}; // double template<> struct DefaultChart { + typedef double type; typedef Eigen::Matrix vector; - double t_; - DefaultChart(double t) : - t_(t) { - } - vector apply(double other) { + + static vector local(double origin, double other) { vector d; - d << other - t_; + d << other - origin; return d; } - double retract(const vector& d) { - return t_ + d[0]; + static double retract(double origin, const vector& d) { + return origin + d[0]; + } + static const int getDimension(double) { + return 1; } }; @@ -204,22 +243,23 @@ struct DefaultChart { template struct DefaultChart > { - typedef Eigen::Matrix T; + typedef Eigen::Matrix type; + typedef type T; typedef Eigen::Matrix::value, 1> vector; BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), "DefaultChart has not been implemented yet for dynamically sized matrices"); - T t_; - DefaultChart(const T& t) : - t_(t) { - } - vector apply(const T& other) { - T diff = other - t_; + static vector local(const T& origin, const T& other) { + T diff = other - origin; Eigen::Map map(diff.data()); return vector(map); + // Why is this function not : return other - origin; ?? what is the Eigen::Map used for? } - T retract(const vector& d) { + static T retract(const T& origin, const vector& d) { Eigen::Map map(d.data()); - return t_ + map; + return origin + map; + } + static int getDimension(const T&origin) { + return origin.rows()*origin.cols(); } }; @@ -227,16 +267,16 @@ struct DefaultChart > { template<> struct DefaultChart { typedef Vector T; + typedef T type; typedef T vector; - T t_; - DefaultChart(const T& t) : - t_(t) { + static vector local(const T& origin, const T& other) { + return other - origin; } - vector apply(const T& other) { - return other - t_; + static T retract(const T& origin, const vector& d) { + return origin + d; } - T retract(const vector& d) { - return t_ + d; + static int getDimension(const T& origin) { + return origin.size(); } }; diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 4ba468a87..99d40b060 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -120,12 +120,17 @@ namespace gtsam { virtual Vector localCoordinates_(const Value& value) const = 0; /** Assignment operator */ - virtual Value& operator=(const Value& rhs) = 0; - + virtual Value& operator=(const Value& rhs) { + //needs a empty definition so recursion in implicit derived assignment operators work + return *this; + } /** Cast to known ValueType */ template const ValueType& cast() const; + template + const Chart& getChart() const; + /** Virutal destructor */ virtual ~Value() {} diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 87c912e57..70edb64e6 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -73,7 +73,7 @@ Vector numericalGradient(boost::function h, const X& x, typedef typename ChartX::vector TangentX; // get chart at x - ChartX chartX(x); + ChartX chartX; // Prepare a tangent vector to perturb x with, only works for fixed size TangentX d; @@ -82,9 +82,9 @@ Vector numericalGradient(boost::function h, const X& x, Vector g = zero(N); // Can be fixed size for (int j = 0; j < N; j++) { d(j) = delta; - double hxplus = h(chartX.retract(d)); + double hxplus = h(chartX.retract(x, d)); d(j) = -delta; - double hxmin = h(chartX.retract(d)); + double hxmin = h(chartX.retract(x, d)); d(j) = 0; g(j) = (hxplus - hxmin) * factor; } @@ -121,14 +121,14 @@ Matrix numericalDerivative11(boost::function h, const X& x, // get value at x, and corresponding chart Y hx = h(x); - ChartY chartY(hx); + ChartY chartY; // Bit of a hack for now to find number of rows - TangentY zeroY = chartY.apply(hx); + TangentY zeroY = chartY.local(hx,hx); size_t m = zeroY.size(); // get chart at x - ChartX chartX(x); + ChartX chartX; // Prepare a tangent vector to perturb x with, only works for fixed size TangentX dx; @@ -139,9 +139,9 @@ Matrix numericalDerivative11(boost::function h, const X& x, double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - TangentY dy1 = chartY.apply(h(chartX.retract(dx))); + TangentY dy1 = chartY.local(hx, h(chartX.retract(x, dx))); dx(j) = -delta; - TangentY dy2 = chartY.apply(h(chartX.retract(dx))); + TangentY dy2 = chartY.local(hx, h(chartX.retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 0e781fbc1..5d4c6420e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -220,5 +220,24 @@ private: } /// @} - };} +}; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public std::true_type { +}; + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + +} diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 9761dfd74..973ec895f 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -142,7 +142,7 @@ TEST( Rot3, retract) // test Canonical coordinates Canonical chart; - Vector v2 = chart.apply(R); + Vector v2 = chart.local(R); CHECK(assert_equal(R, chart.retract(v2))); } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 76d47b429..36bb60144 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -262,11 +262,11 @@ namespace gtsam { throw ValuesKeyDoesNotExist("retrieve", j); // Check the type and throw exception if incorrect - if(typeid(*item->second) != typeid(GenericValue)) + try { + return dynamic_cast&>(*item->second).value(); + } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - - // We have already checked the type, so do a "blind" static_cast, not dynamic_cast - return static_cast&>(*item->second).value(); + } } /* ************************************************************************* */ @@ -276,25 +276,49 @@ namespace gtsam { KeyValueMap::const_iterator item = values_.find(j); if(item != values_.end()) { - // Check the type and throw exception if incorrect - if(typeid(*item->second) != typeid(GenericValue)) + // dynamic cast the type and throw exception if incorrect + try { + return dynamic_cast&>(*item->second).value(); + } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - - // We have already checked the type, so do a "blind" static_cast, not dynamic_cast - return static_cast&>(*item->second).value(); - } else { + } + } else { return boost::none; } } /* ************************************************************************* */ + // insert a plain value using the default chart template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(GenericValue(val))); + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(ChartValue >(val))); + } + + // insert with custom chart type + template + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(ChartValue(val))); + } + // overloaded insert with chart initializer + template + void Values::insert(Key j, const ValueType& val, ChartInit chart) { + insert(j, static_cast(ChartValue(val,chart))); } + // update with default chart template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(GenericValue(val))); + update(j, static_cast(ChartValue >(val))); } + // update with custom chart + template + void Values::update(Key j, const ValueType& val) { + update(j, static_cast(ChartValue(val))); + } + // update with chart initializer, /todo: perhaps there is a way to init chart from old value... + template + void Values::update(Key j, const ValueType& val, ChartInit chart) { + update(j, static_cast(ChartValue(val,chart))); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 5caa735f1..57dec755b 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -44,8 +44,7 @@ #include #include -#include -#include +#include #include #include @@ -249,15 +248,22 @@ namespace gtsam { /** Add a variable with the given j, throws KeyAlreadyExists if j is already present */ void insert(Key j, const Value& val); - /** Templated verion to add a variable with the given j, - * throws KeyAlreadyExists if j is already present - * will wrap the val into a GenericValue to insert*/ - template void insert(Key j, const ValueType& val); - - /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ void insert(const Values& values); + /** Templated verion to add a variable with the given j, + * throws KeyAlreadyExists if j is already present + * if no chart is specified, the DefaultChart is used + */ + template + void insert(Key j, const ValueType& val); + template + void insert(Key j, const ValueType& val); + // overloaded insert version that also specifies a chart initializer + template + void insert(Key j, const ValueType& val, ChartInit chart); + + /** insert that mimics the STL map insert - if the value already exists, the map is not modified * and an iterator to the existing value is returned, along with 'false'. If the value did not * exist, it is inserted and an iterator pointing to the new element, along with 'true', is @@ -266,7 +272,17 @@ namespace gtsam { /** single element change of existing element */ void update(Key j, const Value& val); - template void update(Key j, const T& val); + + /** Templated verion to update a variable with the given j, + * throws KeyAlreadyExists if j is already present + * if no chart is specified, the DefaultChart is used + */ + template + void update(Key j, const T& val); + template + void update(Key j, const T& val); + template + void update(Key j, const T& val, ChartInit chart); /** update the current available values without adding new ones */ void update(const Values& values); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index d77ecaf79..17da8b08e 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -61,6 +61,15 @@ public: Vector localCoordinates(const TestValue&) const { return Vector(); } }; +namespace gtsam { +namespace traits { +template <> +struct is_manifold : public std::true_type {}; +template <> +struct dimension : public std::integral_constant {}; +} +} + /* ************************************************************************* */ TEST( Values, equals1 ) { diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h index fc1f98064..96978d9cf 100644 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ b/gtsam_unstable/nonlinear/AdaptAutoDiff.h @@ -59,8 +59,8 @@ public: using ceres::internal::AutoDiff; // Make arguments - Vector1 v1 = chart1.apply(a1); - Vector2 v2 = chart2.apply(a2); + Vector1 v1 = chart1.local(a1); + Vector2 v2 = chart2.local(a2); bool success; VectorT result; diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index a55e2fdea..b3d45ab19 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -17,6 +17,7 @@ * @brief unit tests for Block Automatic Differentiation */ +#include #include #include #include @@ -65,39 +66,39 @@ TEST(Manifold, _dimension) { // charts TEST(Manifold, DefaultChart) { - DefaultChart chart1(Point2(0, 0)); - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); + DefaultChart chart1; + EXPECT(chart1.local(Point2(0, 0), Point2(1,0)) == Vector2(1, 0)); + EXPECT(chart1.retract(Point2(0, 0), Vector2(1,0)) == Point2(1, 0)); Vector v2(2); v2 << 1, 0; - DefaultChart chart2(Vector2(0, 0)); - EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0)))); - EXPECT(chart2.retract(v2)==Vector2(1,0)); + DefaultChart chart2; + EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); + EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); - DefaultChart chart3(0); + DefaultChart chart3; Vector v1(1); v1 << 1; - EXPECT(assert_equal(v1,chart3.apply(1))); - EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); + EXPECT(assert_equal(v1,chart3.local(0, 1))); + EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! Vector z = zero(2), v(2); v << 1, 0; - DefaultChart chart4(z); - EXPECT(assert_equal(chart4.apply(v),v)); - EXPECT(assert_equal(chart4.retract(v),v)); + DefaultChart chart4; + EXPECT(assert_equal(chart4.local(z, v), v)); + EXPECT(assert_equal(chart4.retract(z, v), v)); Vector v3(3); v3 << 1, 1, 1; Rot3 I = Rot3::identity(); Rot3 R = I.retract(v3); - DefaultChart chart5(I); - EXPECT(assert_equal(v3,chart5.apply(R))); - EXPECT(assert_equal(chart5.retract(v3),R)); + DefaultChart chart5; + EXPECT(assert_equal(v3,chart5.local(I, R))); + EXPECT(assert_equal(chart5.retract(I, v3), R)); // Check zero vector - DefaultChart chart6(R); - EXPECT(assert_equal(zero(3),chart6.apply(R))); + DefaultChart chart6; + EXPECT(assert_equal(zero(3),chart6.local(R, R))); } /* ************************************************************************* */ @@ -114,47 +115,47 @@ TEST(Manifold, _zero) { TEST(Manifold, Canonical) { Canonical chart1; - EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0)); + EXPECT(chart1.local(Point2(1,0))==Vector2(1,0)); EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); Vector v2(2); v2 << 1, 0; Canonical chart2; - EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0)))); + EXPECT(assert_equal(v2,chart2.local(Vector2(1,0)))); EXPECT(chart2.retract(v2)==Vector2(1,0)); Canonical chart3; Eigen::Matrix v1; v1 << 1; - EXPECT(chart3.apply(1)==v1); + EXPECT(chart3.local(1)==v1); EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); Canonical chart4; Point3 point(1, 2, 3); Vector v3(3); v3 << 1, 2, 3; - EXPECT(assert_equal(v3,chart4.apply(point))); + EXPECT(assert_equal(v3,chart4.local(point))); EXPECT(assert_equal(chart4.retract(v3),point)); Canonical chart5; Pose3 pose(Rot3::identity(), point); Vector v6(6); v6 << 0, 0, 0, 1, 2, 3; - EXPECT(assert_equal(v6,chart5.apply(pose))); + EXPECT(assert_equal(v6,chart5.local(pose))); EXPECT(assert_equal(chart5.retract(v6),pose)); Canonical chart6; Cal3Bundler cal0(0, 0, 0); Camera camera(Pose3(), cal0); Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9,chart6.apply(camera))); + EXPECT(assert_equal(z9,chart6.local(camera))); EXPECT(assert_equal(chart6.retract(z9),camera)); Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() Camera camera2(pose, cal); Vector v9(9); v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; - EXPECT(assert_equal(v9,chart6.apply(camera2))); + EXPECT(assert_equal(v9,chart6.local(camera2))); EXPECT(assert_equal(chart6.retract(v9),camera2)); }