From 95827dd4d827ef64ba238fc43c3945b78647c06c Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 24 Oct 2014 11:27:02 +0200 Subject: [PATCH 01/18] GenericValue.h copied from DerivedValue.h --- gtsam/base/GenericValue.h | 143 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 gtsam/base/GenericValue.h diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h new file mode 100644 index 000000000..78155d308 --- /dev/null +++ b/gtsam/base/GenericValue.h @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 DerivedValue.h + * @date Jan 26, 2012 + * @author Duy Nguyen Ta + */ + +#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 { + +template +class DerivedValue : public Value { + +protected: + DerivedValue() {} + +public: + + virtual ~DerivedValue() {} + + /** + * 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(); + DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + return ptr; + } + + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~DerivedValue(); // 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(static_cast(*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 derived class pointer + const DERIVED& derivedValue2 = dynamic_cast(p); + + // Return the result of calling equals on the derived class + return (static_cast(this))->equals(derivedValue2, tol); + } + + /// Generic Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class + const DERIVED retractResult = (static_cast(this))->retract(delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = boost::singleton_pool::malloc(); + Value* resultAsValue = new(resultAsValuePlace) DERIVED(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 derived class pointer + const DERIVED& derivedValue2 = dynamic_cast(value2); + + // Return the result of calling localCoordinates on the derived class + return (static_cast(this))->localCoordinates(derivedValue2); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedRhs = dynamic_cast(rhs); + + // Do the assignment and return the result + return (static_cast(this))->operator=(derivedRhs); + } + + /// Conversion to the derived class + operator const DERIVED& () const { + return static_cast(*this); + } + + /// Conversion to the derived class + operator DERIVED& () { + return static_cast(*this); + } + +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; + } + +private: + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { }; + +}; + +} /* namespace gtsam */ From 0681212084d2452e4cb99600f9d5e7dd9ed883ce Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 24 Oct 2014 16:59:37 +0200 Subject: [PATCH 02/18] GenericValue based on defined traits to replace DerivedValue, first implementation --- gtsam/base/GenericValue.h | 113 +++++++++++++++++++++++++---------- gtsam/nonlinear/Values-inl.h | 63 +++++++++++++++---- gtsam/nonlinear/Values.h | 18 +++--- 3 files changed, 143 insertions(+), 51 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 78155d308..f7b5e985a 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -10,9 +10,10 @@ * -------------------------------------------------------------------------- */ /* - * @file DerivedValue.h + * @file GenericValue.h * @date Jan 26, 2012 * @author Duy Nguyen Ta + * @author Mike Bosse, Abel Gawel, Renaud Dube */ #pragma once @@ -40,15 +41,52 @@ namespace gtsam { -template -class DerivedValue : public Value { +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); + } + +// 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; protected: - DerivedValue() {} + T value_; public: + GenericValue() {} + GenericValue(const T& value) : value_(value) {} - virtual ~DerivedValue() {} + T& value() { return value_; } + const T& value() const { return value_; } + + virtual ~GenericValue() {} /** * Create a duplicate object returned as a pointer to the generic Value interface. @@ -56,8 +94,8 @@ public: * The result must be deleted with Value::deallocate_, not with the 'delete' operator. */ virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + void *place = boost::singleton_pool::malloc(); + This* ptr = new(place) This(*this); return ptr; } @@ -65,34 +103,35 @@ public: * Destroy and deallocate this object, only if it was originally allocated using clone_(). */ virtual void deallocate_() const { - this->~DerivedValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*)this); // Release memory from pool + 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(static_cast(*this)); + 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 derived class pointer - const DERIVED& derivedValue2 = dynamic_cast(p); + // Cast the base class Value pointer to a templated generic class pointer + const This& genericValue2 = dynamic_cast(p); + + // Return the result of using the equals traits for the derived class + return traits::equals(this->value_, genericValue2.value_, tol); - // Return the result of calling equals on the derived class - return (static_cast(this))->equals(derivedValue2, tol); } /// Generic Value interface version of retract virtual Value* retract_(const Vector& delta) const { - // Call retract on the derived class - const DERIVED retractResult = (static_cast(this))->retract(delta); + // 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) DERIVED(retractResult); + void* resultAsValuePlace = boost::singleton_pool::malloc(); + Value* resultAsValue = new(resultAsValuePlace) This(retractResult); // Return the pointer to the Value base class return resultAsValue; @@ -100,44 +139,52 @@ public: /// Generic Value interface version of localCoordinates virtual Vector localCoordinates_(const Value& value2) const { - // Cast the base class Value pointer to a derived class pointer - const DERIVED& derivedValue2 = dynamic_cast(value2); + // Cast the base class Value pointer to a templated generic class pointer + const This& genericValue2 = dynamic_cast(value2); - // Return the result of calling localCoordinates on the derived class - return (static_cast(this))->localCoordinates(derivedValue2); + // 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 DERIVED& derivedRhs = dynamic_cast(rhs); + const This& derivedRhs = dynamic_cast(rhs); // Do the assignment and return the result - return (static_cast(this))->operator=(derivedRhs); + this->value_ = derivedRhs.value_; + return *this; } /// Conversion to the derived class - operator const DERIVED& () const { - return static_cast(*this); + operator const T& () const { + return value_; } /// Conversion to the derived class - operator DERIVED& () { - return static_cast(*this); + 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; - } +// 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 { }; - }; } /* namespace gtsam */ diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 7b812551e..11c44cad4 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -52,6 +52,36 @@ namespace gtsam { _ValuesConstKeyValuePair(const _ValuesKeyValuePair& rhs) : key(rhs.key), value(rhs.value) {} }; + /* ************************************************************************* */ + + // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair + // need to use a struct here for later partial specialization + template + struct ValuesCastHelper { + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return CastedKeyValuePairType(key_value.key, const_cast&>(static_cast&>(key_value.value)).value()); + } + }; + // partial specialized version for ValueType == Value + template + struct ValuesCastHelper { + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + // in this case the casted and keyvalue pair are essentially the same type (key,Value&) so perhaps this could be done with just a cast of the key_value? + return CastedKeyValuePairType(key_value.key, key_value.value); + } + }; + // partial specialized version for ValueType == Value + template + struct ValuesCastHelper { + static CastedKeyValuePairType cast(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + // in this case the casted and keyvalue pair are essentially the same type (key,Value&) so perhaps this could be done with just a cast of the key_value? + return CastedKeyValuePairType(key_value.key, key_value.value); + } + }; + /* ************************************************************************* */ template class Values::Filtered { @@ -99,19 +129,19 @@ namespace gtsam { begin_(boost::make_transform_iterator( boost::make_filter_iterator( filter, values.begin(), values.end()), - &castHelper)), + &ValuesCastHelper::cast)), end_(boost::make_transform_iterator( boost::make_filter_iterator( filter, values.end(), values.end()), - &castHelper)), + &ValuesCastHelper::cast)), constBegin_(boost::make_transform_iterator( boost::make_filter_iterator( filter, ((const Values&)values).begin(), ((const Values&)values).end()), - &castHelper)), + &ValuesCastHelper::cast)), constEnd_(boost::make_transform_iterator( boost::make_filter_iterator( filter, ((const Values&)values).end(), ((const Values&)values).end()), - &castHelper)) {} + &ValuesCastHelper::cast)) {} friend class Values; iterator begin_; @@ -175,7 +205,7 @@ namespace gtsam { Values::Values(const Values::Filtered& view) { BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, key_value.value); } } @@ -184,7 +214,7 @@ namespace gtsam { Values::Values(const Values::ConstFiltered& view) { BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, key_value.value); } } @@ -214,6 +244,13 @@ namespace gtsam { return ConstFiltered(boost::bind(&filterHelper, filterFcn, _1), *this); } + /* ************************************************************************* */ + template<> + inline bool Values::filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + // Filter and check the type + return filter(key_value.key); + } + /* ************************************************************************* */ template const ValueType& Values::at(Key j) const { @@ -225,11 +262,11 @@ namespace gtsam { throw ValuesKeyDoesNotExist("retrieve", j); // Check the type and throw exception if incorrect - if(typeid(*item->second) != typeid(ValueType)) + if(typeid(*item->second) != typeid(GenericValue)) 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); + return static_cast&>(*item->second).value(); } /* ************************************************************************* */ @@ -240,14 +277,20 @@ namespace gtsam { if(item != values_.end()) { // Check the type and throw exception if incorrect - if(typeid(*item->second) != typeid(ValueType)) + if(typeid(*item->second) != typeid(GenericValue)) 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); + return static_cast&>(*item->second).value_; } else { return boost::none; } } + /* ************************************************************************* */ + template + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(GenericValue(val))); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 811846f79..5caa735f1 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -45,6 +45,7 @@ #include #include +#include #include #include @@ -248,6 +249,12 @@ 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); @@ -259,6 +266,7 @@ namespace gtsam { /** single element change of existing element */ void update(Key j, const Value& val); + template void update(Key j, const T& val); /** update the current available values without adding new ones */ void update(const Values& values); @@ -369,15 +377,9 @@ namespace gtsam { // supplied \c filter function. template static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + BOOST_STATIC_ASSERT((!std::is_same::value)); // Filter and check the type - return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value)); - } - - // Cast to the derived ValueType - template - static CastedKeyValuePairType castHelper(KeyValuePairType key_value) { - // Static cast because we already checked the type during filtering - return CastedKeyValuePairType(key_value.key, static_cast(key_value.value)); + return filter(key_value.key) && (typeid(GenericValue) == typeid(key_value.value) ); } /** Serialization function */ From 1fadda83e08139bac6f47ac928d26d16221bde7e Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 24 Oct 2014 18:34:06 +0200 Subject: [PATCH 03/18] removed DerivedValue<> inheritence from classes --- gtsam/base/LieMatrix.h | 5 ++--- gtsam/base/LieScalar.h | 2 +- gtsam/base/LieVector.h | 5 ++--- gtsam/geometry/Cal3Bundler.h | 5 ++--- gtsam/geometry/Cal3DS2.h | 5 ++--- gtsam/geometry/Cal3_S2.h | 5 ++--- gtsam/geometry/CalibratedCamera.h | 5 ++--- gtsam/geometry/EssentialMatrix.h | 5 ++--- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Point2.h | 5 ++--- gtsam/geometry/Point3.h | 5 ++--- gtsam/geometry/Pose2.h | 5 ++--- gtsam/geometry/Pose3.h | 5 ++--- gtsam/geometry/Rot2.h | 5 ++--- gtsam/geometry/Rot3.h | 5 ++--- gtsam/geometry/StereoCamera.h | 5 ++--- gtsam/geometry/StereoPoint2.h | 5 ++--- gtsam/geometry/Unit3.h | 5 ++--- gtsam/navigation/ImuBias.h | 5 ++--- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam_unstable/dynamics/PoseRTV.h | 2 +- gtsam_unstable/geometry/BearingS2.h | 2 +- gtsam_unstable/geometry/Pose3Upright.h | 2 +- 23 files changed, 40 insertions(+), 57 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 2a8d4bc41..cd8489bbc 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -29,7 +29,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieMatrix : public Matrix, public DerivedValue { +struct LieMatrix : public Matrix { /// @name Constructors /// @{ @@ -166,8 +166,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("LieMatrix", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("LieMatrix",*this); ar & boost::serialization::make_nvp("Matrix", boost::serialization::base_object(*this)); diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 750a8a21f..b91d3ca2f 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -26,7 +26,7 @@ namespace gtsam { /** * LieScalar is a wrapper around double to allow it to be a Lie type */ - struct GTSAM_EXPORT LieScalar : public DerivedValue { + struct GTSAM_EXPORT LieScalar { /** default constructor */ LieScalar() : d_(0.0) {} diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 8286c95a6..808a47c2c 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -26,7 +26,7 @@ namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector, public DerivedValue { +struct LieVector : public Vector { /** default constructor - should be unnecessary */ LieVector() {} @@ -123,8 +123,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("LieVector", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("LieVector",*this); ar & boost::serialization::make_nvp("Vector", boost::serialization::base_object(*this)); } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 793f195d5..375749e69 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -28,7 +28,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Bundler: public DerivedValue { +class GTSAM_EXPORT Cal3Bundler { private: double f_; ///< focal length @@ -173,8 +173,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("Cal3Bundler", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("Cal3Bundler",*this); ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index d716d398e..0ef34005d 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -37,7 +37,7 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public DerivedValue { +class GTSAM_EXPORT Cal3DS2 { protected: @@ -153,8 +153,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3DS2", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Cal3DS2",*this); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index a87a30e36..2f3a61bce 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -31,7 +31,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3_S2: public DerivedValue { +class GTSAM_EXPORT Cal3_S2 { private: double fx_, fy_, s_, u0_, v0_; @@ -227,8 +227,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("Cal3_S2", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("Cal3_S2",*this); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 6e7b5a114..0e781fbc1 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -39,7 +39,7 @@ public: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera: public DerivedValue { +class GTSAM_EXPORT CalibratedCamera { private: Pose3 pose_; // 6DOF pose @@ -215,8 +215,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("CalibratedCamera", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("CalibratedCamera",*this); ar & BOOST_SERIALIZATION_NVP(pose_); } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index a973f9cec..48a0fead6 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -20,7 +20,7 @@ namespace gtsam { * but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * We can then non-linearly optimize immediately on this 5-dimensional manifold. */ -class GTSAM_EXPORT EssentialMatrix: public DerivedValue { +class GTSAM_EXPORT EssentialMatrix { private: @@ -176,8 +176,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("EssentialMatrix", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("EssentialMatrix",*this); ar & BOOST_SERIALIZATION_NVP(aRb_); ar & BOOST_SERIALIZATION_NVP(aTb_); diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c2fea07e0..a6c1c6f42 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -37,7 +37,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeCamera: public DerivedValue > { +class PinholeCamera { private: Pose3 pose_; Calibration K_; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 7d1fab133..56570723d 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -32,7 +32,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Point2 : public DerivedValue { +class GTSAM_EXPORT Point2 { private: @@ -237,8 +237,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Point2", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Point2",*this); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index d69ceb861..7739f3d9c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -36,7 +36,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Point3 : public DerivedValue { + class GTSAM_EXPORT Point3 { private: @@ -228,8 +228,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Point3", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Point3",*this); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(z_); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index b6a2314ff..f1cd6bd3f 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -33,7 +33,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose2 : public DerivedValue { +class GTSAM_EXPORT Pose2 { public: @@ -301,8 +301,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Pose2", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Pose2",*this); ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 5f99b25ac..57132b453 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -39,7 +39,7 @@ class Pose2; * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3: public DerivedValue { +class GTSAM_EXPORT Pose3{ public: /** Pose Concept requirements */ @@ -326,8 +326,7 @@ public: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Pose3", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Pose3",*this); ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 4142d4473..55f7bad50 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -31,7 +31,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot2 : public DerivedValue { + class GTSAM_EXPORT Rot2 { public: /** get the dimension by the type */ @@ -235,8 +235,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Rot2", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Rot2",*this); ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7215e159f..88c0fe370 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -58,7 +58,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot3 : public DerivedValue { + class GTSAM_EXPORT Rot3 { private: @@ -456,8 +456,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Rot3", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Rot3",*this); #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); ar & boost::serialization::make_nvp("rot12", rot_(0,1)); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 82914f6ab..ca0377c1b 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -36,7 +36,7 @@ public: * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry */ -class GTSAM_EXPORT StereoCamera : public DerivedValue { +class GTSAM_EXPORT StereoCamera { private: Pose3 leftCamPose_; @@ -147,8 +147,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("StereoCamera", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("StereoCamera",*this); ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 000f7d16f..9da456b60 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -28,7 +28,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT StereoPoint2 : public DerivedValue { + class GTSAM_EXPORT StereoPoint2 { public: static const size_t dimension = 3; private: @@ -162,8 +162,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("StereoPoint2", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("StereoPoint2",*this); ar & BOOST_SERIALIZATION_NVP(uL_); ar & BOOST_SERIALIZATION_NVP(uR_); ar & BOOST_SERIALIZATION_NVP(v_); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index bb2ee318a..6a84e014c 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -27,7 +27,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3: public DerivedValue { +class GTSAM_EXPORT Unit3{ private: @@ -146,8 +146,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Unit3", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Unit3",*this); ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(B_); } diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 8301a0a6b..c08a37905 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -39,7 +39,7 @@ namespace gtsam { /// All bias models live in the imuBias namespace namespace imuBias { - class ConstantBias : public DerivedValue { + class ConstantBias { private: Vector3 biasAcc_; Vector3 biasGyro_; @@ -205,8 +205,7 @@ namespace imuBias { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("imuBias::ConstantBias", - boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasGyro_); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 041ea0387..a8b7b612f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -51,7 +51,7 @@ public: }; int TestValueData::ConstructorCount = 0; int TestValueData::DestructorCount = 0; -class TestValue : public DerivedValue { +class TestValue { TestValueData data_; public: virtual void print(const std::string& str = "") const {} diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 80729e8a2..04da7c513 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -19,7 +19,7 @@ typedef Point3 Velocity3; * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation */ -class GTSAM_UNSTABLE_EXPORT PoseRTV : public DerivedValue { +class GTSAM_UNSTABLE_EXPORT PoseRTV { protected: Pose3 Rt_; diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index d48490ccc..4c84bbe56 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -15,7 +15,7 @@ namespace gtsam { -class GTSAM_UNSTABLE_EXPORT BearingS2 : public DerivedValue { +class GTSAM_UNSTABLE_EXPORT BearingS2 { protected: Rot2 azimuth_, elevation_; diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index c1e12b4a7..a2db1d176 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -22,7 +22,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class GTSAM_UNSTABLE_EXPORT Pose3Upright : public DerivedValue { +class GTSAM_UNSTABLE_EXPORT Pose3Upright { public: static const size_t dimension = 4; From 5b2a61682d99ebe09a331cf02ef0a72c7accea20 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 24 Oct 2014 22:38:03 +0200 Subject: [PATCH 04/18] tests compiling, but many fail --- gtsam/geometry/PinholeCamera.h | 1 - gtsam/nonlinear/Values-inl.h | 8 ++++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index a6c1c6f42..aa42b638f 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -653,7 +653,6 @@ private: 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(pose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 11c44cad4..76d47b429 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -205,7 +205,7 @@ namespace gtsam { Values::Values(const Values::Filtered& view) { BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { Key key = key_value.key; - insert(key, key_value.value); + insert(key, static_cast(key_value.value)); } } @@ -281,7 +281,7 @@ namespace gtsam { 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_; + return static_cast&>(*item->second).value(); } else { return boost::none; } @@ -293,4 +293,8 @@ namespace gtsam { insert(j, static_cast(GenericValue(val))); } + template + void Values::update(Key j, const ValueType& val) { + update(j, static_cast(GenericValue(val))); + } } From 4a3dc51f85d565d4f9a48a88b5aa910faefe66f5 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 24 Oct 2014 23:47:02 +0200 Subject: [PATCH 05/18] more tests work, except for serialization based tests --- .../pose3example-offdiagonal-rewritten.txt | 2 -- examples/Data/pose3example-rewritten.txt | 5 ---- gtsam/nonlinear/tests/testValues.cpp | 28 +++++++++++-------- .../nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- .../nonlinear/tests/testExpressionFactor.cpp | 14 +++++----- 5 files changed, 24 insertions(+), 27 deletions(-) diff --git a/examples/Data/pose3example-offdiagonal-rewritten.txt b/examples/Data/pose3example-offdiagonal-rewritten.txt index b99d266aa..a855eff4d 100644 --- a/examples/Data/pose3example-offdiagonal-rewritten.txt +++ b/examples/Data/pose3example-offdiagonal-rewritten.txt @@ -1,3 +1 @@ -VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 -VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000 diff --git a/examples/Data/pose3example-rewritten.txt b/examples/Data/pose3example-rewritten.txt index 6d342fcb0..d445fa96c 100644 --- a/examples/Data/pose3example-rewritten.txt +++ b/examples/Data/pose3example-rewritten.txt @@ -1,8 +1,3 @@ -VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 -VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 -VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446 -VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024 -VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index a8b7b612f..5d82891dc 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -54,9 +54,9 @@ int TestValueData::DestructorCount = 0; class TestValue { TestValueData data_; public: - virtual void print(const std::string& str = "") const {} + void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } - virtual size_t dim() const { return 0; } + size_t dim() const { return 0; } TestValue retract(const Vector&) const { return TestValue(); } Vector localCoordinates(const TestValue&) const { return Vector(); } }; @@ -353,12 +353,12 @@ TEST(Values, filter) { BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); - EXPECT(typeid(Pose2) == typeid(key_value.value)); - EXPECT(assert_equal(pose2, dynamic_cast(key_value.value))); + EXPECT(typeid(GenericValue) == typeid(key_value.value)); + EXPECT(assert_equal(pose2, dynamic_cast&>(key_value.value).value())); } else if(i == 1) { LONGS_EQUAL(3, (long)key_value.key); - EXPECT(typeid(Pose3) == typeid(key_value.value)); - EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); + EXPECT(typeid(GenericValue) == typeid(key_value.value)); + EXPECT(assert_equal(pose3, dynamic_cast&>(key_value.value).value())); } else { EXPECT(false); } @@ -416,10 +416,10 @@ TEST(Values, Symbol_filter) { BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { if(i == 0) { LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); - EXPECT(assert_equal(pose1, dynamic_cast(key_value.value))); + EXPECT(assert_equal(pose1, dynamic_cast&>(key_value.value).value())); } else if(i == 1) { LONGS_EQUAL(Symbol('y', 3), (long)key_value.key); - EXPECT(assert_equal(pose3, dynamic_cast(key_value.value))); + EXPECT(assert_equal(pose3, dynamic_cast&>(key_value.value).value())); } else { EXPECT(false); } @@ -441,11 +441,15 @@ TEST(Values, Destructors) { values.insert(0, value1); values.insert(1, value2); } - LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); - LONGS_EQUAL(2, (long)TestValueData::DestructorCount); + // additional 2 con/destructor counts for the temporary + // GenericValue in insert() + // but I'm sure some advanced programmer can figure out + // a way to avoid the temporary, or optimize it out + LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(2+2, (long)TestValueData::DestructorCount); } - LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); - LONGS_EQUAL(4, (long)TestValueData::DestructorCount); + LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount); + LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index dc07c4b46..0c9046d6c 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -202,7 +202,7 @@ TEST(Expression, Snavely) { #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(480,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(528,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c063f182f..a320ebc5a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -144,19 +144,19 @@ TEST(ExpressionFactor, Binary) { // traceRaw will fill raw with [Trace | Binary::Record] EXPECT_LONGS_EQUAL(8, sizeof(double)); - EXPECT_LONGS_EQUAL(24, sizeof(Point2)); - EXPECT_LONGS_EQUAL(48, sizeof(Cal3_S2)); + EXPECT_LONGS_EQUAL(16, sizeof(Point2)); + EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); - size_t expectedRecordSize = 24 + 24 + 48 + 2 * 16 + 80 + 32; - EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); + size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size size_t size = binary.traceSize(); CHECK(size); - EXPECT_LONGS_EQUAL(expectedRecordSize, size); + EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla char raw[size]; @@ -208,8 +208,8 @@ TEST(ExpressionFactor, Shallow) { EXPECT_LONGS_EQUAL(464, sizeof(Binary::Record)); LONGS_EQUAL(112+464, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(496, sizeof(Binary::Record)); - LONGS_EQUAL(112+496, expectedTraceSize); + EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); + LONGS_EQUAL(112+400, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); From c2cdd21a7a9b921f715cfd24c7c4534b1e479993 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Sat, 25 Oct 2014 12:18:12 +0200 Subject: [PATCH 06/18] added cast function to Value --- gtsam/base/GenericValue.h | 7 +++++++ gtsam/base/Value.h | 4 ++++ gtsam/nonlinear/tests/testValues.cpp | 12 ++++++------ 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index f7b5e985a..4fe5bc143 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -187,4 +187,11 @@ private: struct PoolTag { }; }; +// define Value::cast here since now GenericValue has been declared +template + const ValueType& Value::cast() const { + return dynamic_cast&>(*this).value(); + } + + } /* namespace gtsam */ diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 4d7104ad7..4ba468a87 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -122,6 +122,10 @@ namespace gtsam { /** Assignment operator */ virtual Value& operator=(const Value& rhs) = 0; + /** Cast to known ValueType */ + template + const ValueType& cast() const; + /** Virutal destructor */ virtual ~Value() {} diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 5d82891dc..5fbe4d6af 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -353,12 +353,12 @@ TEST(Values, filter) { BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); - EXPECT(typeid(GenericValue) == typeid(key_value.value)); - EXPECT(assert_equal(pose2, dynamic_cast&>(key_value.value).value())); + try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} + EXPECT(assert_equal(pose2, key_value.value.cast())); } else if(i == 1) { LONGS_EQUAL(3, (long)key_value.key); - EXPECT(typeid(GenericValue) == typeid(key_value.value)); - EXPECT(assert_equal(pose3, dynamic_cast&>(key_value.value).value())); + try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");} + EXPECT(assert_equal(pose3, key_value.value.cast())); } else { EXPECT(false); } @@ -416,10 +416,10 @@ TEST(Values, Symbol_filter) { BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { if(i == 0) { LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); - EXPECT(assert_equal(pose1, dynamic_cast&>(key_value.value).value())); + EXPECT(assert_equal(pose1, key_value.value.cast())); } else if(i == 1) { LONGS_EQUAL(Symbol('y', 3), (long)key_value.key); - EXPECT(assert_equal(pose3, dynamic_cast&>(key_value.value).value())); + EXPECT(assert_equal(pose3, key_value.value.cast())); } else { EXPECT(false); } From 9ef89803625eb419566d850fa5f3d917b404a4a2 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Sat, 25 Oct 2014 22:23:26 +0200 Subject: [PATCH 07/18] using static_cast in GenericValue's virtual functions should be more efficient, but perhaps will crash instead of throwing an exception when the Value& derived class doesn't match. --- gtsam/base/GenericValue.h | 10 +++++----- gtsam/nonlinear/tests/testValues.cpp | 2 ++ 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 4fe5bc143..43f606a17 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -12,8 +12,8 @@ /* * @file GenericValue.h * @date Jan 26, 2012 - * @author Duy Nguyen Ta - * @author Mike Bosse, Abel Gawel, Renaud Dube + * @author Michael Bosse, Abel Gawel, Renaud Dube + * based on DrivedValue.h by Duy Nguyen Ta */ #pragma once @@ -117,7 +117,7 @@ public: /// 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 = dynamic_cast(p); + const This& genericValue2 = static_cast(p); // Return the result of using the equals traits for the derived class return traits::equals(this->value_, genericValue2.value_, tol); @@ -140,7 +140,7 @@ public: /// 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 = dynamic_cast(value2); + const This& genericValue2 = static_cast(value2); // Return the result of calling localCoordinates trait on the derived class return traits::localCoordinates(value_,genericValue2.value_); @@ -156,7 +156,7 @@ public: /// Assignment operator virtual Value& operator=(const Value& rhs) { // Cast the base class Value pointer to a derived class pointer - const This& derivedRhs = dynamic_cast(rhs); + const This& derivedRhs = static_cast(rhs); // Do the assignment and return the result this->value_ = derivedRhs.value_; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 5fbe4d6af..d77ecaf79 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -354,10 +354,12 @@ TEST(Values, filter) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} + THROWS_EXCEPTION(key_value.value.cast()); EXPECT(assert_equal(pose2, key_value.value.cast())); } else if(i == 1) { LONGS_EQUAL(3, (long)key_value.key); try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");} + THROWS_EXCEPTION(key_value.value.cast()); EXPECT(assert_equal(pose3, key_value.value.cast())); } else { EXPECT(false); From bc094951ed674a8f7884aacedd0288814c8c6cfc Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Tue, 28 Oct 2014 00:57:44 +0100 Subject: [PATCH 08/18] 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)); } From ab76a306b7b2289e6f83665ba495ac0b048ab779 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Tue, 28 Oct 2014 08:54:41 +0100 Subject: [PATCH 09/18] using dynamic_cast to check base class typeid --- gtsam/nonlinear/Values.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 57dec755b..459277dd7 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -395,7 +395,7 @@ namespace gtsam { static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { BOOST_STATIC_ASSERT((!std::is_same::value)); // Filter and check the type - return filter(key_value.key) && (typeid(GenericValue) == typeid(key_value.value) ); + return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } /** Serialization function */ From 80187362b830c5a0277fe1f1e013fba5a1ebde8e Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Tue, 28 Oct 2014 11:20:02 +0100 Subject: [PATCH 10/18] attemping to expose ChartValue for expressions with non DefaultCharts, but needs testing --- gtsam/base/ChartValue.h | 5 ++ gtsam/base/Manifold.h | 3 + gtsam_unstable/nonlinear/Expression-inl.h | 64 ++++++++++++++++--- gtsam_unstable/nonlinear/Expression.h | 6 +- .../nonlinear/tests/testExpressionMeta.cpp | 2 +- 5 files changed, 68 insertions(+), 12 deletions(-) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 52ae1650a..1ba51ff6a 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -139,6 +139,11 @@ private: struct PoolTag { }; }; +namespace traits { +template +struct dimension > : public dimension {}; +} + template const Chart& Value::getChart() const { // define Value::cast here since now ChartValue has been declared diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index c5b0268aa..0cac8cc6d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -161,8 +161,11 @@ struct DefaultChart { return origin.dim(); } }; + namespace traits { +// populate default traits template struct is_chart > : public std::true_type {}; +template struct dimension > : public dimension {}; } template diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index d5c5f1279..5919d1464 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -293,8 +293,9 @@ public: //----------------------------------------------------------------------------- /// Leaf Expression -template +template > class LeafExpression: public ExpressionNode { + typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? /// The key into values Key key_; @@ -303,6 +304,53 @@ class LeafExpression: public ExpressionNode { LeafExpression(Key key) : key_(key) { } + // todo: do we need a virtual destructor here too? + + friend class Expression ; + +public: + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + // get dimension from the chart; only works for fixed dimension charts + map[key_] = traits::dimension::value; + } + + /// Return value + virtual const value_type& value(const Values& values) const { + return dynamic_cast(values.at(key_)); + } + + /// Construct an execution trace for reverse AD + virtual const value_type& traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + trace.setLeaf(key_); + return dynamic_cast(values.at(key_)); + } + +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression >: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + // todo: do we need a virtual destructor here too? friend class Expression ; @@ -374,7 +422,7 @@ struct Jacobian { /// meta-function to generate JacobianTA optional reference template -struct Optional { +struct OptionalJacobian { typedef Eigen::Matrix::value, traits::dimension::value> Jacobian; typedef boost::optional type; @@ -504,7 +552,7 @@ struct FunctionalNode { // Argument types and derived, note these are base 0 ! typedef TYPES Arguments; typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; + typedef typename boost::mpl::transform >::type Optionals; /// Reset expression shared pointer template @@ -559,7 +607,7 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef boost::function::type)> Function; + typedef boost::function::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -604,8 +652,8 @@ class BinaryExpression: public FunctionalNode >::t public: typedef boost::function< - T(const A1&, const A2&, typename Optional::type, - typename Optional::type)> Function; + T(const A1&, const A2&, typename OptionalJacobian::type, + typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -658,8 +706,8 @@ class TernaryExpression: public FunctionalNode public: typedef boost::function< - T(const A1&, const A2&, const A3&, typename Optional::type, - typename Optional::type, typename Optional::type)> Function; + T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, + typename OptionalJacobian::type, typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 6ac7d9ce8..6e97dd583 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -61,7 +61,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename Optional::type) const) : + T (A::*method)(typename OptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -75,8 +75,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename Optional::type, - typename Optional::type) const, + T (A1::*method)(const A2&, typename OptionalJacobian::type, + typename OptionalJacobian::type) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index ecc343384..84a1ca720 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -201,7 +201,7 @@ Pose3 pose; // Now, let's create the optional Jacobian arguments typedef Point3 T; typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; + typedef boost::mpl::transform >::type Optionals; // Unfortunately this is moot: we need a pointer to a function with the // optional derivatives; I don't see a way of calling a function that we From 82f6ed5ca8255faa12829737e0fa8ae6611ad612 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Tue, 28 Oct 2014 14:15:34 +0100 Subject: [PATCH 11/18] inserted spaces after commas --- gtsam/base/ChartValue.h | 18 ++++++------- gtsam/base/Manifold.h | 4 +-- gtsam/base/numericalDerivative.h | 2 +- gtsam/nonlinear/Values-inl.h | 12 ++++----- gtsam/nonlinear/Values.h | 2 +- gtsam/nonlinear/tests/testValues.cpp | 16 +++++------ tests/testManifold.cpp | 40 ++++++++++++++-------------- 7 files changed, 47 insertions(+), 47 deletions(-) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 1ba51ff6a..a5028b0d3 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -42,20 +42,20 @@ 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)); +// 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; + 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) {} + ChartValue(const T& value, C chart_initializer) : GenericValue(value), Chart(chart_initializer) {} virtual ~ChartValue() {} @@ -96,7 +96,7 @@ class ChartValue : public GenericValue, public CHART { // Create a Value pointer copy of the result void* resultAsValuePlace = boost::singleton_pool::malloc(); - Value* resultAsValue = new(resultAsValuePlace) ChartValue(retractResult, static_cast(*this)); + Value* resultAsValue = new(resultAsValuePlace) ChartValue(retractResult, static_cast(*this)); // Return the pointer to the Value base class return resultAsValue; @@ -141,7 +141,7 @@ private: namespace traits { template -struct dimension > : public dimension {}; +struct dimension > : public dimension {}; } template diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 0cac8cc6d..8f897c36d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -181,12 +181,12 @@ struct ChartConcept { /** * Returns Retraction update of val_ */ - type retract_ret = C::retract(val_,vec_); + type retract_ret = C::retract(val_, vec_); /** * Returns local coordinates of another object */ - vec_ = C::local(val_,retract_ret); + vec_ = C::local(val_, retract_ret); // a way to get the dimension that is compatible with dynamically sized types dim_ = C::getDimension(val_); diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 70edb64e6..9339c9c7b 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -124,7 +124,7 @@ Matrix numericalDerivative11(boost::function h, const X& x, ChartY chartY; // Bit of a hack for now to find number of rows - TangentY zeroY = chartY.local(hx,hx); + TangentY zeroY = chartY.local(hx, hx); size_t m = zeroY.size(); // get chart at x diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 36bb60144..87b2a51cc 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -65,19 +65,19 @@ namespace gtsam { }; // partial specialized version for ValueType == Value template - struct ValuesCastHelper { + struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key,Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; // partial specialized version for ValueType == Value template - struct ValuesCastHelper { + struct ValuesCastHelper { static CastedKeyValuePairType cast(KeyValuePairType key_value) { // Static cast because we already checked the type during filtering - // in this case the casted and keyvalue pair are essentially the same type (key,Value&) so perhaps this could be done with just a cast of the key_value? + // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value? return CastedKeyValuePairType(key_value.key, key_value.value); } }; @@ -302,7 +302,7 @@ namespace gtsam { // overloaded insert with chart initializer template void Values::insert(Key j, const ValueType& val, ChartInit chart) { - insert(j, static_cast(ChartValue(val,chart))); + insert(j, static_cast(ChartValue(val, chart))); } // update with default chart @@ -318,7 +318,7 @@ namespace gtsam { // 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))); + update(j, static_cast(ChartValue(val, chart))); } } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 459277dd7..f6a8af3a1 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -393,7 +393,7 @@ namespace gtsam { // supplied \c filter function. template static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { - BOOST_STATIC_ASSERT((!std::is_same::value)); + BOOST_STATIC_ASSERT((!std::is_same::value)); // Filter and check the type return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 17da8b08e..bfc156671 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -38,7 +38,7 @@ static double inf = std::numeric_limits::infinity(); using symbol_shorthand::X; using symbol_shorthand::L; -const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4); +const Symbol key1('v', 1), key2('v', 2), key3('v', 3), key4('v', 4); class TestValueData { @@ -76,10 +76,10 @@ TEST( Values, equals1 ) Values expected; LieVector v((Vector(3) << 5.0, 6.0, 7.0)); - expected.insert(key1,v); + expected.insert(key1, v); Values actual; - actual.insert(key1,v); - CHECK(assert_equal(expected,actual)); + actual.insert(key1, v); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -269,7 +269,7 @@ TEST(Values, expmap_d) CHECK(config0.equals(config0)); Values poseconfig; - poseconfig.insert(key1, Pose2(1,2,3)); + poseconfig.insert(key1, Pose2(1, 2, 3)); poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5)); CHECK(equal(config0, config0)); @@ -339,7 +339,7 @@ TEST(Values, update) Values expected; expected.insert(key1, LieVector((Vector(1) << -1.))); expected.insert(key2, LieVector((Vector(1) << -2.))); - CHECK(assert_equal(expected,config0)); + CHECK(assert_equal(expected, config0)); } /* ************************************************************************* */ @@ -419,9 +419,9 @@ TEST(Values, Symbol_filter) { Values values; values.insert(X(0), pose0); - values.insert(Symbol('y',1), pose1); + values.insert(Symbol('y', 1), pose1); values.insert(X(2), pose2); - values.insert(Symbol('y',3), pose3); + values.insert(Symbol('y', 3), pose3); int i = 0; BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index b3d45ab19..55a5f5af0 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -67,8 +67,8 @@ TEST(Manifold, _dimension) { TEST(Manifold, DefaultChart) { 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)); + 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; @@ -79,7 +79,7 @@ TEST(Manifold, DefaultChart) { DefaultChart chart3; Vector v1(1); v1 << 1; - EXPECT(assert_equal(v1,chart3.local(0, 1))); + EXPECT(assert_equal(v1, chart3.local(0, 1))); EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! @@ -94,20 +94,20 @@ TEST(Manifold, DefaultChart) { Rot3 I = Rot3::identity(); Rot3 R = I.retract(v3); DefaultChart chart5; - EXPECT(assert_equal(v3,chart5.local(I, R))); + EXPECT(assert_equal(v3, chart5.local(I, R))); EXPECT(assert_equal(chart5.retract(I, v3), R)); // Check zero vector DefaultChart chart6; - EXPECT(assert_equal(zero(3),chart6.local(R, R))); + EXPECT(assert_equal(zero(3), chart6.local(R, R))); } /* ************************************************************************* */ // zero TEST(Manifold, _zero) { - EXPECT(assert_equal(Pose3(),traits::zero::value())); + EXPECT(assert_equal(Pose3(), traits::zero::value())); Cal3Bundler cal(0, 0, 0); - EXPECT(assert_equal(cal,traits::zero::value())); - EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); + EXPECT(assert_equal(cal, traits::zero::value())); + EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero::value())); } /* ************************************************************************* */ @@ -115,14 +115,14 @@ TEST(Manifold, _zero) { TEST(Manifold, Canonical) { Canonical chart1; - EXPECT(chart1.local(Point2(1,0))==Vector2(1,0)); - EXPECT(chart1.retract(Vector2(1,0))==Point2(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.local(Vector2(1,0)))); - EXPECT(chart2.retract(v2)==Vector2(1,0)); + EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0)))); + EXPECT(chart2.retract(v2)==Vector2(1, 0)); Canonical chart3; Eigen::Matrix v1; @@ -134,29 +134,29 @@ TEST(Manifold, Canonical) { Point3 point(1, 2, 3); Vector v3(3); v3 << 1, 2, 3; - EXPECT(assert_equal(v3,chart4.local(point))); - EXPECT(assert_equal(chart4.retract(v3),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.local(pose))); - EXPECT(assert_equal(chart5.retract(v6),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.local(camera))); - EXPECT(assert_equal(chart6.retract(z9),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.local(camera2))); - EXPECT(assert_equal(chart6.retract(v9),camera2)); + EXPECT(assert_equal(v9, chart6.local(camera2))); + EXPECT(assert_equal(chart6.retract(v9), camera2)); } /* ************************************************************************* */ From f8183acd8769c317e85aaba022d09b9d5e2e84b4 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Tue, 28 Oct 2014 17:37:45 +0100 Subject: [PATCH 12/18] I should have remembered to compile and check before committing. --- gtsam/base/ChartValue.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index a5028b0d3..fb51a55f8 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -45,7 +45,7 @@ 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 { +class ChartValue : public GenericValue, public Chart_ { BOOST_CONCEPT_ASSERT((ChartConcept)); public: typedef T type; From 2788ec7f33954dc46da24ea7d5bf25ad919df125 Mon Sep 17 00:00:00 2001 From: gawela Date: Wed, 29 Oct 2014 10:23:13 +0100 Subject: [PATCH 13/18] removed std::type_traits which is c++11 --- gtsam/base/LieMatrix.h | 2 +- gtsam/base/LieScalar.h | 6 +++--- gtsam/base/LieVector.h | 2 +- gtsam/base/Manifold.h | 26 +++++++++++++------------- gtsam/geometry/Cal3Bundler.h | 4 ++-- gtsam/geometry/Cal3DS2.h | 4 ++-- gtsam/geometry/Cal3Unified.h | 4 ++-- gtsam/geometry/Cal3_S2.h | 4 ++-- gtsam/geometry/CalibratedCamera.h | 6 +++--- gtsam/geometry/EssentialMatrix.h | 4 ++-- gtsam/geometry/PinholeCamera.h | 4 ++-- gtsam/geometry/Point2.h | 6 +++--- gtsam/geometry/Point3.h | 6 +++--- gtsam/geometry/Pose2.h | 4 ++-- gtsam/geometry/Pose3.h | 6 +++--- gtsam/geometry/Rot2.h | 6 +++--- gtsam/geometry/Rot3.h | 6 +++--- gtsam/geometry/StereoCamera.h | 4 ++-- gtsam/geometry/StereoPoint2.h | 6 +++--- gtsam/geometry/Unit3.h | 4 ++-- gtsam/navigation/ImuBias.h | 6 +++--- gtsam/nonlinear/Values.h | 2 +- gtsam/nonlinear/tests/testValues.cpp | 4 ++-- gtsam_unstable/dynamics/PoseRTV.h | 4 ++-- gtsam_unstable/geometry/Pose3Upright.h | 4 ++-- 25 files changed, 67 insertions(+), 67 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index cd8489bbc..1aca49d55 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -178,7 +178,7 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index b91d3ca2f..1d4aa86e2 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -116,15 +116,15 @@ namespace gtsam { namespace traits { template<> - struct is_group : public std::true_type { + struct is_group : public boost::true_type { }; template<> - struct is_manifold : public std::true_type { + struct is_manifold : public boost::true_type { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 808a47c2c..9f84af73d 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -133,7 +133,7 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 8f897c36d..4fe6c9d99 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -50,7 +50,7 @@ namespace traits { // is group, by default this is false template -struct is_group: public std::false_type { +struct is_group: public boost::false_type { }; // identity, no default provided, by default given by default constructor @@ -63,11 +63,11 @@ struct identity { // is manifold, by default this is false template -struct is_manifold: public std::false_type { +struct is_manifold: public boost::false_type { }; // dimension, can return Eigen::Dynamic (-1) if not known at compile time -typedef std::integral_constant Dynamic; +typedef boost::integral_constant Dynamic; template struct dimension : public Dynamic {}; //default to dynamic @@ -83,15 +83,15 @@ template struct zero: public identity { // double template<> -struct is_group : public std::true_type { +struct is_group : public boost::true_type { }; template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> @@ -104,11 +104,11 @@ struct zero { // Fixed size Eigen::Matrix type template -struct is_group > : public std::true_type { +struct is_group > : public boost::true_type { }; template -struct is_manifold > : public std::true_type { +struct is_manifold > : public boost::true_type { }; // TODO: Could be more sophisticated using Eigen traits and SFINAE? @@ -126,12 +126,12 @@ struct dimension > : public Dy }; template -struct dimension > : public std::integral_constant< +struct dimension > : public boost::integral_constant< int, M * N> { }; template -struct zero > : public std::integral_constant< +struct zero > : public boost::integral_constant< int, M * N> { BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), "traits::zero is only supported for fixed-size matrices"); @@ -140,7 +140,7 @@ struct zero > : public std::integral_consta } }; -template struct is_chart : public std::false_type {}; +template struct is_chart : public boost::false_type {}; } // \ namespace traits @@ -164,7 +164,7 @@ struct DefaultChart { namespace traits { // populate default traits -template struct is_chart > : public std::true_type {}; +template struct is_chart > : public boost::true_type {}; template struct dimension > : public dimension {}; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 375749e69..81c7f6851 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -189,11 +189,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0ef34005d..47dc934b2 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -171,11 +171,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index ad8e7b904..7986161e1 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -142,11 +142,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 2f3a61bce..cb93850c7 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -243,11 +243,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 5d4c6420e..4a8efae7b 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -226,15 +226,15 @@ private: namespace traits { template<> -struct is_group : public std::true_type { +struct is_group : public boost::true_type { }; template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 48a0fead6..5ac83a97a 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -199,11 +199,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index aa42b638f..83851e8c0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -663,11 +663,11 @@ private: namespace traits { template -struct is_manifold > : public std::true_type { +struct is_manifold > : public boost::true_type { }; template -struct dimension > : public std::integral_constant< +struct dimension > : public boost::integral_constant< int, dimension::value + dimension::value> { }; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 56570723d..825d45307 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -253,15 +253,15 @@ inline Point2 operator*(double s, const Point2& p) {return p*s;} namespace traits { template<> -struct is_group : public std::true_type { +struct is_group : public boost::true_type { }; template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7739f3d9c..09e340ad2 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -245,15 +245,15 @@ namespace gtsam { namespace traits { template<> - struct is_group : public std::true_type { + struct is_group : public boost::true_type { }; template<> - struct is_manifold : public std::true_type { + struct is_manifold : public boost::true_type { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f1cd6bd3f..f5042dc98 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -324,11 +324,11 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 57132b453..a8b82bebe 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -357,15 +357,15 @@ GTSAM_EXPORT boost::optional align(const std::vector& pairs); namespace traits { template<> -struct is_group : public std::true_type { +struct is_group : public boost::true_type { }; template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 55f7bad50..f45e9c7eb 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -246,15 +246,15 @@ namespace gtsam { namespace traits { template<> - struct is_group : public std::true_type { + struct is_group : public boost::true_type { }; template<> - struct is_manifold : public std::true_type { + struct is_manifold : public boost::true_type { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 88c0fe370..69a64bc51 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -493,15 +493,15 @@ namespace gtsam { namespace traits { template<> - struct is_group : public std::true_type { + struct is_group : public boost::true_type { }; template<> - struct is_manifold : public std::true_type { + struct is_manifold : public boost::true_type { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index ca0377c1b..9db1976f6 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -158,11 +158,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 9da456b60..8c4ebd3a8 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -176,15 +176,15 @@ namespace gtsam { namespace traits { template<> - struct is_group : public std::true_type { + struct is_group : public boost::true_type { }; template<> - struct is_manifold : public std::true_type { + struct is_manifold : public boost::true_type { }; template<> - struct dimension : public std::integral_constant { + struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 6a84e014c..1e9591a83 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -159,11 +159,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index c08a37905..9bc24c152 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -221,15 +221,15 @@ namespace imuBias { namespace traits { template<> -struct is_group : public std::true_type { +struct is_group : public boost::true_type { }; template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index f6a8af3a1..e31bfa941 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -393,7 +393,7 @@ namespace gtsam { // supplied \c filter function. template static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { - BOOST_STATIC_ASSERT((!std::is_same::value)); + BOOST_STATIC_ASSERT((!boost::is_same::value)); // Filter and check the type return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index bfc156671..9b9e8d0e0 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -64,9 +64,9 @@ public: namespace gtsam { namespace traits { template <> -struct is_manifold : public std::true_type {}; +struct is_manifold : public boost::true_type {}; template <> -struct dimension : public std::integral_constant {}; +struct dimension : public boost::integral_constant {}; } } diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 04da7c513..ae4ddade4 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -187,11 +187,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; template<> diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index a2db1d176..22aab5d44 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -144,11 +144,11 @@ private: namespace traits { template<> -struct is_manifold : public std::true_type { +struct is_manifold : public boost::true_type { }; template<> -struct dimension : public std::integral_constant { +struct dimension : public boost::integral_constant { }; } From 7d8ba565e54099abfa080163248459e1d98cce5d Mon Sep 17 00:00:00 2001 From: Renaud Dube Date: Thu, 30 Oct 2014 15:59:28 +0100 Subject: [PATCH 14/18] Adapted ChartValue so that it can wrap a value to be passed to ExpressionFactor --- gtsam/base/ChartValue.h | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index fb51a55f8..a672d29ee 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -111,6 +111,16 @@ class ChartValue : public GenericValue, public Chart_ { 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); + } + virtual size_t dim() const { return Chart::getDimension(GenericValue::value()); // need functional form here since the dimension may be dynamic } @@ -150,5 +160,13 @@ 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::value, traits::dimension::value >& > H=boost::none) { + if (H) { + *H = Eigen::Matrix::value, traits::dimension::value >::Identity(); + } + return ChartValue(value); +} } /* namespace gtsam */ From 003224ac3fd62471e44899ce1fda75ec4d14d65f Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 30 Oct 2014 17:21:24 +0100 Subject: [PATCH 15/18] fixing serialization code when class has no base --- gtsam/base/LieMatrix.h | 1 - gtsam/base/LieVector.h | 1 - gtsam/geometry/Cal3Bundler.h | 2 -- gtsam/geometry/Cal3DS2.h | 1 - gtsam/geometry/Cal3Unified.h | 2 -- gtsam/geometry/Cal3_S2.h | 2 -- gtsam/geometry/CalibratedCamera.h | 2 -- gtsam/geometry/EssentialMatrix.h | 1 - gtsam/geometry/Point2.h | 1 - gtsam/geometry/Point3.h | 1 - gtsam/geometry/Pose2.h | 1 - gtsam/geometry/Pose3.h | 1 - gtsam/geometry/Rot2.h | 1 - gtsam/geometry/Rot3.h | 1 - gtsam/geometry/StereoCamera.h | 1 - gtsam/geometry/StereoPoint2.h | 1 - gtsam/geometry/Unit3.h | 1 - 17 files changed, 21 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 1aca49d55..5d4f89f48 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -166,7 +166,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("LieMatrix",*this); ar & boost::serialization::make_nvp("Matrix", boost::serialization::base_object(*this)); diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 9f84af73d..99b351ff5 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -123,7 +123,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("LieVector",*this); ar & boost::serialization::make_nvp("Vector", boost::serialization::base_object(*this)); } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 81c7f6851..db06c7aca 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -172,8 +172,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar - & boost::serialization::make_nvp("Cal3Bundler",*this); ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 47dc934b2..4179a76e0 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -153,7 +153,6 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3DS2",*this); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 7986161e1..b75efff94 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -131,8 +131,6 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index cb93850c7..cd1add116 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -226,8 +226,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar - & boost::serialization::make_nvp("Cal3_S2",*this); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 4a8efae7b..66d1c5125 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -214,8 +214,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar - & boost::serialization::make_nvp("CalibratedCamera",*this); ar & BOOST_SERIALIZATION_NVP(pose_); } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 5ac83a97a..e23b22093 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -176,7 +176,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("EssentialMatrix",*this); ar & BOOST_SERIALIZATION_NVP(aRb_); ar & BOOST_SERIALIZATION_NVP(aTb_); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 825d45307..59a18aed7 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -237,7 +237,6 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Point2",*this); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 09e340ad2..af54e0459 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -228,7 +228,6 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Point3",*this); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(z_); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f5042dc98..7aad66710 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -301,7 +301,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Pose2",*this); ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index a8b82bebe..001edb563 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -326,7 +326,6 @@ public: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Pose3",*this); ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index f45e9c7eb..c5d6141b6 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -235,7 +235,6 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Rot2",*this); ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 69a64bc51..4f40d164d 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -456,7 +456,6 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Rot3",*this); #ifndef GTSAM_USE_QUATERNIONS ar & boost::serialization::make_nvp("rot11", rot_(0,0)); ar & boost::serialization::make_nvp("rot12", rot_(0,1)); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 9db1976f6..6b290edac 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -147,7 +147,6 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("StereoCamera",*this); ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8c4ebd3a8..d62a3f067 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -162,7 +162,6 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("StereoPoint2",*this); ar & BOOST_SERIALIZATION_NVP(uL_); ar & BOOST_SERIALIZATION_NVP(uR_); ar & BOOST_SERIALIZATION_NVP(v_); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 1e9591a83..3be83aa15 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -146,7 +146,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Unit3",*this); ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(B_); } From 5e51745b86d6717379ca839e9c4ee1b8db3343d8 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Fri, 31 Oct 2014 14:21:02 +0100 Subject: [PATCH 16/18] debugging serialization of ChartValues --- gtsam/base/ChartValue.h | 18 ++++++++++++++++++ gtsam/base/GenericValue.h | 5 +++++ .../tests/testSerializationNonlinear.cpp | 17 +++++++++++++++++ 3 files changed, 40 insertions(+) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index a672d29ee..2f7cd5813 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -147,6 +147,24 @@ protected: private: /// Fake Tag struct for singleton pool allocator. In fact, it is never used! 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", GenericValue::value()); + // todo: implement a serialization for charts + //ar & boost::serialization::make_nvp("Chart", boost::serialization::base_object(*this)); + } + + /// @} + }; namespace traits { diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 291bdd8f9..a4446d53c 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -61,6 +61,11 @@ public: return traits::equals(this->value_, genericValue2.value_, tol); } + // non virtual equals function + bool equals(const GenericValue &other, double tol = 1e-9) const { + return traits::equals(this->value(),other.value(),tol); + } + virtual void print(const std::string& str) const { traits::print(value_,str); } diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index ee2f0d793..87c75dd5e 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -41,6 +41,8 @@ BOOST_CLASS_EXPORT(gtsam::PinholeCamera) BOOST_CLASS_EXPORT(gtsam::PinholeCamera) BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::ChartValue); + /* ************************************************************************* */ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; @@ -56,12 +58,27 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { + std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(pt3)); + std::cout << __LINE__ << std::endl; + ChartValue chv1(pt3); + std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(chv1)); + std::cout << __LINE__ << std::endl; + PinholeCal3S2 pc(pose3,cal1); + EXPECT(equalsObj(pc)); + std::cout << __LINE__ << std::endl; Values values; + values.insert(1,pt3); + std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(values)); + std::cout << __LINE__ << std::endl; values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); EXPECT(equalsObj(values)); + std::cout << __LINE__ << std::endl; EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); } From 15ab2b27bce8367f3872f316eea8747e94b789ba Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 1 Nov 2014 11:47:56 +0100 Subject: [PATCH 17/18] Fixed one unit test --- gtsam/base/ChartValue.h | 4 +-- gtsam/base/GenericValue.h | 8 +++-- .../tests/testSerializationNonlinear.cpp | 33 ++++++++++++++----- 3 files changed, 32 insertions(+), 13 deletions(-) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 2f7cd5813..6f0cf1406 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -158,9 +158,9 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("value", GenericValue::value()); + // ar & boost::serialization::make_nvp("value",); // todo: implement a serialization for charts - //ar & boost::serialization::make_nvp("Chart", boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object< GenericValue >(*this)); } /// @} diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index a4446d53c..0869769c4 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -56,7 +56,6 @@ public: virtual bool equals_(const Value& p, double tol = 1e-9) const { // Cast the base class Value pointer to a templated generic class pointer 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); } @@ -69,7 +68,12 @@ public: virtual void print(const std::string& str) const { traits::print(value_,str); } - + 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 diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 87c75dd5e..a8f287d1e 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -32,22 +32,37 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2) -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) -BOOST_CLASS_EXPORT(gtsam::Point3) -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Rot3) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::Cal3_S2); +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Pose3); +BOOST_CLASS_EXPORT(gtsam::Rot3); +BOOST_CLASS_EXPORT(gtsam::PinholeCamera); +BOOST_CLASS_EXPORT(gtsam::PinholeCamera); +BOOST_CLASS_EXPORT(gtsam::PinholeCamera); + +namespace detail { +template struct pack { + typedef T type; +}; +} +#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \ + typedef gtsam::ChartValue > UNIQUE_NAME; \ + BOOST_CLASS_EXPORT( UNIQUE_NAME ); -BOOST_CLASS_EXPORT(gtsam::ChartValue); /* ************************************************************************* */ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; +CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3); +CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2); +CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2); +CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler); + + + /* ************************************************************************* */ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); From 95e59d7c5d73bb9cb312cc445a80af1ce810e518 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 1 Nov 2014 19:23:07 +0100 Subject: [PATCH 18/18] Fixed the last failing unit test --- .../pose3example-offdiagonal-rewritten.txt | 2 ++ examples/Data/pose3example-rewritten.txt | 5 ++++ gtsam/nonlinear/Values-inl.h | 1 + gtsam/slam/dataset.cpp | 25 ++++++++----------- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/examples/Data/pose3example-offdiagonal-rewritten.txt b/examples/Data/pose3example-offdiagonal-rewritten.txt index a855eff4d..b99d266aa 100644 --- a/examples/Data/pose3example-offdiagonal-rewritten.txt +++ b/examples/Data/pose3example-offdiagonal-rewritten.txt @@ -1 +1,3 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000 diff --git a/examples/Data/pose3example-rewritten.txt b/examples/Data/pose3example-rewritten.txt index d445fa96c..6d342fcb0 100644 --- a/examples/Data/pose3example-rewritten.txt +++ b/examples/Data/pose3example-rewritten.txt @@ -1,3 +1,8 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 +VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446 +VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488 EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000 diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 87b2a51cc..49ea03e9f 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -33,6 +33,7 @@ namespace gtsam { + /* ************************************************************************* */ template struct _ValuesKeyValuePair { diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db85c65bf..388d712e7 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1,5 +1,4 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved @@ -330,8 +329,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, fstream stream(filename.c_str(), fstream::out); // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { - const Pose2& pose = dynamic_cast(key_value.value); + const Pose2& pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } @@ -373,25 +373,22 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D, /* ************************************************************************* */ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const string& filename) { - fstream stream(filename.c_str(), fstream::out); // save 2D & 3D poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + Values::ConstFiltered viewPose2 = estimate.filter(); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose2) { + stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " + << key_value.value.y() << " " << key_value.value.theta() << endl; + } - const Pose2* pose2D = dynamic_cast(&key_value.value); - if(pose2D){ - stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " " - << pose2D->y() << " " << pose2D->theta() << endl; - } - const Pose3* pose3D = dynamic_cast(&key_value.value); - if(pose3D){ - Point3 p = pose3D->translation(); - Rot3 R = pose3D->rotation(); + Values::ConstFiltered viewPose3 = estimate.filter(); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose3) { + Point3 p = key_value.value.translation(); + Rot3 R = key_value.value.rotation(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w() << endl; - } } // save edges (2D or 3D)