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

Added Group concept, reworked naming and conventions to reduce unnecessary functions

release/4.3a0
Alex Cunningham 2011-11-05 23:01:43 +00:00
parent e0f2087e03
commit 2b9a3db085
52 changed files with 970 additions and 1139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/base/Group.h Normal file
View File

@ -0,0 +1,47 @@
/**
* @file Group.h
*
* @brief Concept check class for variable types with Group properties
* A Group concept extends a Manifold
*
* @date Nov 5, 2011
* @author Alex Cunningham
*/
#pragma once
namespace gtsam {
/**
* Concept check for general Group structure
*/
template<class T>
class GroupConcept {
private:
static void concept_check(const T& t) {
/** assignment */
T t2 = t;
/** compose with another object */
T compose_ret = t.compose(t2);
/** invert the object and yield a new one */
T inverse_ret = t.inverse();
/** identity */
T identity = T::identity();
}
};
} // \namespace gtsam
/**
* Macros for using the GroupConcept
* - An instantiation for use inside unit tests
* - A typedef for use inside generic algorithms
*
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
*/
#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept<T>;
#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T;

54
gtsam/base/Lie.h
View File

@ -24,21 +24,9 @@
* concept checking function in class Lie will check whether or not * concept checking function in class Lie will check whether or not
* the function exists and throw compile-time errors. * the function exists and throw compile-time errors.
* *
* Returns dimensionality of the tangent space * Expmap around identity
* inline size_t dim() const;
*
* Returns Exponential map update of T
* A default implementation of expmap(*this, lp) is available:
* expmap_default()
* T expmap(const Vector& v) const;
*
* expmap around identity
* static T Expmap(const Vector& v); * static T Expmap(const Vector& v);
* *
* Returns Log map
* A default implementation of logmap(*this, lp) is available:
* logmap_default()
* Vector logmap(const T& lp) const;
* *
* Logmap around identity * Logmap around identity
* static Vector Logmap(const T& p); * static Vector Logmap(const T& p);
@ -48,18 +36,13 @@
* between_default() * between_default()
* T between(const T& l2) const; * T between(const T& l2) const;
* *
* compose with another object
* T compose(const T& p) const;
*
* invert the object and yield a new one
* T inverse() const;
*
*/ */
#pragma once #pragma once
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/base/Group.h>
namespace gtsam { namespace gtsam {
@ -101,32 +84,14 @@ namespace gtsam {
*/ */
size_t dim_ret = t.dim(); size_t dim_ret = t.dim();
/**
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
T expmap_ret = t.expmap(gtsam::zero(dim_ret));
/** expmap around identity */ /** expmap around identity */
T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
/**
* Returns Log map
* Default Implementation calls global binary function
*/
Vector logmap_ret = t.logmap(t2);
/** Logmap around identity */ /** Logmap around identity */
Vector logmap_identity_ret = T::Logmap(t); Vector logmap_identity_ret = T::Logmap(t);
/** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
T between_ret = t.between(t2); T between_ret = t.between(t2);
/** compose with another object */
T compose_ret = t.compose(t2);
/** invert the object and yield a new one */
T inverse_ret = t.inverse();
} }
}; };
@ -156,6 +121,7 @@ namespace gtsam {
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
* http://en.wikipedia.org/wiki/Baker<65>Campbell<6C>Hausdorff_formula * http://en.wikipedia.org/wiki/Baker<65>Campbell<6C>Hausdorff_formula
*/ */
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T> template<class T>
T BCH(const T& X, const T& Y) { T BCH(const T& X, const T& Y) {
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
@ -185,13 +151,19 @@ namespace gtsam {
} // namespace gtsam } // namespace gtsam
/** /**
* Macros for using the ManifoldConcept * Macros for using the LieConcept
* - An instantiation for use inside unit tests * - An instantiation for use inside unit tests
* - A typedef for use inside generic algorithms * - A typedef for use inside generic algorithms
* *
* NOTE: intentionally not in the gtsam namespace to allow for classes not in * NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable * the gtsam namespace to be more easily enforced as testable
*/ */
/// TODO: find better name for "INST" macro, something like "UNIT" or similar #define GTSAM_CONCEPT_LIE_INST(T) \
#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::LieConcept<T>; template class gtsam::ManifoldConcept<T>; \
#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T; template class gtsam::GroupConcept<T>; \
template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) \
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;

72
gtsam/base/LieScalar.h
View File

@ -26,8 +26,8 @@ namespace gtsam {
*/ */
struct LieScalar { struct LieScalar {
/** default constructor - should be unnecessary */ /** default constructor */
LieScalar() {} LieScalar() : d_(0.0) {}
/** wrap a double */ /** wrap a double */
LieScalar(double d) : d_(d) {} LieScalar(double d) : d_(d) {}
@ -45,55 +45,51 @@ namespace gtsam {
return fabs(expected.d_ - d_) <= tol; return fabs(expected.d_ - d_) <= tol;
} }
/** // Manifold requirements
* Returns dimensionality of the tangent space
* with member and static versions /** Returns dimensionality of the tangent space */
*/
inline size_t dim() const { return 1; } inline size_t dim() const { return 1; }
inline static size_t Dim() { return 1; } inline static size_t Dim() { return 1; }
/** /** Update the LieScalar with a tangent space update */
* Returns Exponential map update of T inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
* Default implementation calls global binary function
*/
inline LieScalar expmap(const Vector& v) const { return LieScalar(d_ + v(0)); }
/** expmap around identity */ /** @return the local coordinates of another object */
static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); }
/** // Group requirements
* Returns Log map
* Default Implementation calls global binary function /** identity */
*/ inline static LieScalar identity() {
inline Vector logmap(const LieScalar& lp) const { return LieScalar();
return Vector_(1, lp.d_ - d_);
} }
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieScalar& p) { return Vector_(1, p.d_); }
inline LieScalar between(const LieScalar& t2) const { return LieScalar(t2.value() - d_); }
/** compose with another object */ /** compose with another object */
inline LieScalar compose(const LieScalar& t2) const { return LieScalar(t2.value() + d_); } inline LieScalar compose(const LieScalar& p) const {
return LieScalar(d_ + p.d_);
}
/** between operation */
inline LieScalar between(const LieScalar& l2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(1);
if(H2) *H2 = eye(1);
return LieScalar(l2.value() - value());
}
/** invert the object and yield a new one */ /** invert the object and yield a new one */
inline LieScalar inverse() const { return LieScalar(-d_); } inline LieScalar inverse() const {
return LieScalar(-1.0 * value());
}
// Manifold requirements // Lie functions
inline LieScalar retract(const Vector& v) const { return expmap(v); } /** Expmap around identity */
static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
/** expmap around identity */ /** Logmap around identity - just returns with default cast back */
inline static LieScalar Retract(const Vector& v) { return Expmap(v); } static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const LieScalar& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const LieScalar& t) { return Logmap(t); }
private: private:
double d_; double d_;

48
gtsam/base/LieVector.h
View File

@ -57,31 +57,25 @@ struct LieVector : public Vector {
return gtsam::equal(vector(), expected.vector(), tol); return gtsam::equal(vector(), expected.vector(), tol);
} }
/** // Manifold requirements
* Returns dimensionality of the tangent space
*/ /** Returns dimensionality of the tangent space */
inline size_t dim() const { return this->size(); } inline size_t dim() const { return this->size(); }
/** /** Update the LieVector with a tangent space update */
* Returns Exponential map update of T inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); }
* Default implementation calls global binary function
*/
inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
/** expmap around identity */ /** @return the local coordinates of another object */
static inline LieVector Expmap(const Vector& v) { return LieVector(v); } inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); }
/** // Group requirements
* Returns Log map
* Default Implementation calls global binary function /** identity - NOTE: no known size at compile time - so zero length */
*/ inline static LieVector identity() {
inline Vector logmap(const LieVector& lp) const { throw std::runtime_error("LieVector::identity(): Don't use this function");
return lp.vector() - vector(); return LieVector();
} }
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieVector& p) { return p; }
/** compose with another object */ /** compose with another object */
inline LieVector compose(const LieVector& p) const { inline LieVector compose(const LieVector& p) const {
return LieVector(vector() + p); return LieVector(vector() + p);
@ -101,19 +95,13 @@ struct LieVector : public Vector {
return LieVector(-1.0 * vector()); return LieVector(-1.0 * vector());
} }
// Manifold requirements // Lie functions
inline LieVector retract(const Vector& v) const { return expmap(v); } /** Expmap around identity */
static inline LieVector Expmap(const Vector& v) { return LieVector(v); }
/** expmap around identity */ /** Logmap around identity - just returns with default cast back */
inline static LieVector Retract(const Vector& v) { return Expmap(v); } static inline Vector Logmap(const LieVector& p) { return p; }
/**
* Returns inverse retraction
*/
inline Vector unretract(const LieVector& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const LieVector& t) { return Logmap(t); }
}; };
} // \namespace gtsam } // \namespace gtsam

21
gtsam/base/Manifold.h
View File

@ -26,19 +26,11 @@
* Returns Retraction update of T * Returns Retraction update of T
* T retract(const Vector& v) const; * T retract(const Vector& v) const;
* *
* Retract around identity
* static T Retract(const Vector& v);
*
* Returns inverse retraction operation * Returns inverse retraction operation
* A default implementation of unretract(*this, lp) is available: * Vector localCoordinates(const T& lp) const;
* Vector unretract(const T& lp) const;
*
* Unretract around identity
* static Vector Unretract(const T& p);
* *
*/ */
#pragma once #pragma once
#include <string> #include <string>
@ -74,16 +66,10 @@ namespace gtsam {
*/ */
T retract_ret = t.retract(gtsam::zero(dim_ret)); T retract_ret = t.retract(gtsam::zero(dim_ret));
/** expmap around identity */
T retract_identity_ret = T::Retract(gtsam::zero(dim_ret));
/** /**
* Returns inverse retraction * Returns local coordinates of another object
*/ */
Vector unretract_ret = t.unretract(t2); Vector localCoords_ret = t.localCoordinates(t2);
/** Unretract around identity */
Vector unretract_identity_ret = T::Unretract(t);
} }
}; };
@ -97,6 +83,5 @@ namespace gtsam {
* NOTE: intentionally not in the gtsam namespace to allow for classes not in * NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable * the gtsam namespace to be more easily enforced as testable
*/ */
/// TODO: find better name for "INST" macro, something like "UNIT" or similar
#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>; #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;

7
gtsam/base/lieProxies.h
View File

@ -36,13 +36,6 @@ namespace testing {
template<class T> template<class T>
T compose(const T& t1, const T& t2) { return t1.compose(t2); } T compose(const T& t1, const T& t2) { return t1.compose(t2); }
/** expmap and logmap */
template<class T>
Vector logmap(const T& t1, const T& t2) { return t1.logmap(t2); }
template<class T>
T expmap(const T& t1, const Vector& t2) { return t1.expmap(t2); }
/** unary functions */ /** unary functions */
template<class T> template<class T>
T inverse(const T& t) { return t.inverse(); } T inverse(const T& t) { return t.inverse(); }

24
gtsam/base/numericalDerivative.h
View File

@ -97,8 +97,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x.retract(d))); d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x.retract(d))); d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -151,8 +151,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1.retract(d),x2)); d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1.retract(d),x2)); d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -215,8 +215,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1,x2.retract(d))); d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1,x2.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1,x2.retract(d))); d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1,x2.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -281,8 +281,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1.retract(d),x2,x3)); d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1.retract(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1.retract(d),x2,x3)); d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1.retract(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -346,8 +346,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1, x2.retract(d),x3)); d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2.retract(d),x3));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1, x2.retract(d),x3)); d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2.retract(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }
@ -411,8 +411,8 @@ namespace gtsam {
Vector d = zero(n); Vector d = zero(n);
Matrix H = zeros(m,n); Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) { for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1, x2, x3.retract(d))); d(j) += delta; Vector hxplus = hx.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x1, x2, x3.retract(d))); d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor; d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i); for (size_t i=0;i<m;i++) H(i,j) = dh(i);
} }

5
gtsam/base/tests/testLieScalar.cpp
View File

@ -23,7 +23,6 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieScalar) GTSAM_CONCEPT_TESTABLE_INST(LieScalar)
GTSAM_CONCEPT_MANIFOLD_INST(LieScalar)
GTSAM_CONCEPT_LIE_INST(LieScalar) GTSAM_CONCEPT_LIE_INST(LieScalar)
const double tol=1e-9; const double tol=1e-9;
@ -40,10 +39,10 @@ TEST( testLieScalar, construction ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testLieScalar, logmap ) { TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.); LieScalar lie1(1.), lie2(3.);
EXPECT(assert_equal(Vector_(1, 2.), lie1.logmap(lie2))); EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

1
gtsam/base/tests/testLieVector.cpp
View File

@ -23,7 +23,6 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieVector) GTSAM_CONCEPT_TESTABLE_INST(LieVector)
GTSAM_CONCEPT_MANIFOLD_INST(LieVector)
GTSAM_CONCEPT_LIE_INST(LieVector) GTSAM_CONCEPT_LIE_INST(LieVector)
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/geometry/Cal3Bundler.cpp
View File

@ -149,14 +149,6 @@ Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; } Cal3Bundler Cal3Bundler::retract(const Vector& d) const { return Cal3Bundler(vector() + d) ; }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3Bundler::unretract(const Cal3Bundler& T2) const { return vector() - T2.vector(); } Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return vector() - T2.vector(); }
/* ************************************************************************* */
Cal3Bundler Cal3Bundler::Retract(const Vector& v) { return Cal3Bundler(v) ; }
/* ************************************************************************* */
Vector Cal3Bundler::Unretract(const Cal3Bundler& p) { return p.vector(); }
} }

5
gtsam/geometry/Cal3Bundler.h
View File

@ -52,10 +52,7 @@ public:
Matrix D2d_intrinsic_calibration(const Point2& p) const ; Matrix D2d_intrinsic_calibration(const Point2& p) const ;
Cal3Bundler retract(const Vector& d) const ; Cal3Bundler retract(const Vector& d) const ;
Vector unretract(const Cal3Bundler& T2) const ; Vector localCoordinates(const Cal3Bundler& T2) const ;
static Cal3Bundler Retract(const Vector& v) ;
static Vector Unretract(const Cal3Bundler& p) ;
int dim() const { return 3 ; } int dim() const { return 3 ; }
static size_t Dim() { return 3; } static size_t Dim() { return 3; }

8
gtsam/geometry/Cal3DS2.cpp
View File

@ -127,13 +127,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; } Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d) ; }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3DS2::unretract(const Cal3DS2& T2) const { return vector() - T2.vector(); } Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); }
/* ************************************************************************* */
Cal3DS2 Cal3DS2::Retract(const Vector& v) { return Cal3DS2(v) ; }
/* ************************************************************************* */
Vector Cal3DS2::Unretract(const Cal3DS2& p) { return p.vector(); }
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
gtsam/geometry/Cal3DS2.h
View File

@ -63,10 +63,7 @@ public:
Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const ;
Cal3DS2 retract(const Vector& d) const ; Cal3DS2 retract(const Vector& d) const ;
Vector unretract(const Cal3DS2& T2) const ; Vector localCoordinates(const Cal3DS2& T2) const ;
static Cal3DS2 Retract(const Vector& v) ;
static Vector Unretract(const Cal3DS2& p) ;
int dim() const { return 9 ; } int dim() const { return 9 ; }
static size_t Dim() { return 9; } static size_t Dim() { return 9; }

13
gtsam/geometry/Cal3_S2.h
View File

@ -133,22 +133,11 @@ namespace gtsam {
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
} }
/// Retraction from origin
inline static Cal3_S2 Retract(const Vector& d) {
Cal3_S2 c;
return c.retract(d);
}
/// Unretraction for the calibration /// Unretraction for the calibration
Vector unretract(const Cal3_S2& T2) const { Vector localCoordinates(const Cal3_S2& T2) const {
return vector() - T2.vector(); return vector() - T2.vector();
} }
/// Unretraction from origin
inline static Vector Unretract(const Cal3_S2& T2) {
return T2.vector();
}
private: private:
/// Serialization function /// Serialization function

14
gtsam/geometry/CalibratedCamera.cpp
View File

@ -85,18 +85,8 @@ CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector CalibratedCamera::unretract(const CalibratedCamera& T2) const { Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const {
return pose().unretract(T2.pose()) ; return pose().localCoordinates(T2.pose()) ;
}
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Retract(const Vector& v) {
return CalibratedCamera(Pose3::Retract(v)) ;
}
/* ************************************************************************* */
Vector CalibratedCamera::Unretract(const CalibratedCamera& p) {
return Pose3::Unretract(p.pose()) ;
} }
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/geometry/CalibratedCamera.h
View File

@ -61,13 +61,7 @@ namespace gtsam {
CalibratedCamera retract(const Vector& d) const; CalibratedCamera retract(const Vector& d) const;
/// Return canonical coordinate /// Return canonical coordinate
Vector unretract(const CalibratedCamera& T2) const; Vector localCoordinates(const CalibratedCamera& T2) const;
/// move a cameras pose according to d
static CalibratedCamera Retract(const Vector& v);
/// Return canonical coordinate
static Vector Unretract(const CalibratedCamera& p);
/// Lie group dimensionality /// Lie group dimensionality
inline size_t dim() const { return 6 ; } inline size_t dim() const { return 6 ; }

4
gtsam/geometry/CalibratedCameraT.h
View File

@ -53,8 +53,8 @@ namespace gtsam {
CalibratedCameraT retract(const Vector& d) const { CalibratedCameraT retract(const Vector& d) const {
return CalibratedCameraT(pose().retract(d), k_) ; return CalibratedCameraT(pose().retract(d), k_) ;
} }
Vector unretract(const CalibratedCameraT& T2) const { Vector localCoordinates(const CalibratedCameraT& T2) const {
return pose().unretract(T2.pose()) ; return pose().localCoordinates(T2.pose()) ;
} }
void print(const std::string& s = "") const { void print(const std::string& s = "") const {

20
gtsam/geometry/GeneralCameraT.h
View File

@ -122,26 +122,12 @@ class GeneralCameraT {
return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
} }
Vector unretract(const GeneralCameraT &C) const { Vector localCoordinates(const GeneralCameraT &C) const {
const Vector v1(calibrated().unretract(C.calibrated())), const Vector v1(calibrated().localCoordinates(C.calibrated())),
v2(calibration().unretract(C.calibration())); v2(calibration().localCoordinates(C.calibration()));
return concatVectors(2,&v1,&v2) ; return concatVectors(2,&v1,&v2) ;
} }
static GeneralCameraT Retract(const Vector& v) {
return GeneralCameraT(
Camera::Retract(sub(v,0,Camera::Dim())),
Calibration::Retract(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim()))
);
}
static Vector Unretract(const GeneralCameraT& p) {
const Vector v1(Camera::Unretract(p.calibrated())),
v2(Calibration::Unretract(p.calibration()));
return concatVectors(2,&v1,&v2);
}
inline GeneralCameraT compose(const Pose3 &p) const { inline GeneralCameraT compose(const Pose3 &p) const {
return GeneralCameraT( pose().compose(p), calibration_ ) ; return GeneralCameraT( pose().compose(p), calibration_ ) ;
} }

39
gtsam/geometry/Point2.h
View File

@ -51,10 +51,7 @@ namespace gtsam {
/** equals with an tolerance, prints out message if unequal*/ /** equals with an tolerance, prints out message if unequal*/
bool equals(const Point2& q, double tol = 1e-9) const; bool equals(const Point2& q, double tol = 1e-9) const;
/** Lie requirements */ // Group requirements
/** Size of the tangent space of the Lie type */
inline size_t dim() const { return dimension; }
/** "Compose", just adds the coordinates of two points. With optional derivatives */ /** "Compose", just adds the coordinates of two points. With optional derivatives */
inline Point2 compose(const Point2& p2, inline Point2 compose(const Point2& p2,
@ -65,14 +62,26 @@ namespace gtsam {
return *this + p2; return *this + p2;
} }
/** identity */
inline static Point2 identity() {
return Point2();
}
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
inline Point2 inverse() const { return Point2(-x_, -y_); } inline Point2 inverse() const { return Point2(-x_, -y_); }
/** Binary expmap - just adds the points */ // Manifold requirements
inline Point2 expmap(const Vector& v) const { return *this + Point2(v); }
/** Binary Logmap - just subtracts the points */ /** Size of the tangent space */
inline Vector logmap(const Point2& p2) const { return Logmap(between(p2));} inline size_t dim() const { return dimension; }
/** Updates a with tangent space delta */
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
/** Lie requirements */
/** Exponential map around identity - just create a Point2 from a vector */ /** Exponential map around identity - just create a Point2 from a vector */
static inline Point2 Expmap(const Vector& v) { return Point2(v); } static inline Point2 Expmap(const Vector& v) { return Point2(v); }
@ -80,20 +89,6 @@ namespace gtsam {
/** Log map around identity - just return the Point2 as a vector */ /** Log map around identity - just return the Point2 as a vector */
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
// Manifold requirements
inline Point2 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Point2 Retract(const Vector& v) { return Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Point2& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Point2& t) { return Logmap(t); }
/** "Between", subtracts point coordinates */ /** "Between", subtracts point coordinates */
inline Point2 between(const Point2& p2, inline Point2 between(const Point2& p2,

19
gtsam/geometry/Point3.h
View File

@ -60,6 +60,11 @@ namespace gtsam {
/** return DOF, dimensionality of tangent space */ /** return DOF, dimensionality of tangent space */
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** identity */
inline static Point3 identity() {
return Point3();
}
/** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */
inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
@ -78,24 +83,14 @@ namespace gtsam {
/** Log map at identity - return the x,y,z of this point */ /** Log map at identity - return the x,y,z of this point */
static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
/** default implementations of binary functions */
inline Point3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
inline Vector logmap(const Point3& p2) const { return gtsam::logmap_default(*this, p2);}
// Manifold requirements // Manifold requirements
inline Point3 retract(const Vector& v) const { return expmap(v); } inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
/** expmap around identity */
inline static Point3 Retract(const Vector& v) { return Expmap(v); }
/** /**
* Returns inverse retraction * Returns inverse retraction
*/ */
inline Vector unretract(const Point3& t2) const { return logmap(t2); } inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
/** Unretract around identity */
inline static Vector Unretract(const Point3& t) { return Logmap(t); }
/** Between using the default implementation */ /** Between using the default implementation */
inline Point3 between(const Point3& p2, inline Point3 between(const Point3& p2,

31
gtsam/geometry/Pose2.cpp
View File

@ -84,33 +84,24 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::retract(const Vector& v) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
/* ************************************************************************* */ return compose(Expmap(v));
// Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */
Pose2 Pose2::Retract(const Vector& xi) {
return Expmap(xi);
}
/* ************************************************************************* */
Vector Pose2::Unretract(const Pose2& p) {
return Logmap(p);
}
#else #else
/* ************************************************************************* */
Pose2 Pose2::Retract(const Vector& v) {
assert(v.size() == 3); assert(v.size() == 3);
return Pose2(v[0], v[1], v[2]); return compose(Pose2(v[0], v[1], v[2]));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose2::Unretract(const Pose2& p) { Vector Pose2::localCoordinates(const Pose2& p2) const {
return Vector_(3, p.x(), p.y(), p.theta()); #ifdef SLOW_BUT_CORRECT_EXPMAP
} return Logmap(between(p2));
#else
Pose2 r = between(p2);
return Vector_(3, r.x(), r.y(), r.theta());
#endif #endif
}
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map // Calculate Adjoint map

21
gtsam/geometry/Pose2.h
View File

@ -119,27 +119,18 @@ namespace gtsam {
/** syntactic sugar for transform_from */ /** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) const { return transform_from(point);} inline Point2 operator*(const Point2& point) const { return transform_from(point);}
/** /** identity */
* Retraction from se(2) to SE(2) inline static Pose2 identity() {
*/ return Pose2();
static Pose2 Retract(const Vector& v); }
/**
* Inverse of retraction, from SE(2) to se(2)
*/
static Vector Unretract(const Pose2& p);
/** Real versions of Expmap/Logmap */ /** Real versions of Expmap/Logmap */
static Pose2 Expmap(const Vector& xi); static Pose2 Expmap(const Vector& xi);
static Vector Logmap(const Pose2& p); static Vector Logmap(const Pose2& p);
/** default implementations of binary functions */ /** default implementations of binary functions */
inline Pose2 retract(const Vector& v) const { return compose(Retract(v)); } Pose2 retract(const Vector& v) const;
inline Vector unretract(const Pose2& p2) const { return Unretract(between(p2));} Vector localCoordinates(const Pose2& p2) const;
/** non-approximated versions of expmap/logmap */
inline Pose2 expmap(const Vector& v) const { return compose(Expmap(v)); }
inline Vector logmap(const Pose2& p2) const { return Logmap(between(p2));}
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame

62
gtsam/geometry/Pose3.cpp
View File

@ -95,17 +95,17 @@ namespace gtsam {
} }
#ifdef CORRECT_POSE3_EXMAP #ifdef CORRECT_POSE3_EXMAP
/* ************************************************************************* */ // /* ************************************************************************* */
// Changes default to use the full verions of expmap/logmap // // Changes default to use the full verions of expmap/logmap
/* ************************************************************************* */ // /* ************************************************************************* */
Pose3 Retract(const Vector& xi) { // Pose3 Retract(const Vector& xi) {
return Pose3::Expmap(xi); // return Pose3::Expmap(xi);
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
Vector Unretract(const Pose3& p) { // Vector Unretract(const Pose3& p) {
return Pose3::Logmap(p); // return Pose3::Logmap(p);
} // }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 retract(const Vector& d) { Pose3 retract(const Vector& d) {
@ -113,27 +113,27 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector unretract(const Pose3& T1, const Pose3& T2) { Vector localCoordinates(const Pose3& T1, const Pose3& T2) {
return logmap(T2); return localCoordinates(T2);
} }
#else #else
/* ************************************************************************* */ // /* ************************************************************************* */
/* incorrect versions for which we know how to compute derivatives */ // /* incorrect versions for which we know how to compute derivatives */
Pose3 Pose3::Retract(const Vector& d) { // Pose3 Pose3::Retract(const Vector& d) {
Vector w = sub(d, 0,3); // Vector w = sub(d, 0,3);
Vector u = sub(d, 3,6); // Vector u = sub(d, 3,6);
return Pose3(Rot3::Retract(w), Point3::Retract(u)); // return Pose3(Rot3::Retract(w), Point3::Retract(u));
} // }
//
/* ************************************************************************* */ // /* ************************************************************************* */
// Log map at identity - return the translation and canonical rotation // // Log map at identity - return the translation and canonical rotation
// coordinates of a pose. // // coordinates of a pose.
Vector Pose3::Unretract(const Pose3& p) { // Vector Pose3::Unretract(const Pose3& p) {
const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation()); // const Vector w = Rot3::Unretract(p.rotation()), u = Point3::Unretract(p.translation());
return concatVectors(2, &w, &u); // return concatVectors(2, &w, &u);
} // }
/** These are the "old-style" expmap and logmap about the specified /** These are the "old-style" expmap and logmap about the specified
* pose. Increments the offset and rotation independently given a translation and * pose. Increments the offset and rotation independently given a translation and
@ -145,9 +145,9 @@ namespace gtsam {
} }
/** Independently computes the logmap of the translation and rotation. */ /** Independently computes the logmap of the translation and rotation. */
Vector Pose3::unretract(const Pose3& pp) const { Vector Pose3::localCoordinates(const Pose3& pp) const {
const Vector r(R_.unretract(pp.rotation())), const Vector r(R_.localCoordinates(pp.rotation())),
t(t_.unretract(pp.translation())); t(t_.localCoordinates(pp.translation()));
return concatVectors(2, &r, &t); return concatVectors(2, &r, &t);
} }

19
gtsam/geometry/Pose3.h
View File

@ -91,6 +91,11 @@ namespace gtsam {
/** Dimensionality of the tangent space */ /** Dimensionality of the tangent space */
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** identity */
inline static Pose3 identity() {
return Pose3();
}
/** /**
* Derivative of inverse * Derivative of inverse
*/ */
@ -115,28 +120,16 @@ namespace gtsam {
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a pose with a translation and
* rotation (in canonical coordinates). */
static Pose3 Retract(const Vector& v);
/** Log map at identity - return the translation and canonical rotation
* coordinates of a pose. */
static Vector Unretract(const Pose3& p);
/** Exponential map around another pose */ /** Exponential map around another pose */
Pose3 retract(const Vector& d) const; Pose3 retract(const Vector& d) const;
/** Logarithm map around another pose T1 */ /** Logarithm map around another pose T1 */
Vector unretract(const Pose3& T2) const; Vector localCoordinates(const Pose3& T2) const;
/** non-approximated versions of Expmap/Logmap */ /** non-approximated versions of Expmap/Logmap */
static Pose3 Expmap(const Vector& xi); static Pose3 Expmap(const Vector& xi);
static Vector Logmap(const Pose3& p); static Vector Logmap(const Pose3& p);
/** non-approximated versions of expmap/logmap */
inline Pose3 expmap(const Vector& v) const { return compose(Pose3::Expmap(v)); }
inline Vector logmap(const Pose3& p2) const { return Pose3::Logmap(between(p2));}
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives * as well as optionally the derivatives

25
gtsam/geometry/Rot2.h
View File

@ -127,6 +127,11 @@ namespace gtsam {
return dimension; return dimension;
} }
/** identity */
inline static Rot2 identity() {
return Rot2();
}
/** Compose - make a new rotation by adding angles */ /** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 = inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::none, boost::optional<Matrix&> H2 = boost::none) const {
@ -148,30 +153,14 @@ namespace gtsam {
return Vector_(1, r.theta()); return Vector_(1, r.theta());
} }
/** Binary expmap */
inline Rot2 expmap(const Vector& v) const {
return *this * Expmap(v);
}
/** Binary Logmap */
inline Vector logmap(const Rot2& p2) const {
return Logmap(between(p2));
}
// Manifold requirements // Manifold requirements
inline Rot2 retract(const Vector& v) const { return expmap(v); } inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
/** expmap around identity */
inline static Rot2 Retract(const Vector& v) { return Expmap(v); }
/** /**
* Returns inverse retraction * Returns inverse retraction
*/ */
inline Vector unretract(const Rot2& t2) const { return logmap(t2); } inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
/** Unretract around identity */
inline static Vector Unretract(const Rot2& t) { return Logmap(t); }
/** Between using the default implementation */ /** Between using the default implementation */
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 = inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =

19
gtsam/geometry/Rot3.h
View File

@ -197,27 +197,22 @@ public:
else return rodriguez(v); else return rodriguez(v);
} }
/** identity */
inline static Rot3 identity() {
return Rot3();
}
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3& R); static Vector Logmap(const Rot3& R);
/** default implementations of binary functions */
inline Rot3 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
inline Vector logmap(const Rot3& p2) const { return gtsam::logmap_default(*this, p2);}
// Manifold requirements // Manifold requirements
inline Rot3 retract(const Vector& v) const { return expmap(v); } inline Rot3 retract(const Vector& v) const { return compose(Expmap(v)); }
/** expmap around identity */
inline static Rot3 Retract(const Vector& v) { return Expmap(v); }
/** /**
* Returns inverse retraction * Returns inverse retraction
*/ */
inline Vector unretract(const Rot3& t2) const { return logmap(t2); } inline Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); }
/** Unretract around identity */
inline static Vector Unretract(const Rot3& t) { return Logmap(t); }
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3() // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()

29
gtsam/geometry/StereoCamera.h
View File

@ -98,34 +98,19 @@ public:
return 6; return 6;
} }
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().retract(d), K_);
}
Vector logmap(const StereoCamera &camera) const {
const Vector v1(leftCamPose_.unretract(camera.leftCamPose_));
return v1;
}
inline static StereoCamera Expmap(const Vector& d) {
return StereoCamera().expmap(d);
}
inline static Vector Logmap(const StereoCamera &camera) {
return StereoCamera().logmap(camera);
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const { bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol); *camera.K_, tol);
} }
// Manifold requirements - use existing expmap/logmap // Manifold requirements - use existing expmap/logmap
inline StereoCamera retract(const Vector& v) const { return expmap(v); } inline StereoCamera retract(const Vector& v) const {
inline static StereoCamera Retract(const Vector& v) { return Expmap(v); } return StereoCamera(pose().retract(v), K_);
inline Vector unretract(const StereoCamera& t2) const { return logmap(t2); } }
inline static Vector Unretract(const StereoCamera& t) { return Logmap(t); }
inline Vector localCoordinates(const StereoCamera& t2) const {
return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_));
}
Pose3 between(const StereoCamera &camera, Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,

24
gtsam/geometry/StereoPoint2.h
View File

@ -86,6 +86,11 @@ namespace gtsam {
return *this + p1; return *this + p1;
} }
/** identity */
inline static StereoPoint2 identity() {
return StereoPoint2();
}
/** inverse */ /** inverse */
inline StereoPoint2 inverse() const { inline StereoPoint2 inverse() const {
return StereoPoint2()- (*this); return StereoPoint2()- (*this);
@ -101,29 +106,14 @@ namespace gtsam {
return p.vector(); return p.vector();
} }
/** default implementations of binary functions */
inline StereoPoint2 expmap(const Vector& v) const {
return gtsam::expmap_default(*this, v);
}
inline Vector logmap(const StereoPoint2& p2) const {
return gtsam::logmap_default(*this, p2);
}
// Manifold requirements // Manifold requirements
inline StereoPoint2 retract(const Vector& v) const { return expmap(v); } inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
/** expmap around identity */
inline static StereoPoint2 Retract(const Vector& v) { return Expmap(v); }
/** /**
* Returns inverse retraction * Returns inverse retraction
*/ */
inline Vector unretract(const StereoPoint2& t2) const { return logmap(t2); } inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
/** Unretract around identity */
inline static Vector Unretract(const StereoPoint2& t) { return Logmap(t); }
inline StereoPoint2 between(const StereoPoint2& p2) const { inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2); return gtsam::between_default(*this, p2);

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

@ -124,7 +124,7 @@ namespace gtsam {
Index<3, 'C'> I; // contravariant 2D camera Index<3, 'C'> I; // contravariant 2D camera
return toVector(H(I,_T)); return toVector(H(I,_T));
} }
Vector logmap(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
return toVector(A)-toVector(B); // TODO correct order ? return toVector(A)-toVector(B); // TODO correct order ?
} }
} }

7
gtsam/geometry/tests/testPoint2.cpp
View File

@ -23,7 +23,6 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_MANIFOLD_INST(Point2)
GTSAM_CONCEPT_LIE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2)
/* ************************************************************************* */ /* ************************************************************************* */
@ -40,8 +39,8 @@ TEST(Point2, Lie) {
EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2)); EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(5,7), p1.expmap(Vector_(2, 4.,5.)))); EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.))));
EXPECT(assert_equal(Vector_(2, 3.,3.), p1.logmap(p2))); EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -50,7 +49,7 @@ TEST( Point2, expmap)
Vector d(2); Vector d(2);
d(0) = 1; d(0) = 1;
d(1) = -1; d(1) = -1;
Point2 a(4, 5), b = a.expmap(d), c(5, 4); Point2 a(4, 5), b = a.retract(d), c(5, 4);
EXPECT(assert_equal(b,c)); EXPECT(assert_equal(b,c));
} }

5
gtsam/geometry/tests/testPoint3.cpp
View File

@ -21,7 +21,6 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3) GTSAM_CONCEPT_TESTABLE_INST(Point3)
GTSAM_CONCEPT_MANIFOLD_INST(Point3)
GTSAM_CONCEPT_LIE_INST(Point3) GTSAM_CONCEPT_LIE_INST(Point3)
Point3 P(0.2,0.7,-2); Point3 P(0.2,0.7,-2);
@ -40,8 +39,8 @@ TEST(Point3, Lie) {
EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2)); EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.expmap(Vector_(3, 4.,5.,6.)))); EXPECT(assert_equal(Point3(5,7,9), p1.retract(Vector_(3, 4.,5.,6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.logmap(p2))); EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

28
gtsam/geometry/tests/testPose2.cpp
View File

@ -36,14 +36,8 @@ using namespace std;
// #define SLOW_BUT_CORRECT_EXPMAP // #define SLOW_BUT_CORRECT_EXPMAP
GTSAM_CONCEPT_TESTABLE_INST(Pose2) GTSAM_CONCEPT_TESTABLE_INST(Pose2)
GTSAM_CONCEPT_MANIFOLD_INST(Pose2)
GTSAM_CONCEPT_LIE_INST(Pose2) GTSAM_CONCEPT_LIE_INST(Pose2)
// concept checks for testable
GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_TESTABLE_INST(Rot2)
GTSAM_CONCEPT_TESTABLE_INST(LieVector)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, constructors) { TEST(Pose2, constructors) {
Point2 p; Point2 p;
@ -59,10 +53,10 @@ TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2)); Pose2 t1(M_PI_2, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 origin; Pose2 origin;
Vector d12 = t1.unretract(t2); Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
EXPECT(assert_equal(t2, t1*origin.retract(d12))); EXPECT(assert_equal(t2, t1*origin.retract(d12)));
Vector d21 = t2.unretract(t1); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
EXPECT(assert_equal(t1, t2*origin.retract(d21))); EXPECT(assert_equal(t1, t2*origin.retract(d21)));
} }
@ -83,7 +77,7 @@ TEST(Pose2, retract) {
TEST(Pose2, expmap) { TEST(Pose2, expmap) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -91,7 +85,7 @@ TEST(Pose2, expmap) {
TEST(Pose2, expmap2) { TEST(Pose2, expmap2) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -117,12 +111,12 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0) { TEST(Pose2, expmap0) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI_2, Point2(1, 2));
#ifdef SLOW_BUT_CORRECT_EXPMAP //#ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
#else //#else
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); // Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
#endif //#endif
Pose2 actual = pose * Pose2::Retract(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -183,7 +177,7 @@ TEST(Pose2, logmap) {
#else #else
Vector expected = Vector_(3, 0.01, -0.015, 0.018); Vector expected = Vector_(3, 0.01, -0.015, 0.018);
#endif #endif
Vector actual = pose0.unretract(pose); Vector actual = pose0.localCoordinates(pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
@ -192,7 +186,7 @@ TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
Vector actual = pose0.logmap(pose); Vector actual = logmap_default<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }

27
gtsam/geometry/tests/testPose3.cpp
View File

@ -25,7 +25,6 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_MANIFOLD_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3)
static Point3 P(0.2,0.7,-2); static Point3 P(0.2,0.7,-2);
@ -66,9 +65,9 @@ TEST( Pose3, expmap_a_full)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(id.expmap(v), Pose3(R, Point3()))); EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),id.expmap(v),1e-5)); EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -77,9 +76,9 @@ TEST( Pose3, expmap_a_full2)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(id.expmap(v), Pose3(R, Point3()))); EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),id.expmap(v),1e-5)); EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -215,11 +214,11 @@ Pose3 Agrawal06iros(const Vector& xi) {
Vector v = xi.tail(3); Vector v = xi.tail(3);
double t = norm_2(w); double t = norm_2(w);
if (t < 1e-5) if (t < 1e-5)
return Pose3(Rot3(), Point3::Retract(v)); return Pose3(Rot3(), Point3::Expmap(v));
else { else {
Matrix W = skewSymmetric(w/t); Matrix W = skewSymmetric(w/t);
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(Rot3::Expmap (w), Point3::Retract(A * v)); return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v));
} }
} }
@ -526,12 +525,12 @@ TEST(Pose3, manifold)
Pose3 t1 = T; Pose3 t1 = T;
Pose3 t2 = T3; Pose3 t2 = T3;
Pose3 origin; Pose3 origin;
Vector d12 = t1.logmap(t2); Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.expmap(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
// EXPECT(assert_equal(t2, retract(origin,d12)*t1)); // EXPECT(assert_equal(t2, retract(origin,d12)*t1));
Vector d21 = t2.logmap(t1); Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.expmap(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap // todo: richard - commented out because this tests for "compose-style" (new) expmap
// EXPECT(assert_equal(t1, retract(origin,d21)*t2)); // EXPECT(assert_equal(t1, retract(origin,d21)*t2));
@ -656,9 +655,9 @@ TEST( Pose3, unicycle )
{ {
// velocity in X should be X in inertial frame, rather than global frame // velocity in X should be X in inertial frame, rather than global frame
Vector x_step = delta(6,3,1.0); Vector x_step = delta(6,3,1.0);
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), x1.expmap(x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), x2.expmap(x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), x3.expmap(sqrt(2) * x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2) * x_step), tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
gtsam/geometry/tests/testRot2.cpp
View File

@ -23,7 +23,6 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot2) GTSAM_CONCEPT_TESTABLE_INST(Rot2)
GTSAM_CONCEPT_MANIFOLD_INST(Rot2)
GTSAM_CONCEPT_LIE_INST(Rot2) GTSAM_CONCEPT_LIE_INST(Rot2)
Rot2 R(Rot2::fromAngle(0.1)); Rot2 R(Rot2::fromAngle(0.1));
@ -89,7 +88,7 @@ TEST( Rot2, equals)
TEST( Rot2, expmap) TEST( Rot2, expmap)
{ {
Vector v = zero(1); Vector v = zero(1);
CHECK(assert_equal(R.expmap(v), R)); CHECK(assert_equal(R.retract(v), R));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -98,7 +97,7 @@ TEST(Rot2, logmap)
Rot2 rot0(Rot2::fromAngle(M_PI_2)); Rot2 rot0(Rot2::fromAngle(M_PI_2));
Rot2 rot(Rot2::fromAngle(M_PI)); Rot2 rot(Rot2::fromAngle(M_PI));
Vector expected = Vector_(1, M_PI_2); Vector expected = Vector_(1, M_PI_2);
Vector actual = rot0.logmap(rot); Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }

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

@ -26,7 +26,6 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3) GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_MANIFOLD_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3) GTSAM_CONCEPT_LIE_INST(Rot3)
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
@ -142,7 +141,7 @@ TEST( Rot3, rodriguez4)
TEST( Rot3, expmap) TEST( Rot3, expmap)
{ {
Vector v = zero(3); Vector v = zero(3);
CHECK(assert_equal(R.expmap(v), R)); CHECK(assert_equal(R.retract(v), R));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -189,11 +188,11 @@ TEST(Rot3, manifold)
Rot3 origin; Rot3 origin;
// log behaves correctly // log behaves correctly
Vector d12 = gR1.logmap(gR2); Vector d12 = gR1.localCoordinates(gR2);
CHECK(assert_equal(gR2, gR1.expmap(d12))); CHECK(assert_equal(gR2, gR1.retract(d12)));
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
Vector d21 = gR2.logmap(gR1); Vector d21 = gR2.localCoordinates(gR1);
CHECK(assert_equal(gR1, gR2.expmap(d21))); CHECK(assert_equal(gR1, gR2.retract(d21)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1) // Check that log(t1,t2)=-log(t2,t1)

1
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -15,7 +15,6 @@
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_MANIFOLD_INST(StereoPoint2)
GTSAM_CONCEPT_LIE_INST(StereoPoint2) GTSAM_CONCEPT_LIE_INST(StereoPoint2)
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/nonlinear/LieValues-inl.h
View File

@ -193,19 +193,19 @@ namespace gtsam {
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J> template<class J>
inline VectorValues LieValues<J>::unretract(const LieValues<J>& cp, const Ordering& ordering) const { inline VectorValues LieValues<J>::localCoordinates(const LieValues<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
unretract(cp, ordering, delta); localCoordinates(cp, ordering, delta);
return delta; return delta;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void LieValues<J>::unretract(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const { void LieValues<J>::localCoordinates(const LieValues<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first)); assert(this->exists(value.first));
delta[ordering[value.first]] = this->at(value.first).unretract(value.second); delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second);
} }
} }

4
gtsam/nonlinear/LieValues.h
View File

@ -122,10 +122,10 @@ namespace gtsam {
LieValues retract(const VectorValues& delta, const Ordering& ordering) const; LieValues retract(const VectorValues& delta, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
VectorValues unretract(const LieValues& cp, const Ordering& ordering) const; VectorValues localCoordinates(const LieValues& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
void unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const; void localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
// imperative methods: // imperative methods:

4
gtsam/nonlinear/NonlinearConstraint.h
View File

@ -573,7 +573,7 @@ public:
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const { Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
const size_t p = X::Dim(); const size_t p = X::Dim();
if (H1) *H1 = eye(p); if (H1) *H1 = eye(p);
return value_.unretract(x1); return value_.localCoordinates(x1);
} }
/** Print */ /** Print */
@ -628,7 +628,7 @@ public:
const size_t p = X::Dim(); const size_t p = X::Dim();
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
return x1.unretract(x2); return x1.localCoordinates(x2);
} }
private: private:

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -120,7 +120,7 @@ namespace gtsam {
size_t nj = feasible_.dim(); size_t nj = feasible_.dim();
if (allow_error_) { if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
return xj.unretract(feasible_); return xj.localCoordinates(feasible_);
} else if (compare_(feasible_,xj)) { } else if (compare_(feasible_,xj)) {
if (H) *H = eye(nj); if (H) *H = eye(nj);
return zero(nj); // set error to zero if equal return zero(nj); // set error to zero if equal

18
gtsam/nonlinear/TupleValues.h
View File

@ -210,16 +210,16 @@ namespace gtsam {
} }
/** logmap each element */ /** logmap each element */
VectorValues unretract(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const { VectorValues localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
unretract(cp, ordering, delta); localCoordinates(cp, ordering, delta);
return delta; return delta;
} }
/** logmap each element */ /** logmap each element */
void unretract(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const { void localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.unretract(cp.first_, ordering, delta); first_.localCoordinates(cp.first_, ordering, delta);
second_.unretract(cp.second_, ordering, delta); second_.localCoordinates(cp.second_, ordering, delta);
} }
/** /**
@ -322,14 +322,14 @@ namespace gtsam {
return TupleValuesEnd(first_.retract(delta, ordering)); return TupleValuesEnd(first_.retract(delta, ordering));
} }
VectorValues unretract(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const { VectorValues localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
unretract(cp, ordering, delta); localCoordinates(cp, ordering, delta);
return delta; return delta;
} }
void unretract(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const { void localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.unretract(cp.first_, ordering, delta); first_.localCoordinates(cp.first_, ordering, delta);
} }
/** /**

2
gtsam/slam/BetweenConstraint.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
X hx = x1.between(x2, H1, H2); X hx = x1.between(x2, H1, H2);
return measured_.unretract(hx); return measured_.localCoordinates(hx);
} }
inline const X measured() const { inline const X measured() const {

2
gtsam/slam/BetweenFactor.h
View File

@ -83,7 +83,7 @@ namespace gtsam {
boost::none) const { boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x) T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return measured_.unretract(hx); return measured_.localCoordinates(hx);
} }
/** return the measured */ /** return the measured */

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -82,7 +82,7 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> H2=boost::none) const {
Vector error = z_.unretract(camera.project(point,H1,H2)); Vector error = z_.localCoordinates(camera.project(point,H1,H2));
// gtsam::print(error, "error"); // gtsam::print(error, "error");
return error; return error;
} }

9
gtsam/slam/PartialPriorFactor.h
View File

@ -118,16 +118,17 @@ namespace gtsam {
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = zeros(this->dim(), p.dim()); if (H) (*H) = zeros(this->dim(), p.dim());
Vector full_unretraction = T::Unretract(p); // FIXME: this was originally the generic retraction - may not produce same results
Vector masked_unretraction = zero(this->dim()); Vector full_logmap = T::Logmap(p);
Vector masked_logmap = zero(this->dim());
size_t masked_idx=0; size_t masked_idx=0;
for (size_t i=0;i<mask_.size();++i) for (size_t i=0;i<mask_.size();++i)
if (mask_[i]) { if (mask_[i]) {
masked_unretraction(masked_idx) = full_unretraction(i); masked_logmap(masked_idx) = full_logmap(i);
if (H) (*H)(masked_idx, i) = 1.0; if (H) (*H)(masked_idx, i) = 1.0;
++masked_idx; ++masked_idx;
} }
return masked_unretraction - prior_; return masked_logmap - prior_;
} }
// access // access

2
gtsam/slam/PriorFactor.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(p.dim()); if (H) (*H) = eye(p.dim());
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.unretract(p); return prior_.localCoordinates(p);
} }
private: private:

4
gtsam/slam/simulated2DOriented.h
View File

@ -69,7 +69,7 @@ namespace gtsam {
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
return z_.unretract(prior(x, H)); return z_.localCoordinates(prior(x, H));
} }
}; };
@ -93,7 +93,7 @@ namespace gtsam {
Vector evaluateError(const Pose2& x1, const Pose2& x2, Vector evaluateError(const Pose2& x1, const Pose2& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
return z_.unretract(odo(x1, x2, H1, H2)); return z_.localCoordinates(odo(x1, x2, H1, H2));
} }
}; };

4
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -140,9 +140,9 @@ TEST( Pose3Factor, error )
x.insert(1,t1); x.insert(1,t1);
x.insert(2,t2); x.insert(2,t2);
// Get error h(x)-z -> unretract(z,h(x)) = unretract(z,between(t1,t2)) // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x); Vector actual = factor.unwhitenedError(x);
Vector expected = z.unretract(t1.between(t2)); Vector expected = z.localCoordinates(t1.between(t2));
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

2
tests/testExtendedKalmanFilter.cpp
View File

@ -232,7 +232,7 @@ public:
} }
// Return the error between the prediction and the supplied value of p2 // Return the error between the prediction and the supplied value of p2
return prediction.unretract(p2); return prediction.localCoordinates(p2);
} }
}; };

6
tests/testTupleValues.cpp
View File

@ -203,7 +203,7 @@ TEST(TupleValues, zero_expmap_logmap)
// Check log // Check log
VectorValues expected_log = delta; VectorValues expected_log = delta;
VectorValues actual_log = values1.unretract(actual, o); VectorValues actual_log = values1.localCoordinates(actual, o);
CHECK(assert_equal(expected_log, actual_log)); CHECK(assert_equal(expected_log, actual_log));
} }
@ -460,7 +460,7 @@ TEST(TupleValues, expmap)
expected.insert(l2k, Point2(10.3, 11.4)); expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, values1.retract(delta, o))); CHECK(assert_equal(expected, values1.retract(delta, o)));
CHECK(assert_equal(delta, values1.unretract(expected, o))); CHECK(assert_equal(delta, values1.localCoordinates(expected, o)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -491,7 +491,7 @@ TEST(TupleValues, expmap_typedefs)
expected.insert(l2k, Point2(10.3, 11.4)); expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, TupleValues2<PoseValues, PointValues>(values1.retract(delta, o)))); CHECK(assert_equal(expected, TupleValues2<PoseValues, PointValues>(values1.retract(delta, o))));
//CHECK(assert_equal(delta, values1.unretract(expected))); //CHECK(assert_equal(delta, values1.localCoordinates(expected)));
} }
/* ************************************************************************* */ /* ************************************************************************* */