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

all values in Values container are now a ChartValue<ValueType, Chart=DefaultChart<ValueType> > 

ChartValues are GenericValues and a Chart, which defaults to DefaultChart
had to make charts functional (ie no storage of the chart origin) so that they could be zero sized base class
otherwise there would have been a double of the memory for values (ones for the value, and once for the chart origin, which default to the same)

most tests work, execept for serialization based stuff, and const filtering of values.
release/4.3a0
Mike Bosse 2014-10-28 00:57:44 +01:00
parent 9ef8980362
commit bc094951ed
12 changed files with 375 additions and 229 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,149 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file ChartValue.h
* @date Jan 26, 2012
* @author Michael Bosse, Abel Gawel, Renaud Dube
* based on DrivedValue.h by Duy Nguyen Ta
*/
#pragma once
#include <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());
}
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 { };
};
template<typename Chart>
const Chart& Value::getChart() const {
// define Value::cast here since now ChartValue has been declared
return dynamic_cast<const Chart&>(*this);
}
} /* namespace gtsam */

125
gtsam/base/GenericValue.h
View File

@ -19,25 +19,6 @@
#pragma once
#include <gtsam/base/Value.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 {
@ -49,142 +30,44 @@ template<typename T>
return a.equals(b,tol);
}
// trait to compute the local coordinates of other with respect to origin
template<typename T>
Vector localCoordinates(const T& origin, const T& other) {
return origin.localCoordinates(other);
}
template<typename T>
T retract(const T& origin, const Vector& delta) {
return origin.retract(delta);
}
template<typename T>
void print(const T& obj, const std::string& str) {
obj.print(str);
}
template<typename T>
size_t getDimension(const T& obj) {
return obj.dim();
}
}
template<class T>
class GenericValue : public Value {
public:
typedef T ValueType;
typedef GenericValue This;
typedef T type;
protected:
T value_;
public:
GenericValue() {}
GenericValue(const T& value) : value_(value) {}
T& value() { return value_; }
const T& value() const { return value_; }
T& value() { return value_; }
virtual ~GenericValue() {}
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
* The result must be deleted with Value::deallocate_, not with the 'delete' operator.
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(This)>::malloc();
This* ptr = new(place) This(*this);
return ptr;
}
/**
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
this->~GenericValue(); // Virtual destructor cleans up the derived object
boost::singleton_pool<PoolTag, sizeof(This)>::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<This>(*this);
}
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
// Cast the base class Value pointer to a templated generic class pointer
const This& genericValue2 = static_cast<const This&>(p);
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);
}
/// Generic Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class using the retract trait function
const T retractResult = traits::retract<T>(value_,delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(This)>::malloc();
Value* resultAsValue = new(resultAsValuePlace) This(retractResult);
// Return the pointer to the Value base class
return resultAsValue;
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
// Cast the base class Value pointer to a templated generic class pointer
const This& genericValue2 = static_cast<const This&>(value2);
// Return the result of calling localCoordinates trait on the derived class
return traits::localCoordinates<T>(value_,genericValue2.value_);
}
virtual void print(const std::string& str) const {
traits::print<T>(value_,str);
}
virtual size_t dim() const {
return traits::getDimension<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 This& derivedRhs = static_cast<const This&>(rhs);
// Do the assignment and return the result
this->value_ = derivedRhs.value_;
return *this;
}
/// Conversion to the derived class
operator const T& () const {
return value_;
}
/// Conversion to the derived class
operator T& () {
return value_;
}
protected:
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.
// DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// // Nothing to do, do not call base class assignment operator
// return *this;
// }
/// Assignment operator for this class not needed since GenricValue<T> is an abstract class
private:
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag { };
};
// define Value::cast here since now GenericValue has been declared

136
gtsam/base/Manifold.h
View File

@ -67,12 +67,13 @@ struct is_manifold: public std::false_type {
};
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
typedef std::integral_constant<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> {
@ -112,8 +113,6 @@ struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_typ
// 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 {
};
@ -141,62 +140,102 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_consta
}
};
template <typename T> struct is_chart : public std::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 {
template <typename T> struct is_chart<DefaultChart<T> > : public std::true_type {};
}
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 +243,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 +267,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();
}
};

9
gtsam/base/Value.h
View File

@ -120,12 +120,17 @@ namespace gtsam {
virtual Vector localCoordinates_(const Value& value) const = 0;
/** Assignment operator */
virtual Value& operator=(const Value& rhs) = 0;
virtual Value& operator=(const Value& rhs) {
//needs a empty definition so recursion in implicit derived assignment operators work
return *this;
}
/** Cast to known ValueType */
template<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;
}

21
gtsam/geometry/CalibratedCamera.h
View File

@ -220,5 +220,24 @@ private:
}
/// @}
};}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<CalibratedCamera> : public std::true_type {
};
template<>
struct is_manifold<CalibratedCamera> : public std::true_type {
};
template<>
struct dimension<CalibratedCamera> : public std::integral_constant<int, 6> {
};
}
}

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)));
}

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

@ -262,11 +262,11 @@ namespace gtsam {
throw ValuesKeyDoesNotExist("retrieve", j);
// Check the type and throw exception if incorrect
if(typeid(*item->second) != typeid(GenericValue<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 GenericValue<ValueType>&>(*item->second).value();
}
}
/* ************************************************************************* */
@ -276,25 +276,49 @@ namespace gtsam {
KeyValueMap::const_iterator item = values_.find(j);
if(item != values_.end()) {
// Check the type and throw exception if incorrect
if(typeid(*item->second) != typeid(GenericValue<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 GenericValue<ValueType>&>(*item->second).value();
}
} 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&>(GenericValue<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&>(GenericValue<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)));
}
}

34
gtsam/nonlinear/Values.h
View File

@ -44,8 +44,7 @@
#include <string>
#include <utility>
#include <gtsam/base/Value.h>
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/ChartValue.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h>
@ -249,15 +248,22 @@ namespace gtsam {
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(Key j, const Value& val);
/** Templated verion to add a variable with the given j,
* throws KeyAlreadyExists<J> if j is already present
* will wrap the val into a GenericValue<ValueType> to insert*/
template <typename ValueType> void insert(Key j, const ValueType& val);
/** 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
@ -266,7 +272,17 @@ namespace gtsam {
/** single element change of existing element */
void update(Key j, const Value& val);
template <typename T> void update(Key j, const T& 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);

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

@ -61,6 +61,15 @@ public:
Vector localCoordinates(const TestValue&) const { return Vector(); }
};
namespace gtsam {
namespace traits {
template <>
struct is_manifold<TestValue> : public std::true_type {};
template <>
struct dimension<TestValue> : public std::integral_constant<int, 0> {};
}
}
/* ************************************************************************* */
TEST( Values, equals1 )
{

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;

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));
}