Replaced ChartValue with GenericValue. Now at the point where we are bumping up against missing Jacobians in gtsam types. It feels like progress.
Paul Furgale
parent
6b04fee048
commit
7f7c181945
|
|
@ -1,222 +1,222 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file ChartValue.h
|
||||
* @brief
|
||||
* @date October, 2014
|
||||
* @author Michael Bosse, Abel Gawel, Renaud Dube
|
||||
* based on DerivedValue.h by Duy Nguyen Ta
|
||||
*/
|
||||
|
||||
///* ----------------------------------------------------------------------------
|
||||
//
|
||||
// * GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
// * Atlanta, Georgia 30332-0415
|
||||
// * All Rights Reserved
|
||||
// * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
//
|
||||
// * See LICENSE for the license information
|
||||
//
|
||||
// * -------------------------------------------------------------------------- */
|
||||
//
|
||||
///*
|
||||
// * @file ChartValue.h
|
||||
// * @brief
|
||||
// * @date October, 2014
|
||||
// * @author Michael Bosse, Abel Gawel, Renaud Dube
|
||||
// * based on DerivedValue.h by Duy Nguyen Ta
|
||||
// */
|
||||
//
|
||||
#pragma once
|
||||
|
||||
//
|
||||
#include <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 ChartValue: 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
|
||||
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.
|
||||
* 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(ChartValue)>::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<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(ChartValue)>::malloc();
|
||||
Value* resultAsValue = new (resultAsValuePlace) ChartValue(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
|
||||
ChartValue retract(const Vector& delta) const {
|
||||
return ChartValue(Chart::retract(GenericValue<T>::value(), delta),
|
||||
static_cast<const Chart&>(*this));
|
||||
}
|
||||
|
||||
/// Non-virtual version of localCoordinates
|
||||
Vector localCoordinates(const ChartValue& value2) const {
|
||||
return localCoordinates_(value2);
|
||||
}
|
||||
|
||||
/// Return run-time dimensionality
|
||||
virtual size_t dim() const {
|
||||
// need functional form here since the dimension may be dynamic
|
||||
return Chart::getDimension(GenericValue<T>::value());
|
||||
}
|
||||
|
||||
/// Assignment operator
|
||||
virtual Value& operator=(const Value& rhs) {
|
||||
// Cast the base class Value pointer to a derived class pointer
|
||||
const ChartValue& derivedRhs = static_cast<const ChartValue&>(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<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 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 {
|
||||
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_x<T>::dimension,
|
||||
traits_x<T>::dimension>&> H = boost::none) {
|
||||
if (H) {
|
||||
*H = Eigen::Matrix<double, traits_x<T>::dimension,
|
||||
traits_x<T>::dimension>::Identity();
|
||||
}
|
||||
return ChartValue<T>(value);
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
//#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_x<T>::dimension,
|
||||
// traits_x<T>::dimension>&> H = boost::none) {
|
||||
// if (H) {
|
||||
// *H = Eigen::Matrix<double, traits_x<T>::dimension,
|
||||
// traits_x<T>::dimension>::Identity();
|
||||
// }
|
||||
// return GenericValue<T>(value);
|
||||
//}
|
||||
//
|
||||
//} /* namespace gtsam */
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
#include <gtsam/base/Value.h>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -144,18 +145,108 @@ public:
|
|||
traits::print<T>()(value_, str);
|
||||
}
|
||||
|
||||
// Serialization below:
|
||||
/**
|
||||
* Create a duplicate object returned as a pointer to the generic Value interface.
|
||||
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
|
||||
* The result must be deleted with Value::deallocate_, not with the 'delete' operator.
|
||||
*/
|
||||
virtual Value* clone_() const {
|
||||
void *place = boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
|
||||
GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
|
||||
return ptr;
|
||||
}
|
||||
|
||||
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_);
|
||||
}
|
||||
/**
|
||||
* 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<PoolTag, sizeof(GenericValue)>::free((void*) this); // Release memory from pool
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Clone this value (normal clone on the heap, delete with 'delete' operator)
|
||||
*/
|
||||
virtual boost::shared_ptr<Value> clone() const {
|
||||
return boost::make_shared<GenericValue>(*this);
|
||||
}
|
||||
|
||||
// Assignment operator for this class not needed since GenericValue<T> is an abstract class
|
||||
/// Generic Value interface version of retract
|
||||
virtual Value* retract_(const Vector& delta) const {
|
||||
// Call retract on the derived class using the retract trait function
|
||||
const T retractResult = traits_x<T>::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);
|
||||
|
||||
// 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 traits_x<T>::Local(GenericValue<T>::value(), genericValue2.value());
|
||||
}
|
||||
|
||||
/// Non-virtual version of retract
|
||||
GenericValue retract(const Vector& delta) const {
|
||||
return GenericValue(traits_x<T>::Retract(GenericValue<T>::value(), delta));
|
||||
}
|
||||
|
||||
/// Non-virtual version of localCoordinates
|
||||
Vector localCoordinates(const GenericValue& value2) const {
|
||||
return localCoordinates_(value2);
|
||||
}
|
||||
|
||||
/// Return run-time dimensionality
|
||||
virtual size_t dim() const {
|
||||
// need functional form here since the dimension may be dynamic
|
||||
return traits_x<T>::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 GenericValue&>(rhs);
|
||||
|
||||
// Do the assignment and return the result
|
||||
*this = GenericValue(derivedRhs); // calls copy constructor
|
||||
return *this;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
// implicit assignment operator for (const GenericValue& rhs) works fine here
|
||||
/// Assignment operator, protected because only the Value or DERIVED
|
||||
/// assignment operators should be used.
|
||||
// DerivedValue<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("GenericValue",
|
||||
boost::serialization::base_object<GenericValue<T> >(*this));
|
||||
ar & boost::serialization::make_nvp("value", value_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -357,6 +357,8 @@
|
|||
//
|
||||
//} // \ namespace gtsam
|
||||
//
|
||||
|
||||
// TODO(ASL) Remove these and fix the compiler errors.
|
||||
///**
|
||||
// * Macros for using the ManifoldConcept
|
||||
// * - An instantiation for use inside unit tests
|
||||
|
|
|
|||
|
|
@ -63,10 +63,10 @@ struct Manifold {
|
|||
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
|
||||
|
||||
// For Testable
|
||||
void Print(const ManifoldType& m) {
|
||||
m.print();
|
||||
static void Print(const ManifoldType& m, const std::string& str = "") {
|
||||
m.print(str);
|
||||
}
|
||||
bool Equals(const ManifoldType& m1,
|
||||
static bool Equals(const ManifoldType& m1,
|
||||
const ManifoldType& m2,
|
||||
double tol = 1e-8) {
|
||||
return m1.equals(m2, tol);
|
||||
|
|
@ -113,10 +113,10 @@ struct LieGroup {
|
|||
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
|
||||
|
||||
// For Testable
|
||||
void Print(const ManifoldType& m) {
|
||||
static void Print(const ManifoldType& m, const std::string& str = "") {
|
||||
m.print();
|
||||
}
|
||||
bool Equals(const ManifoldType& m1,
|
||||
static bool Equals(const ManifoldType& m1,
|
||||
const ManifoldType& m2,
|
||||
double tol = 1e-8) {
|
||||
return m1.equals(m2, tol);
|
||||
|
|
|
|||
|
|
@ -486,7 +486,8 @@ private:
|
|||
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits_x<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
|
||||
|
||||
template<typename Calibration>
|
||||
struct traits_x< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
|
||||
|
||||
} // \ gtsam
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
enum { dimension = 6 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -292,37 +292,13 @@ namespace gtsam {
|
|||
// insert a plain value using the default chart
|
||||
template<typename ValueType>
|
||||
void Values::insert(Key j, const ValueType& val) {
|
||||
insert(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val)));
|
||||
insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
|
||||
}
|
||||
|
||||
// insert with custom chart type
|
||||
template<typename ValueType, typename Chart>
|
||||
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>
|
||||
void Values::insert(Key j, const ValueType& val, Chart chart) {
|
||||
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
|
||||
}
|
||||
|
||||
// update with default chart
|
||||
template <typename ValueType>
|
||||
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>
|
||||
void Values::update(Key j, const ValueType& val, Chart chart) {
|
||||
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
|
||||
update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -76,7 +76,7 @@ TEST (Serialization, TemplatedValues) {
|
|||
std::cout << __LINE__ << std::endl;
|
||||
EXPECT(equalsObj(pt3));
|
||||
std::cout << __LINE__ << std::endl;
|
||||
ChartValue<Point3> chv1(pt3);
|
||||
GenericValue<Point3> chv1(pt3);
|
||||
std::cout << __LINE__ << std::endl;
|
||||
EXPECT(equalsObj(chv1));
|
||||
std::cout << __LINE__ << std::endl;
|
||||
|
|
|
|||
|
|
@ -43,8 +43,9 @@ namespace gtsam {
|
|||
VALUE measured_; /** The measurement */
|
||||
|
||||
/** concept check by type */
|
||||
GTSAM_CONCEPT_LIE_TYPE(T)
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(T)
|
||||
// TODO(ASL) Reenable
|
||||
//GTSAM_CONCEPT_LIE_TYPE(T)
|
||||
//GTSAM_CONCEPT_TESTABLE_TYPE(T)
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -74,14 +75,14 @@ namespace gtsam {
|
|||
std::cout << s << "BetweenFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ")\n";
|
||||
traits::print<T>()(measured_, " measured: ");
|
||||
traits_x<T>::Print(measured_, " measured: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol);
|
||||
return e != NULL && Base::equals(*e, tol) && traits_x<T>::Equals(this->measured_, e->measured_, tol);
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
|
@ -90,10 +91,13 @@ namespace gtsam {
|
|||
Vector evaluateError(const T& p1, const T& p2,
|
||||
boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
|
||||
boost::none) const {
|
||||
T hx = p1.between(p2, H1, H2); // h(x)
|
||||
DefaultChart<T> chart;
|
||||
T hx = traits_x<T>::Between(p1, p2, H1, H2); // h(x)
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return chart.local(measured_, hx);
|
||||
OptionalJacobian<traits_x<T>::dimension, traits_x<T>::dimension> Hlocal;
|
||||
Vector rval = traits_x<T>::Local(measured_, hx, boost::none, Hlocal);
|
||||
(*H1) = ((*Hlocal) * (*H1)).eval();
|
||||
(*H2) = ((*Hlocal) * (*H2)).eval();
|
||||
return rval;
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
|
|
@ -131,7 +135,7 @@ namespace gtsam {
|
|||
/** Syntactic sugar for constrained version */
|
||||
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
|
||||
BetweenFactor<VALUE>(key1, key2, measured,
|
||||
noiseModel::Constrained::All(DefaultChart<VALUE>::getDimension(measured), fabs(mu)))
|
||||
noiseModel::Constrained::All(traits_x<VALUE>::GetDimension(measured), fabs(mu)))
|
||||
{}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -67,24 +67,24 @@ namespace gtsam {
|
|||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
|
||||
traits::print<T>()(prior_, " prior mean: ");
|
||||
traits_x<T>::Print(prior_, " prior mean: ");
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
||||
const This* e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(prior_, e->prior_, tol);
|
||||
return e != NULL && Base::equals(*e, tol) && traits_x<T>::Equals(prior_, e->prior_, tol);
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
|
||||
DefaultChart<T> chart;
|
||||
if (H) (*H) = eye(chart.getDimension(p));
|
||||
if (H) (*H) = eye(traits_x<T>::GetDimension(p));
|
||||
// manifold equivalent of h(x)-z -> log(z,h(x))
|
||||
return chart.local(prior_,p);
|
||||
// TODO(ASL) Add Jacobians.
|
||||
return traits_x<T>::Local(prior_,p);
|
||||
}
|
||||
|
||||
const VALUE & prior() const { return prior_; }
|
||||
|
|
|
|||
Loading…
Reference in New Issue