223 lines
7.0 KiB
C
223 lines
7.0 KiB
C
///* ----------------------------------------------------------------------------
|
|
//
|
|
// * GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
// * Atlanta, Georgia 30332-0415
|
|
// * All Rights Reserved
|
|
// * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
//
|
|
// * See LICENSE for the license information
|
|
//
|
|
// * -------------------------------------------------------------------------- */
|
|
//
|
|
///*
|
|
// * @file ChartValue.h
|
|
// * @brief
|
|
// * @date October, 2014
|
|
// * @author Michael Bosse, Abel Gawel, Renaud Dube
|
|
// * based on DerivedValue.h by Duy Nguyen Ta
|
|
// */
|
|
//
|
|
#pragma once
|
|
//
|
|
#include <gtsam/base/concepts.h>
|
|
#include <gtsam/base/GenericValue.h>
|
|
//#include <gtsam/base/Manifold.h>
|
|
//#include <boost/make_shared.hpp>
|
|
//
|
|
////////////////////
|
|
//// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
|
|
//#include <boost/pool/singleton_pool.hpp>
|
|
//
|
|
//#ifdef min
|
|
//#undef min
|
|
//#endif
|
|
//
|
|
//#ifdef max
|
|
//#undef max
|
|
//#endif
|
|
//
|
|
//#ifdef ERROR
|
|
//#undef ERROR
|
|
//#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 GenericValue: public GenericValue<T>, public Chart_ {
|
|
//
|
|
// BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
|
|
//
|
|
//public:
|
|
//
|
|
// typedef T type;
|
|
// typedef Chart_ Chart;
|
|
//
|
|
//public:
|
|
//
|
|
// /// Default Constructor. TODO might not make sense for some types
|
|
// GenericValue() :
|
|
// GenericValue<T>(T()) {
|
|
// }
|
|
//
|
|
// /// Construct froma value
|
|
// GenericValue(const T& value) :
|
|
// GenericValue<T>(value) {
|
|
// }
|
|
//
|
|
// /// Construct from a value and initialize the chart
|
|
// template<typename C>
|
|
// GenericValue(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.
|
|
// * 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<PoolTag, sizeof(GenericValue)>::malloc();
|
|
// ChartValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
|
|
// return ptr;
|
|
// }
|
|
//
|
|
// /**
|
|
// * Destroy and deallocate this object, only if it was originally allocated using clone_().
|
|
// */
|
|
// virtual void deallocate_() const {
|
|
// this->~ChartValue(); // Virtual destructor cleans up the derived object
|
|
// boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
|
|
// }
|
|
//
|
|
// /**
|
|
// * Clone this value (normal clone on the heap, delete with 'delete' operator)
|
|
// */
|
|
// virtual boost::shared_ptr<Value> clone() const {
|
|
// return boost::make_shared<ChartValue>(*this);
|
|
// }
|
|
//
|
|
// /// Chart Value interface version of retract
|
|
// virtual Value* retract_(const Vector& delta) const {
|
|
// // Call retract on the derived class using the retract trait function
|
|
// const T retractResult = Chart::retract(GenericValue<T>::value(), delta);
|
|
//
|
|
// // Create a Value pointer copy of the result
|
|
// void* resultAsValuePlace =
|
|
// boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
|
|
// Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult,
|
|
// static_cast<const Chart&>(*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<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());
|
|
// }
|
|
//
|
|
// /// Non-virtual version of retract
|
|
// GenericValue retract(const Vector& delta) const {
|
|
// return GenericValue(Chart::retract(GenericValue<T>::value(), delta),
|
|
// static_cast<const Chart&>(*this));
|
|
// }
|
|
//
|
|
// /// Non-virtual version of localCoordinates
|
|
// Vector localCoordinates(const GenericValue& value2) const {
|
|
// return localCoordinates_(value2);
|
|
// }
|
|
//
|
|
// /// Return run-time dimensionality
|
|
// virtual size_t dim() const {
|
|
// // need functional form here since the dimension may be dynamic
|
|
// return Chart::getDimension(GenericValue<T>::value());
|
|
// }
|
|
//
|
|
// /// Assignment operator
|
|
// virtual Value& operator=(const Value& rhs) {
|
|
// // Cast the base class Value pointer to a derived class pointer
|
|
// const GenericValue& derivedRhs = static_cast<const ChartValue&>(rhs);
|
|
//
|
|
// // Do the assignment and return the result
|
|
// *this = GenericValue(derivedRhs); // calls copy constructor
|
|
// return *this;
|
|
// }
|
|
//
|
|
//protected:
|
|
//
|
|
// // implicit assignment operator for (const ChartValue& rhs) works fine here
|
|
// /// Assignment operator, protected because only the Value or DERIVED
|
|
// /// assignment operators should be used.
|
|
// // DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
|
|
// // // Nothing to do, do not call base class assignment operator
|
|
// // return *this;
|
|
// // }
|
|
//
|
|
//private:
|
|
//
|
|
// /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
|
|
// struct PoolTag {
|
|
// };
|
|
//
|
|
//private:
|
|
//
|
|
// /** Serialization function */
|
|
// friend class boost::serialization::access;
|
|
// template<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 {
|
|
//
|
|
///// The dimension of a GenericValue is the dimension of the chart
|
|
//template<typename T, typename Chart>
|
|
//struct dimension<GenericValue<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 {
|
|
// 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>
|
|
//GenericValue<T> convertToChartValue(const T& value,
|
|
// boost::optional<
|
|
// Eigen::Matrix<double, traits<T>::dimension,
|
|
// traits<T>::dimension>&> H = boost::none) {
|
|
// if (H) {
|
|
// *H = Eigen::Matrix<double, traits<T>::dimension,
|
|
// traits<T>::dimension>::Identity();
|
|
// }
|
|
// return GenericValue<T>(value);
|
|
//}
|
|
//
|
|
//} /* namespace gtsam */
|