12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/BAD_generic_value_traits (pull request #24) 

GenericValue to replace DerivedValue
release/4.3a0
Frank Dellaert 2014-11-03 00:09:08 +01:00
parent 17d352bab4 11416cac65
commit ff036ac05c
41 changed files with 796 additions and 302 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

190
gtsam/base/ChartValue.h Normal file
View File

@ -0,0 +1,190 @@
/* ----------------------------------------------------------------------------
* 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 <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:
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) {}
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);
}
/// 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));
// 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);
}
virtual size_t dim() const {
return Chart::getDimension(GenericValue<T>::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<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:
/// @}
/// @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));
}
/// @}
};
namespace traits {
template <typename T, typename Chart>
struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {};
}
template<typename Chart>
const Chart& Value::getChart() const {
// define Value::cast here since now ChartValue has been declared
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) {
if (H) {
*H = Eigen::Matrix<double, traits::dimension<T>::value, traits::dimension<T>::value >::Identity();
}
return ChartValue<T>(value);
}
} /* namespace gtsam */

89
gtsam/base/GenericValue.h Normal file
View File

@ -0,0 +1,89 @@
/* ----------------------------------------------------------------------------
* 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 GenericValue.h
* @date Jan 26, 2012
* @author Michael Bosse, Abel Gawel, Renaud Dube
* based on DrivedValue.h by Duy Nguyen Ta
*/
#pragma once
#include <gtsam/base/Value.h>
namespace gtsam {
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);
}
template<typename T>
void print(const T& obj, const std::string& str) {
obj.print(str);
}
}
template<class T>
class GenericValue : public Value {
public:
typedef T type;
protected:
T value_;
public:
GenericValue(const T& value) : value_(value) {}
const T& value() const { return value_; }
T& value() { return value_; }
virtual ~GenericValue() {}
/// 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 GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
// Return the result of using the equals traits for the derived class
return traits::equals<T>(this->value_, genericValue2.value_, tol);
}
// non virtual equals function
bool equals(const GenericValue &other, double tol = 1e-9) const {
return traits::equals<T>(this->value(),other.value(),tol);
}
virtual void print(const std::string& str) const {
traits::print<T>(value_,str);
}
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
};
// 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();
}
} /* namespace gtsam */

6
gtsam/base/LieMatrix.h
View File

@ -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<LieMatrix> {
struct LieMatrix : public Matrix {
/// @name Constructors
/// @{
@ -166,8 +166,6 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LieMatrix",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Matrix",
boost::serialization::base_object<Matrix>(*this));
@ -179,7 +177,7 @@ private:
namespace traits {
template<>
struct is_manifold<LieMatrix> : public std::true_type {
struct is_manifold<LieMatrix> : public boost::true_type {
};
template<>

8
gtsam/base/LieScalar.h
View File

@ -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<LieScalar> {
struct GTSAM_EXPORT LieScalar {
/** default constructor */
LieScalar() : d_(0.0) {}
@ -116,15 +116,15 @@ namespace gtsam {
namespace traits {
template<>
struct is_group<LieScalar> : public std::true_type {
struct is_group<LieScalar> : public boost::true_type {
};
template<>
struct is_manifold<LieScalar> : public std::true_type {
struct is_manifold<LieScalar> : public boost::true_type {
};
template<>
struct dimension<LieScalar> : public std::integral_constant<int, 1> {
struct dimension<LieScalar> : public boost::integral_constant<int, 1> {
};
}

6
gtsam/base/LieVector.h
View File

@ -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<LieVector> {
struct LieVector : public Vector {
/** default constructor - should be unnecessary */
LieVector() {}
@ -123,8 +123,6 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LieVector",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this));
}
@ -134,7 +132,7 @@ private:
namespace traits {
template<>
struct is_manifold<LieVector> : public std::true_type {
struct is_manifold<LieVector> : public boost::true_type {
};
template<>

159
gtsam/base/Manifold.h
View File

@ -20,7 +20,7 @@
#include <gtsam/base/Matrix.h>
#include <boost/static_assert.hpp>
#include <type_traits>
#include <boost/type_traits.hpp>
#include <string>
namespace gtsam {
@ -50,7 +50,7 @@ namespace traits {
// is group, by default this is false
template<typename T>
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,16 +63,17 @@ struct identity {
// is manifold, by default this is false
template<typename T>
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 boost::integral_constant<int, Eigen::Dynamic> Dynamic;
template<typename T>
struct dimension;
struct dimension : public Dynamic {}; //default to dynamic
/**
* zero<T>::value is intended to be the origin of a canonical coordinate system
* with canonical(t) == DefaultChart<T>(zero<T>::value).apply(t)
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
* Below we provide the group identity as zero *in case* it is a group
*/
template<typename T> struct zero: public identity<T> {
@ -82,15 +83,15 @@ template<typename T> struct zero: public identity<T> {
// double
template<>
struct is_group<double> : public std::true_type {
struct is_group<double> : public boost::true_type {
};
template<>
struct is_manifold<double> : public std::true_type {
struct is_manifold<double> : public boost::true_type {
};
template<>
struct dimension<double> : public std::integral_constant<int, 1> {
struct dimension<double> : public boost::integral_constant<int, 1> {
};
template<>
@ -103,17 +104,15 @@ struct zero<double> {
// Fixed size Eigen::Matrix type
template<int M, int N, int Options>
struct is_group<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
struct is_group<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
};
template<int M, int N, int Options>
struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
};
// TODO: Could be more sophisticated using Eigen traits and SFINAE?
typedef std::integral_constant<int, Eigen::Dynamic> Dynamic;
template<int Options>
struct dimension<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Options> > : public Dynamic {
};
@ -127,12 +126,12 @@ struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dy
};
template<int M, int N, int Options>
struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
struct dimension<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<
int, M * N> {
};
template<int M, int N, int Options>
struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
struct zero<Eigen::Matrix<double, M, N, Options> > : 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");
@ -141,62 +140,105 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_consta
}
};
template <typename T> struct is_chart : public boost::false_type {};
} // \ namespace traits
// Chart is a map from T -> vector, retract is its inverse
template<typename T>
struct DefaultChart {
BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
//BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
typedef T type;
typedef Eigen::Matrix<double, traits::dimension<T>::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 {
// 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<class C>
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<C>::value));
/**
* Canonical<T>::value is a chart around zero<T>::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_;
};
/**
* CanonicalChart<Chart<T> > is a chart around zero<T>::value
* Canonical<T> is CanonicalChart<DefaultChart<T> >
* An example is Canonical<Rot3>
*/
template<typename T> struct Canonical {
typedef T type;
typedef typename DefaultChart<T>::vector vector;
DefaultChart<T> chart;
Canonical() :
chart(traits::zero<T>::value()) {
}
template<typename C> struct CanonicalChart {
BOOST_CONCEPT_ASSERT((ChartConcept<C>));
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<type>::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<type>::value(), v);
}
};
template <typename T> struct Canonical : public CanonicalChart<DefaultChart<T> > {};
// double
template<>
struct DefaultChart<double> {
typedef double type;
typedef Eigen::Matrix<double, 1, 1> 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 +246,23 @@ struct DefaultChart<double> {
template<int M, int N, int Options>
struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> T;
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),
"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<vector> 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<const T> map(d.data());
return t_ + map;
return origin + map;
}
static int getDimension(const T&origin) {
return origin.rows()*origin.cols();
}
};
@ -227,16 +270,16 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
template<>
struct DefaultChart<Vector> {
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();
}
};

11
gtsam/base/Value.h
View File

@ -120,7 +120,16 @@ 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<typename ValueType>
const ValueType& cast() const;
template<typename Chart>
const Chart& getChart() const;
/** Virutal destructor */
virtual ~Value() {}

16
gtsam/base/numericalDerivative.h
View File

@ -73,7 +73,7 @@ Vector numericalGradient(boost::function<double(const X&)> 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<double(const X&)> 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<Y(const X&)> 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<Y(const X&)> 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;
}

9
gtsam/geometry/Cal3Bundler.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Bundler: public DerivedValue<Cal3Bundler> {
class GTSAM_EXPORT Cal3Bundler {
private:
double f_; ///< focal length
@ -172,9 +172,6 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3Bundler",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(f_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
@ -190,11 +187,11 @@ private:
namespace traits {
template<>
struct is_manifold<Cal3Bundler> : public std::true_type {
struct is_manifold<Cal3Bundler> : public boost::true_type {
};
template<>
struct dimension<Cal3Bundler> : public std::integral_constant<int, 3> {
struct dimension<Cal3Bundler> : public boost::integral_constant<int, 3> {
};
template<>

8
gtsam/geometry/Cal3DS2.h
View File

@ -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<Cal3DS2> {
class GTSAM_EXPORT Cal3DS2 {
protected:
@ -153,8 +153,6 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
@ -172,11 +170,11 @@ private:
namespace traits {
template<>
struct is_manifold<Cal3DS2> : public std::true_type {
struct is_manifold<Cal3DS2> : public boost::true_type {
};
template<>
struct dimension<Cal3DS2> : public std::integral_constant<int, 9> {
struct dimension<Cal3DS2> : public boost::integral_constant<int, 9> {
};
}

6
gtsam/geometry/Cal3Unified.h
View File

@ -131,8 +131,6 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
}
@ -142,11 +140,11 @@ private:
namespace traits {
template<>
struct is_manifold<Cal3Unified> : public std::true_type {
struct is_manifold<Cal3Unified> : public boost::true_type {
};
template<>
struct dimension<Cal3Unified> : public std::integral_constant<int, 10> {
struct dimension<Cal3Unified> : public boost::integral_constant<int, 10> {
};
template<>

9
gtsam/geometry/Cal3_S2.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2: public DerivedValue<Cal3_S2> {
class GTSAM_EXPORT Cal3_S2 {
private:
double fx_, fy_, s_, u0_, v0_;
@ -226,9 +226,6 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
@ -244,11 +241,11 @@ private:
namespace traits {
template<>
struct is_manifold<Cal3_S2> : public std::true_type {
struct is_manifold<Cal3_S2> : public boost::true_type {
};
template<>
struct dimension<Cal3_S2> : public std::integral_constant<int, 5> {
struct dimension<Cal3_S2> : public boost::integral_constant<int, 5> {
};
template<>

26
gtsam/geometry/CalibratedCamera.h
View File

@ -39,7 +39,7 @@ public:
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT CalibratedCamera: public DerivedValue<CalibratedCamera> {
class GTSAM_EXPORT CalibratedCamera {
private:
Pose3 pose_; // 6DOF pose
@ -214,12 +214,28 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("CalibratedCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(pose_);
}
/// @}
};}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<CalibratedCamera> : public boost::true_type {
};
template<>
struct is_manifold<CalibratedCamera> : public boost::true_type {
};
template<>
struct dimension<CalibratedCamera> : public boost::integral_constant<int, 6> {
};
}
}

8
gtsam/geometry/EssentialMatrix.h
View File

@ -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<EssentialMatrix> {
class GTSAM_EXPORT EssentialMatrix {
private:
@ -176,8 +176,6 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("EssentialMatrix",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(aRb_);
ar & BOOST_SERIALIZATION_NVP(aTb_);
@ -200,11 +198,11 @@ private:
namespace traits {
template<>
struct is_manifold<EssentialMatrix> : public std::true_type {
struct is_manifold<EssentialMatrix> : public boost::true_type {
};
template<>
struct dimension<EssentialMatrix> : public std::integral_constant<int, 5> {
struct dimension<EssentialMatrix> : public boost::integral_constant<int, 5> {
};
template<>

7
gtsam/geometry/PinholeCamera.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
* \nosubgrouping
*/
template<typename Calibration>
class PinholeCamera: public DerivedValue<PinholeCamera<Calibration> > {
class PinholeCamera {
private:
Pose3 pose_;
Calibration K_;
@ -653,7 +653,6 @@ private:
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(pose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
@ -664,11 +663,11 @@ private:
namespace traits {
template<typename Calibration>
struct is_manifold<PinholeCamera<Calibration> > : public std::true_type {
struct is_manifold<PinholeCamera<Calibration> > : public boost::true_type {
};
template<typename Calibration>
struct dimension<PinholeCamera<Calibration> > : public std::integral_constant<
struct dimension<PinholeCamera<Calibration> > : public boost::integral_constant<
int, dimension<Pose3>::value + dimension<Calibration>::value> {
};

10
gtsam/geometry/Point2.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 : public DerivedValue<Point2> {
class GTSAM_EXPORT Point2 {
private:
@ -237,8 +237,6 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Point2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
}
@ -254,15 +252,15 @@ inline Point2 operator*(double s, const Point2& p) {return p*s;}
namespace traits {
template<>
struct is_group<Point2> : public std::true_type {
struct is_group<Point2> : public boost::true_type {
};
template<>
struct is_manifold<Point2> : public std::true_type {
struct is_manifold<Point2> : public boost::true_type {
};
template<>
struct dimension<Point2> : public std::integral_constant<int, 2> {
struct dimension<Point2> : public boost::integral_constant<int, 2> {
};
}

10
gtsam/geometry/Point3.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 : public DerivedValue<Point3> {
class GTSAM_EXPORT Point3 {
private:
@ -228,8 +228,6 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Point3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);
@ -246,15 +244,15 @@ namespace gtsam {
namespace traits {
template<>
struct is_group<Point3> : public std::true_type {
struct is_group<Point3> : public boost::true_type {
};
template<>
struct is_manifold<Point3> : public std::true_type {
struct is_manifold<Point3> : public boost::true_type {
};
template<>
struct dimension<Point3> : public std::integral_constant<int, 3> {
struct dimension<Point3> : public boost::integral_constant<int, 3> {
};
}

8
gtsam/geometry/Pose2.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Pose2 : public DerivedValue<Pose2> {
class GTSAM_EXPORT Pose2 {
public:
@ -301,8 +301,6 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Pose2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(r_);
}
@ -325,11 +323,11 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
namespace traits {
template<>
struct is_manifold<Pose2> : public std::true_type {
struct is_manifold<Pose2> : public boost::true_type {
};
template<>
struct dimension<Pose2> : public std::integral_constant<int, 3> {
struct dimension<Pose2> : public boost::integral_constant<int, 3> {
};
}

10
gtsam/geometry/Pose3.h
View File

@ -39,7 +39,7 @@ class Pose2;
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
class GTSAM_EXPORT Pose3{
public:
/** Pose Concept requirements */
@ -326,8 +326,6 @@ public:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Pose3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
@ -358,15 +356,15 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
namespace traits {
template<>
struct is_group<Pose3> : public std::true_type {
struct is_group<Pose3> : public boost::true_type {
};
template<>
struct is_manifold<Pose3> : public std::true_type {
struct is_manifold<Pose3> : public boost::true_type {
};
template<>
struct dimension<Pose3> : public std::integral_constant<int, 6> {
struct dimension<Pose3> : public boost::integral_constant<int, 6> {
};
}

10
gtsam/geometry/Rot2.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot2 : public DerivedValue<Rot2> {
class GTSAM_EXPORT Rot2 {
public:
/** get the dimension by the type */
@ -235,8 +235,6 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Rot2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(s_);
}
@ -247,15 +245,15 @@ namespace gtsam {
namespace traits {
template<>
struct is_group<Rot2> : public std::true_type {
struct is_group<Rot2> : public boost::true_type {
};
template<>
struct is_manifold<Rot2> : public std::true_type {
struct is_manifold<Rot2> : public boost::true_type {
};
template<>
struct dimension<Rot2> : public std::integral_constant<int, 1> {
struct dimension<Rot2> : public boost::integral_constant<int, 1> {
};
}

10
gtsam/geometry/Rot3.h
View File

@ -58,7 +58,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
class GTSAM_EXPORT Rot3 {
private:
@ -456,8 +456,6 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Rot3",
boost::serialization::base_object<Value>(*this));
#ifndef GTSAM_USE_QUATERNIONS
ar & boost::serialization::make_nvp("rot11", rot_(0,0));
ar & boost::serialization::make_nvp("rot12", rot_(0,1));
@ -494,15 +492,15 @@ namespace gtsam {
namespace traits {
template<>
struct is_group<Rot3> : public std::true_type {
struct is_group<Rot3> : public boost::true_type {
};
template<>
struct is_manifold<Rot3> : public std::true_type {
struct is_manifold<Rot3> : public boost::true_type {
};
template<>
struct dimension<Rot3> : public std::integral_constant<int, 3> {
struct dimension<Rot3> : public boost::integral_constant<int, 3> {
};
}

8
gtsam/geometry/StereoCamera.h
View File

@ -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<StereoCamera> {
class GTSAM_EXPORT StereoCamera {
private:
Pose3 leftCamPose_;
@ -147,8 +147,6 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("StereoCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
@ -159,11 +157,11 @@ private:
namespace traits {
template<>
struct is_manifold<StereoCamera> : public std::true_type {
struct is_manifold<StereoCamera> : public boost::true_type {
};
template<>
struct dimension<StereoCamera> : public std::integral_constant<int, 6> {
struct dimension<StereoCamera> : public boost::integral_constant<int, 6> {
};
template<>

10
gtsam/geometry/StereoPoint2.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT StereoPoint2 : public DerivedValue<StereoPoint2> {
class GTSAM_EXPORT StereoPoint2 {
public:
static const size_t dimension = 3;
private:
@ -162,8 +162,6 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("StereoPoint2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(uL_);
ar & BOOST_SERIALIZATION_NVP(uR_);
ar & BOOST_SERIALIZATION_NVP(v_);
@ -177,15 +175,15 @@ namespace gtsam {
namespace traits {
template<>
struct is_group<StereoPoint2> : public std::true_type {
struct is_group<StereoPoint2> : public boost::true_type {
};
template<>
struct is_manifold<StereoPoint2> : public std::true_type {
struct is_manifold<StereoPoint2> : public boost::true_type {
};
template<>
struct dimension<StereoPoint2> : public std::integral_constant<int, 3> {
struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
};
}

8
gtsam/geometry/Unit3.h
View File

@ -27,7 +27,7 @@
namespace gtsam {
/// Represents a 3D point on a unit sphere.
class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {
class GTSAM_EXPORT Unit3{
private:
@ -146,8 +146,6 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Unit3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(B_);
}
@ -160,11 +158,11 @@ private:
namespace traits {
template<>
struct is_manifold<Unit3> : public std::true_type {
struct is_manifold<Unit3> : public boost::true_type {
};
template<>
struct dimension<Unit3> : public std::integral_constant<int, 2> {
struct dimension<Unit3> : public boost::integral_constant<int, 2> {
};
template<>

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -142,7 +142,7 @@ TEST( Rot3, retract)
// test Canonical coordinates
Canonical<Rot3> chart;
Vector v2 = chart.apply(R);
Vector v2 = chart.local(R);
CHECK(assert_equal(R, chart.retract(v2)));
}

11
gtsam/navigation/ImuBias.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
/// All bias models live in the imuBias namespace
namespace imuBias {
class ConstantBias : public DerivedValue<ConstantBias> {
class ConstantBias {
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
@ -205,8 +205,7 @@ namespace imuBias {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("imuBias::ConstantBias",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this);
ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_);
}
@ -222,15 +221,15 @@ namespace imuBias {
namespace traits {
template<>
struct is_group<imuBias::ConstantBias> : public std::true_type {
struct is_group<imuBias::ConstantBias> : public boost::true_type {
};
template<>
struct is_manifold<imuBias::ConstantBias> : public std::true_type {
struct is_manifold<imuBias::ConstantBias> : public boost::true_type {
};
template<>
struct dimension<imuBias::ConstantBias> : public std::integral_constant<int, 6> {
struct dimension<imuBias::ConstantBias> : public boost::integral_constant<int, 6> {
};
}

102
gtsam/nonlinear/Values-inl.h
View File

@ -33,6 +33,7 @@
namespace gtsam {
/* ************************************************************************* */
template<class ValueType>
struct _ValuesKeyValuePair {
@ -52,6 +53,36 @@ namespace gtsam {
_ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& 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<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
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<GenericValue<ValueType>&>(static_cast<const GenericValue<ValueType>&>(key_value.value)).value());
}
};
// partial specialized version for ValueType == Value
template<class CastedKeyValuePairType, class KeyValuePairType>
struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
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<class CastedKeyValuePairType, class KeyValuePairType>
struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
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 ValueType>
class Values::Filtered {
@ -99,19 +130,19 @@ namespace gtsam {
begin_(boost::make_transform_iterator(
boost::make_filter_iterator(
filter, values.begin(), values.end()),
&castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)),
&ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
end_(boost::make_transform_iterator(
boost::make_filter_iterator(
filter, values.end(), values.end()),
&castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)),
&ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
constBegin_(boost::make_transform_iterator(
boost::make_filter_iterator(
filter, ((const Values&)values).begin(), ((const Values&)values).end()),
&castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)),
&ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)),
constEnd_(boost::make_transform_iterator(
boost::make_filter_iterator(
filter, ((const Values&)values).end(), ((const Values&)values).end()),
&castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)) {}
&ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)) {}
friend class Values;
iterator begin_;
@ -175,7 +206,7 @@ namespace gtsam {
Values::Values(const Values::Filtered<ValueType>& view) {
BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) {
Key key = key_value.key;
insert(key, key_value.value);
insert(key, static_cast<const ValueType&>(key_value.value));
}
}
@ -184,7 +215,7 @@ namespace gtsam {
Values::Values(const Values::ConstFiltered<ValueType>& view) {
BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
Key key = key_value.key;
insert(key, key_value.value);
insert<ValueType>(key, key_value.value);
}
}
@ -214,6 +245,13 @@ namespace gtsam {
return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
}
/* ************************************************************************* */
template<>
inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
// Filter and check the type
return filter(key_value.key);
}
/* ************************************************************************* */
template<typename ValueType>
const ValueType& Values::at(Key j) const {
@ -225,11 +263,11 @@ namespace gtsam {
throw ValuesKeyDoesNotExist("retrieve", j);
// Check the type and throw exception if incorrect
if(typeid(*item->second) != typeid(ValueType))
try {
return dynamic_cast<const GenericValue<ValueType>&>(*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<const ValueType&>(*item->second);
}
}
/* ************************************************************************* */
@ -239,15 +277,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(ValueType))
// dynamic cast the type and throw exception if incorrect
try {
return dynamic_cast<const GenericValue<ValueType>&>(*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<const ValueType&>(*item->second);
}
} else {
return boost::none;
}
}
/* ************************************************************************* */
// 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 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, typename ChartInit>
void Values::insert(Key j, const ValueType& val, ChartInit 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, typename ChartInit>
void Values::update(Key j, const ValueType& val, ChartInit chart) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}
}

36
gtsam/nonlinear/Values.h
View File

@ -44,7 +44,7 @@
#include <string>
#include <utility>
#include <gtsam/base/Value.h>
#include <gtsam/base/ChartValue.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h>
@ -251,6 +251,19 @@ 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,
* 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);
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val);
// 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);
/** 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
@ -260,6 +273,17 @@ 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,
* 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);
template <typename T, typename Chart>
void update(Key j, const T& val);
template <typename T, typename Chart, typename ChartInit>
void update(Key j, const T& val, ChartInit chart);
/** update the current available values without adding new ones */
void update(const Values& values);
@ -369,15 +393,9 @@ namespace gtsam {
// supplied \c filter function.
template<class ValueType>
static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::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<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
static CastedKeyValuePairType castHelper(KeyValuePairType key_value) {
// Static cast because we already checked the type during filtering
return CastedKeyValuePairType(key_value.key, static_cast<ValueType&>(key_value.value));
return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
}
/** Serialization function */

48
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -32,20 +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<Cal3_S2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
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<Cal3_S2>);
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>);
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>);
namespace detail {
template<class T> struct pack {
typedef T type;
};
}
#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \
typedef gtsam::ChartValue<TYPE,gtsam::DefaultChart<TYPE> > UNIQUE_NAME; \
BOOST_CLASS_EXPORT( UNIQUE_NAME );
/* ************************************************************************* */
typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
typedef PinholeCamera<Cal3Bundler> 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);
@ -56,12 +73,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<Point3> 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));
}

41
gtsam/nonlinear/tests/testValues.cpp
View File

@ -51,16 +51,25 @@ public:
};
int TestValueData::ConstructorCount = 0;
int TestValueData::DestructorCount = 0;
class TestValue : public DerivedValue<TestValue> {
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(); }
};
namespace gtsam {
namespace traits {
template <>
struct is_manifold<TestValue> : public boost::true_type {};
template <>
struct dimension<TestValue> : public boost::integral_constant<int, 0> {};
}
}
/* ************************************************************************* */
TEST( Values, equals1 )
{
@ -353,12 +362,14 @@ 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<const Pose2&>(key_value.value)));
try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
THROWS_EXCEPTION(key_value.value.cast<Pose3>());
EXPECT(assert_equal(pose2, key_value.value.cast<Pose2>()));
} else if(i == 1) {
LONGS_EQUAL(3, (long)key_value.key);
EXPECT(typeid(Pose3) == typeid(key_value.value));
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
try {key_value.value.cast<Pose3>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");}
THROWS_EXCEPTION(key_value.value.cast<Pose2>());
EXPECT(assert_equal(pose3, key_value.value.cast<Pose3>()));
} else {
EXPECT(false);
}
@ -416,10 +427,10 @@ TEST(Values, Symbol_filter) {
BOOST_FOREACH(const Values::Filtered<Value>::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<const Pose3&>(key_value.value)));
EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));
} else if(i == 1) {
LONGS_EQUAL(Symbol('y', 3), (long)key_value.key);
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
EXPECT(assert_equal(pose3, key_value.value.cast<Pose3>()));
} else {
EXPECT(false);
}
@ -441,11 +452,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<TestValue> 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);
}
/* ************************************************************************* */

25
gtsam/slam/dataset.cpp
View File

@ -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<const Pose2&>(key_value.value);
const Pose2& pose = key_value.value.cast<Pose2>();
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< " " << pose.theta() << endl;
}
@ -373,26 +373,23 @@ 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) {
const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value);
if(pose2D){
stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " "
<< pose2D->y() << " " << pose2D->theta() << endl;
Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, viewPose2) {
stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
<< key_value.value.y() << " " << key_value.value.theta() << endl;
}
const Pose3* pose3D = dynamic_cast<const Pose3*>(&key_value.value);
if(pose3D){
Point3 p = pose3D->translation();
Rot3 R = pose3D->rotation();
Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::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)
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {

6
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -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<PoseRTV> {
class GTSAM_UNSTABLE_EXPORT PoseRTV {
protected:
Pose3 Rt_;
@ -187,11 +187,11 @@ private:
namespace traits {
template<>
struct is_manifold<PoseRTV> : public std::true_type {
struct is_manifold<PoseRTV> : public boost::true_type {
};
template<>
struct dimension<PoseRTV> : public std::integral_constant<int, 9> {
struct dimension<PoseRTV> : public boost::integral_constant<int, 9> {
};
template<>

2
gtsam_unstable/geometry/BearingS2.h
View File

@ -15,7 +15,7 @@
namespace gtsam {
class GTSAM_UNSTABLE_EXPORT BearingS2 : public DerivedValue<BearingS2> {
class GTSAM_UNSTABLE_EXPORT BearingS2 {
protected:
Rot2 azimuth_, elevation_;

6
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class GTSAM_UNSTABLE_EXPORT Pose3Upright : public DerivedValue<Pose3Upright> {
class GTSAM_UNSTABLE_EXPORT Pose3Upright {
public:
static const size_t dimension = 4;
@ -144,11 +144,11 @@ private:
namespace traits {
template<>
struct is_manifold<Pose3Upright> : public std::true_type {
struct is_manifold<Pose3Upright> : public boost::true_type {
};
template<>
struct dimension<Pose3Upright> : public std::integral_constant<int, 4> {
struct dimension<Pose3Upright> : public boost::integral_constant<int, 4> {
};
}

4
gtsam_unstable/nonlinear/AdaptAutoDiff.h
View File

@ -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;

64
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -311,8 +311,9 @@ public:
//-----------------------------------------------------------------------------
/// Leaf Expression
template<class T>
template<class T, class Chart=DefaultChart<T> >
class LeafExpression: public ExpressionNode<T> {
typedef ChartValue<T,Chart> value_type; // perhaps this can be something else like a std::pair<T,Chart> ??
/// The key into values
Key key_;
@ -321,6 +322,53 @@ class LeafExpression: public ExpressionNode<T> {
LeafExpression(Key key) :
key_(key) {
}
// todo: do we need a virtual destructor here too?
friend class Expression<value_type> ;
public:
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys;
keys.insert(key_);
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
// get dimension from the chart; only works for fixed dimension charts
map[key_] = traits::dimension<Chart>::value;
}
/// Return value
virtual const value_type& value(const Values& values) const {
return dynamic_cast<const value_type&>(values.at(key_));
}
/// Construct an execution trace for reverse AD
virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace,
char* raw) const {
trace.setLeaf(key_);
return dynamic_cast<const value_type&>(values.at(key_));
}
};
//-----------------------------------------------------------------------------
/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
template<typename T>
class LeafExpression<T, DefaultChart<T> >: public ExpressionNode<T> {
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<T> ;
@ -392,7 +440,7 @@ struct Jacobian {
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct Optional {
struct OptionalJacobian {
typedef Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<A>::value> Jacobian;
typedef boost::optional<Jacobian&> type;
@ -522,7 +570,7 @@ struct FunctionalNode {
// Argument types and derived, note these are base 0 !
typedef TYPES Arguments;
typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
typedef typename boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
/// Reset expression shared pointer
template<class A, size_t N>
@ -577,7 +625,7 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
public:
typedef boost::function<T(const A1&, typename Optional<T, A1>::type)> Function;
typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
typedef typename Base::Record Record;
@ -622,8 +670,8 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
public:
typedef boost::function<
T(const A1&, const A2&, typename Optional<T, A1>::type,
typename Optional<T, A2>::type)> Function;
T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record;
@ -676,8 +724,8 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
public:
typedef boost::function<
T(const A1&, const A2&, const A3&, typename Optional<T, A1>::type,
typename Optional<T, A2>::type, typename Optional<T, A3>::type)> Function;
T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;

6
gtsam_unstable/nonlinear/Expression.h
View File

@ -61,7 +61,7 @@ public:
/// Construct a nullary method expression
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename Optional<T, A>::type) const) :
T (A::*method)(typename OptionalJacobian<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
@ -75,8 +75,8 @@ public:
/// Construct a unary method expression
template<typename A1, typename A2>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename Optional<T, A1>::type,
typename Optional<T, A2>::type) const,
T (A1::*method)(const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),

2
gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -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(448,expression.traceSize()); // Todo, should be zero
EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
#endif
set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys());

14
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -147,19 +147,19 @@ TEST(ExpressionFactor, Binary) {
// traceRaw will fill raw with [Trace<Point2> | 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<Point2>));
EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Cal3_S2>));
EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian<Point2,Cal3_S2>::type));
EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian<Point2,Point2>::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];
@ -211,8 +211,8 @@ TEST(ExpressionFactor, Shallow) {
EXPECT_LONGS_EQUAL(464, sizeof(Binary::Record));
LONGS_EQUAL(112+464, expectedTraceSize);
#else
EXPECT_LONGS_EQUAL(432, sizeof(Binary::Record));
LONGS_EQUAL(112+432, expectedTraceSize);
EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record));
LONGS_EQUAL(112+400, expectedTraceSize);
#endif
size_t size = expression.traceSize();
CHECK(size);

2
gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
View File

@ -201,7 +201,7 @@ Pose3 pose;
// Now, let's create the optional Jacobian arguments
typedef Point3 T;
typedef boost::mpl::vector<Pose3, Point3> TYPES;
typedef boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
typedef boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::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

49
tests/testManifold.cpp
View File

@ -17,6 +17,7 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
@ -65,39 +66,39 @@ TEST(Manifold, _dimension) {
// charts
TEST(Manifold, DefaultChart) {
DefaultChart<Point2> chart1(Point2(0, 0));
EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
DefaultChart<Point2> 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<Vector2> chart2(Vector2(0, 0));
EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
EXPECT(chart2.retract(v2)==Vector2(1,0));
DefaultChart<Vector2> chart2;
EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0))));
EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0));
DefaultChart<double> chart3(0);
DefaultChart<double> 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<Vector> chart4(z);
EXPECT(assert_equal(chart4.apply(v),v));
EXPECT(assert_equal(chart4.retract(v),v));
DefaultChart<Vector> 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<Rot3> chart5(I);
EXPECT(assert_equal(v3,chart5.apply(R)));
EXPECT(assert_equal(chart5.retract(v3),R));
DefaultChart<Rot3> chart5;
EXPECT(assert_equal(v3, chart5.local(I, R)));
EXPECT(assert_equal(chart5.retract(I, v3), R));
// Check zero vector
DefaultChart<Rot3> chart6(R);
EXPECT(assert_equal(zero(3),chart6.apply(R)));
DefaultChart<Rot3> chart6;
EXPECT(assert_equal(zero(3), chart6.local(R, R)));
}
/* ************************************************************************* */
@ -114,47 +115,47 @@ TEST(Manifold, _zero) {
TEST(Manifold, Canonical) {
Canonical<Point2> 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<Vector2> 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<double> chart3;
Eigen::Matrix<double, 1, 1> v1;
v1 << 1;
EXPECT(chart3.apply(1)==v1);
EXPECT(chart3.local(1)==v1);
EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
Canonical<Point3> 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<Pose3> 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<Camera> 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<Cal3Bundler>::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));
}