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)); }