Comments, formatting, some TODO questions
dellaert
parent
e0c4d84dd7
commit
329e7f1383
|
|
@ -11,9 +11,10 @@
|
|||
|
||||
/*
|
||||
* @file ChartValue.h
|
||||
* @date Jan 26, 2012
|
||||
* @brief
|
||||
* @date October, 2014
|
||||
* @author Michael Bosse, Abel Gawel, Renaud Dube
|
||||
* based on DrivedValue.h by Duy Nguyen Ta
|
||||
* based on DerivedValue.h by Duy Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -39,25 +40,44 @@
|
|||
#endif
|
||||
//////////////////
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// ChartValue is derived from GenericValue<T> and Chart so that Chart can be zero sized (as in DefaultChart<T>)
|
||||
// if the Chart is a member variable then it won't ever be zero sized.
|
||||
template<class T, class Chart_=DefaultChart<T> >
|
||||
class ChartValue : public GenericValue<T>, public Chart_ {
|
||||
/**
|
||||
* ChartValue is derived from GenericValue<T> and Chart so that
|
||||
* Chart can be zero sized (as in DefaultChart<T>)
|
||||
* if the Chart is a member variable then it won't ever be zero sized.
|
||||
*/
|
||||
template<class T, class Chart_ = DefaultChart<T> >
|
||||
class ChartValue: public GenericValue<T>, public Chart_ {
|
||||
|
||||
BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
|
||||
public:
|
||||
|
||||
public:
|
||||
|
||||
typedef T type;
|
||||
typedef Chart_ Chart;
|
||||
|
||||
public:
|
||||
ChartValue() : GenericValue<T>(T()) {}
|
||||
ChartValue(const T& value) : GenericValue<T>(value) {}
|
||||
template<typename C>
|
||||
ChartValue(const T& value, C chart_initializer) : GenericValue<T>(value), Chart(chart_initializer) {}
|
||||
public:
|
||||
|
||||
virtual ~ChartValue() {}
|
||||
/// Default Constructor. TODO might not make sense for some types
|
||||
ChartValue() :
|
||||
GenericValue<T>(T()) {
|
||||
}
|
||||
|
||||
/// Construct froma value
|
||||
ChartValue(const T& value) :
|
||||
GenericValue<T>(value) {
|
||||
}
|
||||
|
||||
/// Construct from a value and initialize the chart
|
||||
template<typename C>
|
||||
ChartValue(const T& value, C chart_initializer) :
|
||||
GenericValue<T>(value), Chart(chart_initializer) {
|
||||
}
|
||||
|
||||
/// Destructor
|
||||
virtual ~ChartValue() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a duplicate object returned as a pointer to the generic Value interface.
|
||||
|
|
@ -66,7 +86,7 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
*/
|
||||
virtual Value* clone_() const {
|
||||
void *place = boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
|
||||
ChartValue* ptr = new(place) ChartValue(*this); // calls copy constructor to fill in
|
||||
ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in
|
||||
return ptr;
|
||||
}
|
||||
|
||||
|
|
@ -75,7 +95,7 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
*/
|
||||
virtual void deallocate_() const {
|
||||
this->~ChartValue(); // Virtual destructor cleans up the derived object
|
||||
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*)this); // Release memory from pool
|
||||
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -85,18 +105,16 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
return boost::make_shared<ChartValue>(*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<T>::value(), delta);
|
||||
|
||||
// Create a Value pointer copy of the result
|
||||
void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
|
||||
Value* resultAsValue = new(resultAsValuePlace) ChartValue(retractResult, static_cast<const Chart&>(*this));
|
||||
void* resultAsValuePlace =
|
||||
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
|
||||
Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult,
|
||||
static_cast<const Chart&>(*this));
|
||||
|
||||
// Return the pointer to the Value base class
|
||||
return resultAsValue;
|
||||
|
|
@ -105,7 +123,8 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
/// Generic Value interface version of localCoordinates
|
||||
virtual Vector localCoordinates_(const Value& value2) const {
|
||||
// Cast the base class Value pointer to a templated generic class pointer
|
||||
const GenericValue<T>& genericValue2 = static_cast<const GenericValue<T>&>(value2);
|
||||
const GenericValue<T>& genericValue2 =
|
||||
static_cast<const GenericValue<T>&>(value2);
|
||||
|
||||
// Return the result of calling localCoordinates trait on the derived class
|
||||
return Chart::local(GenericValue<T>::value(), genericValue2.value());
|
||||
|
|
@ -113,7 +132,8 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
|
||||
/// Non-virtual version of retract
|
||||
ChartValue retract(const Vector& delta) const {
|
||||
return ChartValue(Chart::retract(GenericValue<T>::value(), delta),static_cast<const Chart&>(*this));
|
||||
return ChartValue(Chart::retract(GenericValue<T>::value(), delta),
|
||||
static_cast<const Chart&>(*this));
|
||||
}
|
||||
|
||||
/// Non-virtual version of localCoordinates
|
||||
|
|
@ -121,8 +141,10 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
return localCoordinates_(value2);
|
||||
}
|
||||
|
||||
/// Return run-time dimensionality
|
||||
virtual size_t dim() const {
|
||||
return Chart::getDimension(GenericValue<T>::value()); // need functional form here since the dimension may be dynamic
|
||||
// need functional form here since the dimension may be dynamic
|
||||
return Chart::getDimension(GenericValue<T>::value());
|
||||
}
|
||||
|
||||
/// Assignment operator
|
||||
|
|
@ -136,6 +158,7 @@ class ChartValue : public GenericValue<T>, public Chart_ {
|
|||
}
|
||||
|
||||
protected:
|
||||
|
||||
// implicit assignment operator for (const ChartValue& rhs) works fine here
|
||||
/// Assignment operator, protected because only the Value or DERIVED
|
||||
/// assignment operators should be used.
|
||||
|
|
@ -145,44 +168,52 @@ protected:
|
|||
// }
|
||||
|
||||
private:
|
||||
|
||||
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
|
||||
struct PoolTag { };
|
||||
struct PoolTag {
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
// ar & boost::serialization::make_nvp("value",);
|
||||
// todo: implement a serialization for charts
|
||||
ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object< GenericValue<T> >(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
// ar & boost::serialization::make_nvp("value",);
|
||||
// todo: implement a serialization for charts
|
||||
ar
|
||||
& boost::serialization::make_nvp("GenericValue",
|
||||
boost::serialization::base_object<GenericValue<T> >(*this));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
// Define
|
||||
namespace traits {
|
||||
template <typename T, typename Chart>
|
||||
struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {};
|
||||
}
|
||||
|
||||
/// The dimension of a ChartValue is the dimension of the chart
|
||||
template<typename T, typename Chart>
|
||||
struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {
|
||||
// TODO Frank thinks dimension is a property of type, chart should conform
|
||||
};
|
||||
|
||||
} // \ traits
|
||||
|
||||
/// Get the chart from a Value
|
||||
template<typename Chart>
|
||||
const Chart& Value::getChart() const {
|
||||
// define Value::cast here since now ChartValue has been declared
|
||||
return dynamic_cast<const Chart&>(*this);
|
||||
}
|
||||
return dynamic_cast<const Chart&>(*this);
|
||||
}
|
||||
|
||||
/// Convenience function that can be used to make an expression to convert a value to a chart
|
||||
template <typename T>
|
||||
ChartValue<T> convertToChartValue(const T& value, boost::optional<Eigen::Matrix<double, traits::dimension<T>::value, traits::dimension<T>::value >& > H=boost::none) {
|
||||
template<typename T>
|
||||
ChartValue<T> convertToChartValue(const T& value,
|
||||
boost::optional<
|
||||
Eigen::Matrix<double, traits::dimension<T>::value,
|
||||
traits::dimension<T>::value>&> H = boost::none) {
|
||||
if (H) {
|
||||
*H = Eigen::Matrix<double, traits::dimension<T>::value, traits::dimension<T>::value >::Identity();
|
||||
*H = Eigen::Matrix<double, traits::dimension<T>::value,
|
||||
traits::dimension<T>::value>::Identity();
|
||||
}
|
||||
return ChartValue<T>(value);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,9 +11,10 @@
|
|||
|
||||
/*
|
||||
* @file GenericValue.h
|
||||
* @date Jan 26, 2012
|
||||
* @brief Wraps any type T so it can play as a Value
|
||||
* @date October, 2014
|
||||
* @author Michael Bosse, Abel Gawel, Renaud Dube
|
||||
* based on DrivedValue.h by Duy Nguyen Ta
|
||||
* based on DerivedValue.h by Duy Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -22,14 +23,16 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// To play as a GenericValue, we need the following traits
|
||||
namespace traits {
|
||||
|
||||
// trait to wrap the default equals of types
|
||||
template<typename T>
|
||||
bool equals(const T& a, const T& b, double tol) {
|
||||
return a.equals(b,tol);
|
||||
}
|
||||
bool equals(const T& a, const T& b, double tol) {
|
||||
return a.equals(b, tol);
|
||||
}
|
||||
|
||||
// trait to wrap the default print of types
|
||||
template<typename T>
|
||||
void print(const T& obj, const std::string& str) {
|
||||
obj.print(str);
|
||||
|
|
@ -37,20 +40,40 @@ void print(const T& obj, const std::string& str) {
|
|||
|
||||
}
|
||||
|
||||
/**
|
||||
* Wraps any type T so it can play as a Value
|
||||
*/
|
||||
template<class T>
|
||||
class GenericValue : public Value {
|
||||
class GenericValue: public Value {
|
||||
|
||||
public:
|
||||
|
||||
typedef T type;
|
||||
|
||||
protected:
|
||||
T value_;
|
||||
|
||||
T value_; ///< The wrapped value
|
||||
|
||||
public:
|
||||
GenericValue(const T& value) : value_(value) {}
|
||||
|
||||
const T& value() const { return value_; }
|
||||
T& value() { return value_; }
|
||||
/// Construct from value
|
||||
GenericValue(const T& value) :
|
||||
value_(value) {
|
||||
}
|
||||
|
||||
virtual ~GenericValue() {}
|
||||
/// Return a constant value
|
||||
const T& value() const {
|
||||
return value_;
|
||||
}
|
||||
|
||||
/// Return the value
|
||||
T& value() {
|
||||
return value_;
|
||||
}
|
||||
|
||||
/// Destructor
|
||||
virtual ~GenericValue() {
|
||||
}
|
||||
|
||||
/// equals implementing generic Value interface
|
||||
virtual bool equals_(const Value& p, double tol = 1e-9) const {
|
||||
|
|
@ -60,30 +83,35 @@ public:
|
|||
return traits::equals<T>(this->value_, genericValue2.value_, tol);
|
||||
}
|
||||
|
||||
// non virtual equals function
|
||||
/// non virtual equals function, uses traits
|
||||
bool equals(const GenericValue &other, double tol = 1e-9) const {
|
||||
return traits::equals<T>(this->value(),other.value(),tol);
|
||||
return traits::equals<T>(this->value(), other.value(), tol);
|
||||
}
|
||||
|
||||
/// Virtual print function, uses traits
|
||||
virtual void print(const std::string& str) const {
|
||||
traits::print<T>(value_,str);
|
||||
traits::print<T>(value_, str);
|
||||
}
|
||||
|
||||
// Serialization below:
|
||||
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
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<T> is an abstract class
|
||||
|
||||
// Assignment operator for this class not needed since GenericValue<T> is an abstract class
|
||||
|
||||
};
|
||||
|
||||
// define Value::cast here since now GenericValue has been declared
|
||||
template<typename ValueType>
|
||||
const ValueType& Value::cast() const {
|
||||
return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
|
||||
}
|
||||
|
||||
const ValueType& Value::cast() const {
|
||||
return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
|
|||
|
|
@ -67,9 +67,12 @@ struct is_manifold: public boost::false_type {
|
|||
};
|
||||
|
||||
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
|
||||
// defaults to dynamic, TODO makes sense ?
|
||||
typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
|
||||
template<typename T>
|
||||
struct dimension : public Dynamic {}; //default to dynamic
|
||||
struct dimension: public Dynamic {
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* zero<T>::value is intended to be the origin of a canonical coordinate system
|
||||
|
|
@ -140,7 +143,8 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public boost::integral_cons
|
|||
}
|
||||
};
|
||||
|
||||
template <typename T> struct is_chart : public boost::false_type {};
|
||||
template<typename T> struct is_chart: public boost::false_type {
|
||||
};
|
||||
|
||||
} // \ namespace traits
|
||||
|
||||
|
|
@ -164,13 +168,15 @@ struct DefaultChart {
|
|||
|
||||
namespace traits {
|
||||
// populate default traits
|
||||
template <typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {};
|
||||
template <typename T> struct dimension<DefaultChart<T> > : public dimension<T> {};
|
||||
template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
|
||||
};
|
||||
template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
|
||||
};
|
||||
}
|
||||
|
||||
template<class C>
|
||||
struct ChartConcept {
|
||||
public:
|
||||
public:
|
||||
typedef typename C::type type;
|
||||
typedef typename C::vector vector;
|
||||
|
||||
|
|
@ -192,20 +198,19 @@ struct ChartConcept {
|
|||
dim_ = C::getDimension(val_);
|
||||
}
|
||||
|
||||
private:
|
||||
private:
|
||||
type val_;
|
||||
vector vec_;
|
||||
int dim_;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* CanonicalChart<Chart<T> > is a chart around zero<T>::value
|
||||
* Canonical<T> is CanonicalChart<DefaultChart<T> >
|
||||
* An example is Canonical<Rot3>
|
||||
*/
|
||||
template<typename C> struct CanonicalChart {
|
||||
BOOST_CONCEPT_ASSERT((ChartConcept<C>));
|
||||
BOOST_CONCEPT_ASSERT((ChartConcept<C>));
|
||||
|
||||
typedef C Chart;
|
||||
typedef typename Chart::type type;
|
||||
|
|
@ -220,7 +225,8 @@ template<typename C> struct CanonicalChart {
|
|||
return Chart::retract(traits::zero<type>::value(), v);
|
||||
}
|
||||
};
|
||||
template <typename T> struct Canonical : public CanonicalChart<DefaultChart<T> > {};
|
||||
template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
|
||||
};
|
||||
|
||||
// double
|
||||
|
||||
|
|
@ -248,8 +254,7 @@ template<int M, int N, int Options>
|
|||
struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
|
||||
typedef Eigen::Matrix<double, M, N, Options> type;
|
||||
typedef type T;
|
||||
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
|
||||
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
|
||||
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
|
||||
"DefaultChart has not been implemented yet for dynamically sized matrices");
|
||||
static vector local(const T& origin, const T& other) {
|
||||
T diff = other - origin;
|
||||
|
|
@ -262,7 +267,7 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
|
|||
return origin + map;
|
||||
}
|
||||
static int getDimension(const T&origin) {
|
||||
return origin.rows()*origin.cols();
|
||||
return origin.rows() * origin.cols();
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -124,6 +124,7 @@ namespace gtsam {
|
|||
//needs a empty definition so recursion in implicit derived assignment operators work
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Cast to known ValueType */
|
||||
template<typename ValueType>
|
||||
const ValueType& cast() const;
|
||||
|
|
|
|||
|
|
@ -300,6 +300,7 @@ namespace gtsam {
|
|||
void Values::insert(Key j, const ValueType& val) {
|
||||
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
|
||||
}
|
||||
|
||||
// overloaded insert with chart initializer
|
||||
template<typename ValueType, typename Chart, typename ChartInit>
|
||||
void Values::insert(Key j, const ValueType& val, ChartInit chart) {
|
||||
|
|
@ -311,11 +312,13 @@ namespace gtsam {
|
|||
void Values::update(Key j, const ValueType& val) {
|
||||
update(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val)));
|
||||
}
|
||||
|
||||
// update with custom chart
|
||||
template <typename ValueType, typename Chart>
|
||||
void Values::update(Key j, const ValueType& val) {
|
||||
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
|
||||
}
|
||||
|
||||
// update with chart initializer, /todo: perhaps there is a way to init chart from old value...
|
||||
template<typename ValueType, typename Chart, typename ChartInit>
|
||||
void Values::update(Key j, const ValueType& val, ChartInit chart) {
|
||||
|
|
|
|||
|
|
@ -251,15 +251,18 @@ namespace gtsam {
|
|||
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
|
||||
void insert(const Values& values);
|
||||
|
||||
/** Templated verion to add a variable with the given j,
|
||||
/** Templated version to add a variable with the given j,
|
||||
* throws KeyAlreadyExists<J> if j is already present
|
||||
* if no chart is specified, the DefaultChart<ValueType> is used
|
||||
*/
|
||||
template <typename ValueType>
|
||||
void insert(Key j, const ValueType& val);
|
||||
|
||||
/// overloaded insert version that also specifies a chart
|
||||
template <typename ValueType, typename Chart>
|
||||
void insert(Key j, const ValueType& val);
|
||||
// overloaded insert version that also specifies a chart initializer
|
||||
|
||||
/// overloaded insert version that also specifies a chart initializer
|
||||
template <typename ValueType, typename Chart, typename ChartInit>
|
||||
void insert(Key j, const ValueType& val, ChartInit chart);
|
||||
|
||||
|
|
@ -273,14 +276,18 @@ namespace gtsam {
|
|||
/** single element change of existing element */
|
||||
void update(Key j, const Value& val);
|
||||
|
||||
/** Templated verion to update a variable with the given j,
|
||||
/** Templated version to update a variable with the given j,
|
||||
* throws KeyAlreadyExists<J> if j is already present
|
||||
* if no chart is specified, the DefaultChart<ValueType> is used
|
||||
*/
|
||||
template <typename T>
|
||||
void update(Key j, const T& val);
|
||||
|
||||
/// overloaded insert version that also specifies a chart
|
||||
template <typename T, typename Chart>
|
||||
void update(Key j, const T& val);
|
||||
|
||||
/// overloaded insert version that also specifies a chart initializer
|
||||
template <typename T, typename Chart, typename ChartInit>
|
||||
void update(Key j, const T& val, ChartInit chart);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue