diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 6f0cf1406..3cc6a041c 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -11,9 +11,10 @@ /* * @file ChartValue.h - * @date Jan 26, 2012 + * @brief + * @date October, 2014 * @author Michael Bosse, Abel Gawel, Renaud Dube - * based on DrivedValue.h by Duy Nguyen Ta + * based on DerivedValue.h by Duy Nguyen Ta */ #pragma once @@ -39,25 +40,44 @@ #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_ { +/** + * 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: + +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) {} +public: - virtual ~ChartValue() {} + /// 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. @@ -66,7 +86,7 @@ class ChartValue : public GenericValue, public Chart_ { */ virtual Value* clone_() const { void *place = boost::singleton_pool::malloc(); - ChartValue* ptr = new(place) ChartValue(*this); // calls copy constructor to fill in + ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in return ptr; } @@ -75,7 +95,7 @@ class ChartValue : public GenericValue, public Chart_ { */ virtual void deallocate_() const { this->~ChartValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*)this); // Release memory from pool + boost::singleton_pool::free((void*) this); // Release memory from pool } /** @@ -85,18 +105,16 @@ class ChartValue : public GenericValue, public Chart_ { 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)); + 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; @@ -105,7 +123,8 @@ class ChartValue : public GenericValue, public Chart_ { /// 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); + const GenericValue& genericValue2 = + static_cast&>(value2); // Return the result of calling localCoordinates trait on the derived class return Chart::local(GenericValue::value(), genericValue2.value()); @@ -113,7 +132,8 @@ class ChartValue : public GenericValue, public Chart_ { /// Non-virtual version of retract ChartValue retract(const Vector& delta) const { - return ChartValue(Chart::retract(GenericValue::value(), delta),static_cast(*this)); + return ChartValue(Chart::retract(GenericValue::value(), delta), + static_cast(*this)); } /// Non-virtual version of localCoordinates @@ -121,8 +141,10 @@ class ChartValue : public GenericValue, public Chart_ { return localCoordinates_(value2); } + /// Return run-time dimensionality virtual size_t dim() const { - return Chart::getDimension(GenericValue::value()); // need functional form here since the dimension may be dynamic + // need functional form here since the dimension may be dynamic + return Chart::getDimension(GenericValue::value()); } /// Assignment operator @@ -136,6 +158,7 @@ class ChartValue : public GenericValue, public Chart_ { } 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. @@ -145,44 +168,52 @@ protected: // } private: + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { }; + struct PoolTag { + }; private: - /// @} - /// @name Advanced Interface - /// @{ - - /** 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< GenericValue >(*this)); - } - - /// @} + /** 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 { -template -struct dimension > : public dimension {}; -} +/// 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 { -// define Value::cast here since now ChartValue has been declared - return dynamic_cast(*this); - } + 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::value, traits::dimension::value >& > H=boost::none) { +template +ChartValue convertToChartValue(const T& value, + boost::optional< + Eigen::Matrix::value, + traits::dimension::value>&> H = boost::none) { if (H) { - *H = Eigen::Matrix::value, traits::dimension::value >::Identity(); + *H = Eigen::Matrix::value, + traits::dimension::value>::Identity(); } return ChartValue(value); } diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 0869769c4..7a48d85c3 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -11,9 +11,10 @@ /* * @file GenericValue.h - * @date Jan 26, 2012 + * @brief Wraps any type T so it can play as a Value + * @date October, 2014 * @author Michael Bosse, Abel Gawel, Renaud Dube - * based on DrivedValue.h by Duy Nguyen Ta + * based on DerivedValue.h by Duy Nguyen Ta */ #pragma once @@ -22,14 +23,16 @@ namespace gtsam { +// To play as a GenericValue, we need the following traits namespace traits { // trait to wrap the default equals of types template - bool equals(const T& a, const T& b, double tol) { - return a.equals(b,tol); - } +bool equals(const T& a, const T& b, double tol) { + return a.equals(b, tol); +} +// trait to wrap the default print of types template void print(const T& obj, const std::string& str) { obj.print(str); @@ -37,20 +40,40 @@ void print(const T& obj, const std::string& str) { } +/** + * Wraps any type T so it can play as a Value + */ template -class GenericValue : public Value { +class GenericValue: public Value { + public: + typedef T type; + protected: - T value_; + + T value_; ///< The wrapped value public: - GenericValue(const T& value) : value_(value) {} - const T& value() const { return value_; } - T& value() { return value_; } + /// Construct from value + GenericValue(const T& value) : + value_(value) { + } - virtual ~GenericValue() {} + /// Return a constant value + const T& value() const { + return value_; + } + + /// Return the value + T& value() { + return value_; + } + + /// Destructor + virtual ~GenericValue() { + } /// equals implementing generic Value interface virtual bool equals_(const Value& p, double tol = 1e-9) const { @@ -60,30 +83,35 @@ public: return traits::equals(this->value_, genericValue2.value_, tol); } - // non virtual equals function + /// non virtual equals function, uses traits bool equals(const GenericValue &other, double tol = 1e-9) const { - return traits::equals(this->value(),other.value(),tol); + return traits::equals(this->value(), other.value(), tol); } + /// Virtual print function, uses traits virtual void print(const std::string& str) const { - traits::print(value_,str); + traits::print(value_, str); } + + // Serialization below: + 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_); } + protected: - /// Assignment operator for this class not needed since GenricValue is an abstract class + + // Assignment operator for this class not needed since GenericValue is an abstract class }; // define Value::cast here since now GenericValue has been declared template - const ValueType& Value::cast() const { - return dynamic_cast&>(*this).value(); - } - +const ValueType& Value::cast() const { + return dynamic_cast&>(*this).value(); +} } /* namespace gtsam */ diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 4fe6c9d99..901960001 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -67,9 +67,12 @@ struct is_manifold: public boost::false_type { }; // dimension, can return Eigen::Dynamic (-1) if not known at compile time +// defaults to dynamic, TODO makes sense ? typedef boost::integral_constant Dynamic; template -struct dimension : public Dynamic {}; //default to dynamic +struct dimension: public Dynamic { +}; + /** * zero::value is intended to be the origin of a canonical coordinate system @@ -140,7 +143,8 @@ struct zero > : public boost::integral_cons } }; -template struct is_chart : public boost::false_type {}; +template struct is_chart: public boost::false_type { +}; } // \ namespace traits @@ -164,13 +168,15 @@ struct DefaultChart { namespace traits { // populate default traits -template struct is_chart > : public boost::true_type {}; -template struct dimension > : public dimension {}; +template struct is_chart > : public boost::true_type { +}; +template struct dimension > : public dimension { +}; } template struct ChartConcept { - public: +public: typedef typename C::type type; typedef typename C::vector vector; @@ -192,20 +198,19 @@ struct ChartConcept { dim_ = C::getDimension(val_); } - private: +private: type val_; vector vec_; int dim_; }; - /** * CanonicalChart > is a chart around zero::value * Canonical is CanonicalChart > * An example is Canonical */ template struct CanonicalChart { - BOOST_CONCEPT_ASSERT((ChartConcept)); + BOOST_CONCEPT_ASSERT((ChartConcept)); typedef C Chart; typedef typename Chart::type type; @@ -220,7 +225,8 @@ template struct CanonicalChart { return Chart::retract(traits::zero::value(), v); } }; -template struct Canonical : public CanonicalChart > {}; +template struct Canonical: public CanonicalChart > { +}; // double @@ -248,8 +254,7 @@ template struct DefaultChart > { typedef Eigen::Matrix type; typedef type T; - typedef Eigen::Matrix::value, 1> vector; - BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + 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"); static vector local(const T& origin, const T& other) { T diff = other - origin; @@ -262,7 +267,7 @@ struct DefaultChart > { return origin + map; } static int getDimension(const T&origin) { - return origin.rows()*origin.cols(); + return origin.rows() * origin.cols(); } }; diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 99d40b060..edb205a8b 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -124,6 +124,7 @@ namespace gtsam { //needs a empty definition so recursion in implicit derived assignment operators work return *this; } + /** Cast to known ValueType */ template const ValueType& cast() const; diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 49ea03e9f..0d559cfe6 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -300,6 +300,7 @@ namespace gtsam { 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) { @@ -311,11 +312,13 @@ namespace gtsam { 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, ChartInit chart) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e31bfa941..e4a27849d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -251,15 +251,18 @@ namespace gtsam { /** 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, + /** Templated version 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); + + /// overloaded insert version that also specifies a chart template void insert(Key j, const ValueType& val); - // overloaded insert version that also specifies a chart initializer + + /// overloaded insert version that also specifies a chart initializer template void insert(Key j, const ValueType& val, ChartInit chart); @@ -273,14 +276,18 @@ namespace gtsam { /** single element change of existing element */ void update(Key j, const Value& val); - /** Templated verion to update a variable with the given j, + /** Templated version 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); + + /// overloaded insert version that also specifies a chart template void update(Key j, const T& val); + + /// overloaded insert version that also specifies a chart initializer template void update(Key j, const T& val, ChartInit chart);