diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index a63b51f29..f6e472d23 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -1,222 +1,222 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @brief - * @date October, 2014 - * @author Michael Bosse, Abel Gawel, Renaud Dube - * based on DerivedValue.h by Duy Nguyen Ta - */ - +///* ---------------------------------------------------------------------------- +// +// * 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 +// * @brief +// * @date October, 2014 +// * @author Michael Bosse, Abel Gawel, Renaud Dube +// * based on DerivedValue.h by Duy Nguyen Ta +// */ +// #pragma once - +// #include #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: - - /// Default Constructor. TODO might not make sense for some types - ChartValue() : - GenericValue(T()) { - } - - /// Construct froma value - ChartValue(const T& value) : - GenericValue(value) { - } - - /// Construct from a value and initialize the chart - template - ChartValue(const T& value, C chart_initializer) : - GenericValue(value), Chart(chart_initializer) { - } - - /// Destructor - 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); - } - - /// 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()); - } - - /// Non-virtual version of retract - ChartValue retract(const Vector& delta) const { - return ChartValue(Chart::retract(GenericValue::value(), delta), - static_cast(*this)); - } - - /// Non-virtual version of localCoordinates - Vector localCoordinates(const ChartValue& value2) const { - return localCoordinates_(value2); - } - - /// Return run-time dimensionality - virtual size_t dim() const { - // need functional form here since the dimension may be dynamic - return Chart::getDimension(GenericValue::value()); - } - - /// 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 { - }; - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - // ar & boost::serialization::make_nvp("value",); - // todo: implement a serialization for charts - ar - & boost::serialization::make_nvp("GenericValue", - boost::serialization::base_object >(*this)); - } - -}; - -// Define -namespace traits { - -/// The dimension of a ChartValue is the dimension of the chart -template -struct dimension > : public dimension { - // TODO Frank thinks dimension is a property of type, chart should conform -}; - -} // \ traits - -/// Get the chart from a Value -template -const Chart& Value::getChart() const { - return dynamic_cast(*this); -} - -/// Convenience function that can be used to make an expression to convert a value to a chart -template -ChartValue convertToChartValue(const T& value, - boost::optional< - Eigen::Matrix::dimension, - traits_x::dimension>&> H = boost::none) { - if (H) { - *H = Eigen::Matrix::dimension, - traits_x::dimension>::Identity(); - } - return ChartValue(value); -} - -} /* namespace gtsam */ +//#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 GenericValue: public GenericValue, public Chart_ { +// +// BOOST_CONCEPT_ASSERT((ChartConcept)); +// +//public: +// +// typedef T type; +// typedef Chart_ Chart; +// +//public: +// +// /// Default Constructor. TODO might not make sense for some types +// GenericValue() : +// GenericValue(T()) { +// } +// +// /// Construct froma value +// GenericValue(const T& value) : +// GenericValue(value) { +// } +// +// /// Construct from a value and initialize the chart +// template +// GenericValue(const T& value, C chart_initializer) : +// GenericValue(value), Chart(chart_initializer) { +// } +// +// /// Destructor +// 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) GenericValue(*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); +// } +// +// /// 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) GenericValue(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()); +// } +// +// /// Non-virtual version of retract +// GenericValue retract(const Vector& delta) const { +// return GenericValue(Chart::retract(GenericValue::value(), delta), +// static_cast(*this)); +// } +// +// /// Non-virtual version of localCoordinates +// Vector localCoordinates(const GenericValue& value2) const { +// return localCoordinates_(value2); +// } +// +// /// Return run-time dimensionality +// virtual size_t dim() const { +// // need functional form here since the dimension may be dynamic +// return Chart::getDimension(GenericValue::value()); +// } +// +// /// Assignment operator +// virtual Value& operator=(const Value& rhs) { +// // Cast the base class Value pointer to a derived class pointer +// const GenericValue& derivedRhs = static_cast(rhs); +// +// // Do the assignment and return the result +// *this = GenericValue(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 { +// }; +// +//private: +// +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE & ar, const unsigned int version) { +// // ar & boost::serialization::make_nvp("value",); +// // todo: implement a serialization for charts +// ar +// & boost::serialization::make_nvp("GenericValue", +// boost::serialization::base_object >(*this)); +// } +// +//}; +// +//// Define +//namespace traits { +// +///// The dimension of a GenericValue is the dimension of the chart +//template +//struct dimension > : public dimension { +// // TODO Frank thinks dimension is a property of type, chart should conform +//}; +// +//} // \ traits +// +///// Get the chart from a Value +//template +//const Chart& Value::getChart() const { +// return dynamic_cast(*this); +//} +// +///// Convenience function that can be used to make an expression to convert a value to a chart +//template +//GenericValue convertToChartValue(const T& value, +// boost::optional< +// Eigen::Matrix::dimension, +// traits_x::dimension>&> H = boost::none) { +// if (H) { +// *H = Eigen::Matrix::dimension, +// traits_x::dimension>::Identity(); +// } +// return GenericValue(value); +//} +// +//} /* namespace gtsam */ diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 6c109675d..5b0009a7f 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -23,6 +23,7 @@ #include #include #include +#include namespace gtsam { @@ -144,18 +145,108 @@ public: traits::print()(value_, str); } - // Serialization below: + /** + * 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(); + GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in + return ptr; + } - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); - ar & BOOST_SERIALIZATION_NVP(value_); - } + /** + * 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 + } -protected: + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } - // Assignment operator for this class not needed since GenericValue is an abstract class + /// 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_x::Retract(GenericValue::value(), delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = + boost::singleton_pool::malloc(); + Value* resultAsValue = new (resultAsValuePlace) GenericValue(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 GenericValue& genericValue2 = + static_cast&>(value2); + + // Return the result of calling localCoordinates trait on the derived class + return traits_x::Local(GenericValue::value(), genericValue2.value()); + } + + /// Non-virtual version of retract + GenericValue retract(const Vector& delta) const { + return GenericValue(traits_x::Retract(GenericValue::value(), delta)); + } + + /// Non-virtual version of localCoordinates + Vector localCoordinates(const GenericValue& value2) const { + return localCoordinates_(value2); + } + + /// Return run-time dimensionality + virtual size_t dim() const { + // need functional form here since the dimension may be dynamic + return traits_x::GetDimension(GenericValue::value()); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const GenericValue& derivedRhs = static_cast(rhs); + + // Do the assignment and return the result + *this = GenericValue(derivedRhs); // calls copy constructor + return *this; + } + + protected: + + // implicit assignment operator for (const GenericValue& 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 { + }; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("GenericValue", + boost::serialization::base_object >(*this)); + ar & boost::serialization::make_nvp("value", value_); + } }; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 22315bf15..038111260 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -357,6 +357,8 @@ // //} // \ namespace gtsam // + +// TODO(ASL) Remove these and fix the compiler errors. ///** // * Macros for using the ManifoldConcept // * - An instantiation for use inside unit tests diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 4667e87fc..324ed111f 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -63,10 +63,10 @@ struct Manifold { typedef OptionalJacobian ChartJacobian; // For Testable - void Print(const ManifoldType& m) { - m.print(); + static void Print(const ManifoldType& m, const std::string& str = "") { + m.print(str); } - bool Equals(const ManifoldType& m1, + static bool Equals(const ManifoldType& m1, const ManifoldType& m2, double tol = 1e-8) { return m1.equals(m2, tol); @@ -113,10 +113,10 @@ struct LieGroup { typedef OptionalJacobian ChartJacobian; // For Testable - void Print(const ManifoldType& m) { + static void Print(const ManifoldType& m, const std::string& str = "") { m.print(); } - bool Equals(const ManifoldType& m1, + static bool Equals(const ManifoldType& m1, const ManifoldType& m2, double tol = 1e-8) { return m1.equals(m2, tol); diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 38e3b538d..cb3269e8b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -486,7 +486,8 @@ private: }; -template<> -struct traits_x : public internal::Manifold {}; + +template +struct traits_x< PinholeCamera > : public internal::Manifold > {}; } // \ gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 140db7b41..7fc9406e8 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -53,7 +53,7 @@ private: public: - enum { dimension = 3 }; + enum { dimension = 6 }; /// @name Standard Constructors /// @{ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index e983ccb17..2128b6bb7 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -292,37 +292,13 @@ namespace gtsam { // insert a plain value using the default chart template void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(ChartValue >(val))); + insert(j, static_cast(GenericValue(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, Chart 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(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, Chart chart) { - update(j, static_cast(ChartValue(val, chart))); + update(j, static_cast(GenericValue(val))); } } diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index a8f287d1e..3f39730ff 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -76,7 +76,7 @@ TEST (Serialization, TemplatedValues) { std::cout << __LINE__ << std::endl; EXPECT(equalsObj(pt3)); std::cout << __LINE__ << std::endl; - ChartValue chv1(pt3); + GenericValue chv1(pt3); std::cout << __LINE__ << std::endl; EXPECT(equalsObj(chv1)); std::cout << __LINE__ << std::endl; diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 9c5df7ea0..163d20013 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -43,8 +43,9 @@ namespace gtsam { VALUE measured_; /** The measurement */ /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) + // TODO(ASL) Reenable + //GTSAM_CONCEPT_LIE_TYPE(T) + //GTSAM_CONCEPT_TESTABLE_TYPE(T) public: @@ -74,14 +75,14 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - traits::print()(measured_, " measured: "); + traits_x::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::equals()(this->measured_, e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits_x::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -90,10 +91,13 @@ namespace gtsam { Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { - T hx = p1.between(p2, H1, H2); // h(x) - DefaultChart chart; + T hx = traits_x::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return chart.local(measured_, hx); + OptionalJacobian::dimension, traits_x::dimension> Hlocal; + Vector rval = traits_x::Local(measured_, hx, boost::none, Hlocal); + (*H1) = ((*Hlocal) * (*H1)).eval(); + (*H2) = ((*Hlocal) * (*H2)).eval(); + return rval; } /** return the measured */ @@ -131,7 +135,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(DefaultChart::getDimension(measured), fabs(mu))) + noiseModel::Constrained::All(traits_x::GetDimension(measured), fabs(mu))) {} private: diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8fbbd4d88..9b7aa5513 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,24 +67,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - traits::print()(prior_, " prior mean: "); + traits_x::Print(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits_x::Equals(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - DefaultChart chart; - if (H) (*H) = eye(chart.getDimension(p)); + if (H) (*H) = eye(traits_x::GetDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return chart.local(prior_,p); + // TODO(ASL) Add Jacobians. + return traits_x::Local(prior_,p); } const VALUE & prior() const { return prior_; }