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 {

278
gtsam/geometry/GeneralCameraT.h
View File

@ -32,178 +32,164 @@ namespace gtsam {
template <typename Camera, typename Calibration> template <typename Camera, typename Calibration>
class GeneralCameraT { class GeneralCameraT {
private: private:
Camera calibrated_; // Calibrated camera Camera calibrated_; // Calibrated camera
Calibration calibration_; // Calibration Calibration calibration_; // Calibration
public: public:
GeneralCameraT(){} GeneralCameraT(){}
GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {}
GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {}
GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {}
GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} GeneralCameraT(const Pose3& pose) : calibrated_(pose) {}
GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {}
// Vector Initialization // Vector Initialization
GeneralCameraT(const Vector &v) : GeneralCameraT(const Vector &v) :
calibrated_(sub(v, 0, Camera::Dim())), calibrated_(sub(v, 0, Camera::Dim())),
calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
inline const Pose3& pose() const { return calibrated_.pose(); } inline const Pose3& pose() const { return calibrated_.pose(); }
inline const Camera& calibrated() const { return calibrated_; } inline const Camera& calibrated() const { return calibrated_; }
inline const Calibration& calibration() const { return calibration_; } inline const Calibration& calibration() const { return calibration_; }
std::pair<Point2,bool> projectSafe( std::pair<Point2,bool> projectSafe(
const Point3& P, const Point3& P,
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 {
Point3 cameraPoint = calibrated_.pose().transform_to(P); Point3 cameraPoint = calibrated_.pose().transform_to(P);
return std::pair<Point2, bool>( return std::pair<Point2, bool>(
project(P,H1,H2), project(P,H1,H2),
cameraPoint.z() > 0); cameraPoint.z() > 0);
}
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = calibration_.calibrate(projection);
Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
return calibrated_.pose().transform_from(cameraPoint);
}
/**
* project function that does not merge the Jacobians of calibration and pose
*/
Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const {
Matrix tmp;
Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt);
Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp);
H1_pose = tmp * H1_pose;
H2_pt = tmp * H2_pt;
return projection;
}
/**
* project a 3d point to the camera
* P is point in camera coordinate
* H1 is respect to pose + calibration
* H2 is respect to landmark
*/
Point2 project(const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (!H1 && !H2) {
Point2 intrinsic = calibrated_.project(P);
return calibration_.uncalibrate(intrinsic) ;
} }
Point3 backproject(const Point2& projection, const double scale) const { Matrix H1_k, H1_pose, H2_pt;
Point2 intrinsic = calibration_.calibrate(projection); Point2 projection = project(P, H1_pose, H1_k, H2_pt);
Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k);
return calibrated_.pose().transform_from(cameraPoint); if ( H2 ) *H2 = H2_pt;
}
/** return projection;
* project function that does not merge the Jacobians of calibration and pose }
*/
Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const {
Matrix tmp;
Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt);
Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp);
H1_pose = tmp * H1_pose;
H2_pt = tmp * H2_pt;
return projection;
}
/** // dump functions for compilation for now
* project a 3d point to the camera bool equals (const GeneralCameraT &camera, double tol = 1e-9) const {
* P is point in camera coordinate return calibrated_.equals(camera.calibrated_, tol) &&
* H1 is respect to pose + calibration calibration_.equals(camera.calibration_, tol) ;
* H2 is respect to landmark }
*/
Point2 project(const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (!H1 && !H2) { void print(const std::string& s = "") const {
Point2 intrinsic = calibrated_.project(P); calibrated_.pose().print(s + ".camera.") ;
return calibration_.uncalibrate(intrinsic) ; calibration_.print(s + ".calibration.") ;
} }
Matrix H1_k, H1_pose, H2_pt; GeneralCameraT retract(const Vector &v) const {
Point2 projection = project(P, H1_pose, H1_k, H2_pt); Vector v1 = sub(v,0,Camera::Dim());
if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k); Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
if ( H2 ) *H2 = H2_pt; return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
}
return projection; Vector localCoordinates(const GeneralCameraT &C) const {
} const Vector v1(calibrated().localCoordinates(C.calibrated())),
v2(calibration().localCoordinates(C.calibration()));
return concatVectors(2,&v1,&v2) ;
}
// dump functions for compilation for now inline GeneralCameraT compose(const Pose3 &p) const {
bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { return GeneralCameraT( pose().compose(p), calibration_ ) ;
return calibrated_.equals(camera.calibrated_, tol) && }
calibration_.equals(camera.calibration_, tol) ;
}
void print(const std::string& s = "") const { Matrix D2d_camera(const Point3& point) const {
calibrated_.pose().print(s + ".camera.") ; Point2 intrinsic = calibrated_.project(point);
calibration_.print(s + ".calibration.") ; Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
} Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
GeneralCameraT retract(const Vector &v) const { const int n1 = calibrated_.dim() ;
Vector v1 = sub(v,0,Camera::Dim()); const int n2 = calibration_.dim() ;
Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); Matrix D(2,n1+n2) ;
return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
}
Vector unretract(const GeneralCameraT &C) const { sub(D,0,2,0,n1) = D_2d_pose ;
const Vector v1(calibrated().unretract(C.calibrated())), sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
v2(calibration().unretract(C.calibration())); return D;
return concatVectors(2,&v1,&v2) ; }
}
static GeneralCameraT Retract(const Vector& v) { Matrix D2d_3d(const Point3& point) const {
return GeneralCameraT( Point2 intrinsic = calibrated_.project(point);
Camera::Retract(sub(v,0,Camera::Dim())), Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Calibration::Retract(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim())) Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
); return D_2d_intrinsic * D_intrinsic_3d;
} }
static Vector Unretract(const GeneralCameraT& p) { Matrix D2d_camera_3d(const Point3& point) const {
const Vector v1(Camera::Unretract(p.calibrated())), Point2 intrinsic = calibrated_.project(point);
v2(Calibration::Unretract(p.calibration())); Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
return concatVectors(2,&v1,&v2); Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
} Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d;
inline GeneralCameraT compose(const Pose3 &p) const { const int n1 = calibrated_.dim() ;
return GeneralCameraT( pose().compose(p), calibration_ ) ; const int n2 = calibration_.dim() ;
}
Matrix D2d_camera(const Point3& point) const { Matrix D(2,n1+n2+3) ;
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
const int n1 = calibrated_.dim() ; sub(D,0,2,0,n1) = D_2d_pose ;
const int n2 = calibration_.dim() ; sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
Matrix D(2,n1+n2) ; sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
return D;
}
sub(D,0,2,0,n1) = D_2d_pose ; //inline size_t dim() { return Camera::dim() + Calibration::dim() ; }
sub(D,0,2,n1,n1+n2) = D_2d_calibration ; inline size_t dim() const { return calibrated().dim() + calibration().dim() ; }
return D; static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; }
}
Matrix D2d_3d(const Point3& point) const {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
return D_2d_intrinsic * D_intrinsic_3d;
}
Matrix D2d_camera_3d(const Point3& point) const {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d;
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2+3) ;
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
return D;
}
//inline size_t dim() { return Camera::dim() + Calibration::dim() ; }
inline size_t dim() const { return calibrated().dim() + calibration().dim() ; }
static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; }
private: private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & BOOST_SERIALIZATION_NVP(calibrated_); ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(calibration_); ar & BOOST_SERIALIZATION_NVP(calibration_);
} }
}; };
typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera; typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera; typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera;

28
gtsam/geometry/Point2.cpp
View File

@ -22,22 +22,22 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2); INSTANTIATE_LIE(Point2);
/* ************************************************************************* */ /* ************************************************************************* */
void Point2::print(const string& s) const { void Point2::print(const string& s) const {
cout << s << "(" << x_ << ", " << y_ << ")" << endl; cout << s << "(" << x_ << ", " << y_ << ")" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const { bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Point2::norm() const { double Point2::norm() const {
return sqrt(x_*x_ + y_*y_); return sqrt(x_*x_ + y_*y_);
} }
} // namespace gtsam } // namespace gtsam

189
gtsam/geometry/Point2.h
View File

@ -23,127 +23,122 @@
namespace gtsam { namespace gtsam {
/** /**
* A 2D point * A 2D point
* Complies with the Testable Concept * Complies with the Testable Concept
* Functional, so no set functions: once created, a point is constant. * Functional, so no set functions: once created, a point is constant.
* @ingroup geometry * @ingroup geometry
*/ */
class Point2 { class Point2 {
public: public:
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2; static const size_t dimension = 2;
private: private:
double x_, y_; double x_, y_;
public: public:
Point2(): x_(0), y_(0) {} Point2(): x_(0), y_(0) {}
Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {}
Point2(double x, double y): x_(x), y_(y) {} Point2(double x, double y): x_(x), y_(y) {}
Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); }
/** dimension of the variable - used to autodetect sizes */ /** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return dimension; }
/** print with optional string */ /** print with optional string */
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** 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 */ /** "Compose", just adds the coordinates of two points. With optional derivatives */
inline size_t dim() const { return dimension; } inline Point2 compose(const Point2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(2);
if(H2) *H2 = eye(2);
return *this + p2;
}
/** "Compose", just adds the coordinates of two points. With optional derivatives */ /** identity */
inline Point2 compose(const Point2& p2, inline static Point2 identity() {
boost::optional<Matrix&> H1=boost::none, return Point2();
boost::optional<Matrix&> H2=boost::none) const { }
if(H1) *H1 = eye(2);
if(H2) *H2 = eye(2);
return *this + p2;
}
/** "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; }
/** Exponential map around identity - just create a Point2 from a vector */ /** Updates a with tangent space delta */
static inline Point2 Expmap(const Vector& v) { return Point2(v); } inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
/** Log map around identity - just return the Point2 as a vector */ /// Local coordinates of manifold neighborhood around current value
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
// Manifold requirements /** Lie requirements */
inline Point2 retract(const Vector& v) const { return expmap(v); } /** Exponential map around identity - just create a Point2 from a vector */
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/** expmap around identity */ /** Log map around identity - just return the Point2 as a vector */
inline static Point2 Retract(const Vector& v) { return Expmap(v); } static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Point2& t2) const { return logmap(t2); }
/** Unretract around identity */ /** "Between", subtracts point coordinates */
inline static Vector Unretract(const Point2& t) { return Logmap(t); } inline Point2 between(const Point2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(2);
if(H2) *H2 = eye(2);
return p2 - (*this);
}
/** "Between", subtracts point coordinates */ /** get functions for x, y */
inline Point2 between(const Point2& p2, double x() const {return x_;}
boost::optional<Matrix&> H1=boost::none, double y() const {return y_;}
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(2);
if(H2) *H2 = eye(2);
return p2 - (*this);
}
/** get functions for x, y */ /** return vectorized form (column-wise) */
double x() const {return x_;} Vector vector() const { return Vector_(2, x_, y_); }
double y() const {return y_;}
/** return vectorized form (column-wise) */ /** operators */
Vector vector() const { return Vector_(2, x_, y_); } inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
inline void operator *= (double s) {x_*=s;y_*=s;}
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
inline Point2 operator- () const {return Point2(-x_,-y_);}
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
/** operators */ /** norm of point */
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} double norm() const;
inline void operator *= (double s) {x_*=s;y_*=s;}
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
inline Point2 operator- () const {return Point2(-x_,-y_);}
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
/** norm of point */ /** creates a unit vector */
double norm() const; Point2 unit() const { return *this/norm(); }
/** creates a unit vector */ /** distance between two points */
Point2 unit() const { return *this/norm(); } inline double dist(const Point2& p2) const {
return (p2 - *this).norm();
}
/** distance between two points */ private:
inline double dist(const Point2& p2) const { /** Serialization function */
return (p2 - *this).norm(); friend class boost::serialization::access;
} template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
}
};
private: /** multiply with scalar */
/** Serialization function */ inline Point2 operator*(double s, const Point2& p) {return p*s;}
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
}
};
/** multiply with scalar */
inline Point2 operator*(double s, const Point2& p) {return p*s;}
} }

118
gtsam/geometry/Point3.cpp
View File

@ -20,71 +20,71 @@
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point3); INSTANTIATE_LIE(Point3);
/* ************************************************************************* */ /* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const { bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol); return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Point3::print(const std::string& s) const { void Point3::print(const std::string& s) const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Point3::operator== (const Point3& q) const { bool Point3::operator== (const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const { Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ ); return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const { Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::operator*(double s) const { Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s); return Point3(x_ * s, y_ * s, z_ * s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::operator/(double s) const { Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s); return Point3(x_ / s, y_ / s, z_ / s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::add(const Point3 &q, Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3); if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3); if (H2) *H2 = eye(3,3);
return *this + q; return *this + q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3); if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3); if (H2) *H2 = -eye(3,3);
return *this - q; return *this - q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const { Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_, return Point3( y_*q.z_ - z_*q.y_,
z_*q.x_ - x_*q.z_, z_*q.x_ - x_*q.z_,
x_*q.y_ - y_*q.x_ ); x_*q.y_ - y_*q.x_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Point3::dot(const Point3 &q) const { double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Point3::norm() const { double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ ); return sqrt( x_*x_ + y_*y_ + z_*z_ );
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

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,

521
gtsam/geometry/Pose2.cpp
View File

@ -25,287 +25,278 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2); INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2); GTSAM_CONCEPT_POSE_INST(Pose2);
static const Matrix I3 = eye(3), Z12 = zeros(1,2); static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Pose2::matrix() const { Matrix Pose2::matrix() const {
Matrix R = r_.matrix(); Matrix R = r_.matrix();
R = stack(2, &R, &Z12); R = stack(2, &R, &Z12);
Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
return collect(2, &R, &T); return collect(2, &R, &T);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Pose2::print(const string& s) const { void Pose2::print(const string& s) const {
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl; cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const { bool Pose2::equals(const Pose2& q, double tol) const {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) { Pose2 Pose2::Expmap(const Vector& xi) {
assert(xi.size() == 3); assert(xi.size() == 3);
Point2 v(xi(0),xi(1)); Point2 v(xi(0),xi(1));
double w = xi(2); double w = xi(2);
if (std::abs(w) < 1e-10) if (std::abs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]); return Pose2(xi[0], xi[1], xi[2]);
else { else {
Rot2 R(Rot2::fromAngle(w)); Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - R.rotate(v_ortho)) / w; Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t); return Pose2(R, t);
}
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) { Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r(); const Rot2& R = p.r();
const Point2& t = p.t(); const Point2& t = p.t();
double w = R.theta(); double w = R.theta();
if (std::abs(w) < 1e-10) if (std::abs(w) < 1e-10)
return Vector_(3, t.x(), t.y(), w); return Vector_(3, t.x(), t.y(), w);
else { else {
double c_1 = R.c()-1.0, s = R.s(); double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s; double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p; Point2 v = (w/det) * p;
return Vector_(3, v.x(), v.y(), w); return Vector_(3, v.x(), v.y(), w);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
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
assert(v.size() == 3);
/* ************************************************************************* */ return compose(Pose2(v[0], v[1], v[2]));
Pose2 Pose2::Retract(const Vector& v) {
assert(v.size() == 3);
return Pose2(v[0], v[1], v[2]);
}
/* ************************************************************************* */
Vector Pose2::Unretract(const Pose2& p) {
return Vector_(3, p.x(), p.y(), p.theta());
}
#endif #endif
}
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map Vector Pose2::localCoordinates(const Pose2& p2) const {
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) #ifdef SLOW_BUT_CORRECT_EXPMAP
Matrix Pose2::AdjointMap() const { return Logmap(between(p2));
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); #else
return Matrix_(3,3, Pose2 r = between(p2);
c, -s, y, return Vector_(3, r.x(), r.y(), r.theta());
s, c, -x, #endif
0.0, 0.0, 1.0 }
);
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const { // Calculate Adjoint map
if (H1) *H1 = -AdjointMap(); // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); Matrix Pose2::AdjointMap() const {
} double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return Matrix_(3,3,
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
);
}
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
Point2 Pose2::transform_to(const Point2& point, if (H1) *H1 = -AdjointMap();
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
Point2 d = point - t_; }
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q; /* ************************************************************************* */
if (H1) *H1 = Matrix_(2, 3, // see doc/math.lyx, SE(2) section
-1.0, 0.0, q.y(), Point2 Pose2::transform_to(const Point2& point,
0.0, -1.0, -q.x()); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H2) *H2 = r_.transpose(); Point2 d = point - t_;
return q; Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = Matrix_(2, 3,
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3;
return (*this)*p2;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + t_;
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix_(3,3,
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0);
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = transform_to(point, H1, H2);
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return result;
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 result = bearing(point.t(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
}
return result;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point2 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
Matrix D_result_d;
if(std::abs(r) > 1e-10)
D_result_d = Matrix_(1, 2, x / r, y / r);
else {
D_result_d = Matrix_(1,2, 1.0, 1.0);
}
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return r;
}
/* ************************************************************************* */
double Pose2::range(const Pose2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double r = range(point.t(), H1, H2);
if (H2) {
// NOTE: expmap changes the orientation of expmap direction,
// so we must rotate the jacobian
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
}
return r;
}
/* *************************************************************************
* New explanation, from scan.ml
* It finds the angle using a linear method:
* q = Pose2::transform_from(p) = t + R*p
* We need to remove the centroids from the data to find the rotation
* using dp=[dpx;dpy] and q=[dqx;dqy] we have
* |dqx| |c -s| |dpx| |dpx -dpy| |c|
* | | = | | * | | = | | * | | = H_i*cs
* |dqy| |s c| |dpy| |dpy dpx| |s|
* where the Hi are the 2*2 matrices. Then we will minimize the criterion
* J = \sum_i norm(q_i - H_i * cs)
* Taking the derivative with respect to cs and setting to zero we have
* cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
* The hessian is diagonal and just divides by a constant, but this
* normalization constant is irrelevant, since we take atan2.
* i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
* The translation is then found from the centroids
* as they also satisfy cq = t + R*cp, hence t = cq - R*cp
*/
boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
size_t n = pairs.size();
if (n<2) return boost::none; // we need at least two pairs
// calculate centroids
Point2 cp,cq;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
cp += pair.first;
cq += pair.second;
}
double f = 1.0/n;
cp *= f; cq *= f;
// calculate cos and sin
double c=0,s=0;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
Point2 dq = pair.first - cp;
Point2 dp = pair.second - cq;
c += dp.x() * dq.x() + dp.y() * dq.y();
s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
} }
/* ************************************************************************* */ // calculate angle and translation
// see doc/math.lyx, SE(2) section double theta = atan2(s,c);
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1, Rot2 R = Rot2::fromAngle(theta);
boost::optional<Matrix&> H2) const { Point2 t = cq - R*cp;
// TODO: inline and reuse? return Pose2(R, t);
if(H1) *H1 = p2.inverse().AdjointMap(); }
if(H2) *H2 = I3;
return (*this)*p2;
}
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
return q + t_;
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix_(3,3,
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0);
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = transform_to(point, H1, H2);
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return result;
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 result = bearing(point.t(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
}
return result;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point2 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
Matrix D_result_d;
if(std::abs(r) > 1e-10)
D_result_d = Matrix_(1, 2, x / r, y / r);
else {
D_result_d = Matrix_(1,2, 1.0, 1.0);
}
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return r;
}
/* ************************************************************************* */
double Pose2::range(const Pose2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double r = range(point.t(), H1, H2);
if (H2) {
// NOTE: expmap changes the orientation of expmap direction,
// so we must rotate the jacobian
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
}
return r;
}
/* *************************************************************************
* New explanation, from scan.ml
* It finds the angle using a linear method:
* q = Pose2::transform_from(p) = t + R*p
* We need to remove the centroids from the data to find the rotation
* using dp=[dpx;dpy] and q=[dqx;dqy] we have
* |dqx| |c -s| |dpx| |dpx -dpy| |c|
* | | = | | * | | = | | * | | = H_i*cs
* |dqy| |s c| |dpy| |dpy dpx| |s|
* where the Hi are the 2*2 matrices. Then we will minimize the criterion
* J = \sum_i norm(q_i - H_i * cs)
* Taking the derivative with respect to cs and setting to zero we have
* cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
* The hessian is diagonal and just divides by a constant, but this
* normalization constant is irrelevant, since we take atan2.
* i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
* The translation is then found from the centroids
* as they also satisfy cq = t + R*cp, hence t = cq - R*cp
*/
boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
size_t n = pairs.size();
if (n<2) return boost::none; // we need at least two pairs
// calculate centroids
Point2 cp,cq;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
cp += pair.first;
cq += pair.second;
}
double f = 1.0/n;
cp *= f; cq *= f;
// calculate cos and sin
double c=0,s=0;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
Point2 dq = pair.first - cp;
Point2 dp = pair.second - cq;
c += dp.x() * dq.x() + dp.y() * dq.y();
s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
}
// calculate angle and translation
double theta = atan2(s,c);
Rot2 R = Rot2::fromAngle(theta);
Point2 t = cq - R*cp;
return Pose2(R, t);
}
/* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

277
gtsam/geometry/Pose2.h
View File

@ -27,148 +27,139 @@
namespace gtsam { namespace gtsam {
/** /**
* A 2D pose (Point2,Rot2) * A 2D pose (Point2,Rot2)
* @ingroup geometry * @ingroup geometry
*/ */
class Pose2 { class Pose2 {
public: public:
static const size_t dimension = 3; static const size_t dimension = 3;
/** Pose Concept requirements */ /** Pose Concept requirements */
typedef Rot2 Rotation; typedef Rot2 Rotation;
typedef Point2 Translation; typedef Point2 Translation;
private: private:
Rot2 r_; Rot2 r_;
Point2 t_; Point2 t_;
public: public:
/** default constructor = origin */ /** default constructor = origin */
Pose2() {} // default is origin Pose2() {} // default is origin
/** copy constructor */ /** copy constructor */
Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
/** /**
* construct from (x,y,theta) * construct from (x,y,theta)
* @param x x coordinate * @param x x coordinate
* @param y y coordinate * @param y y coordinate
* @param theta angle with positive X-axis * @param theta angle with positive X-axis
*/ */
Pose2(double x, double y, double theta) : Pose2(double x, double y, double theta) :
r_(Rot2::fromAngle(theta)), t_(x, y) { r_(Rot2::fromAngle(theta)), t_(x, y) {
} }
/** construct from rotation and translation */ /** construct from rotation and translation */
Pose2(double theta, const Point2& t) : Pose2(double theta, const Point2& t) :
r_(Rot2::fromAngle(theta)), t_(t) { r_(Rot2::fromAngle(theta)), t_(t) {
} }
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
/** Constructor from 3*3 matrix */ /** Constructor from 3*3 matrix */
Pose2(const Matrix &T) : Pose2(const Matrix &T) :
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
assert(T.rows() == 3 && T.cols() == 3); assert(T.rows() == 3 && T.cols() == 3);
} }
/** Construct from canonical coordinates (Lie algebra) */ /** Construct from canonical coordinates (Lie algebra) */
Pose2(const Vector& v) { Pose2(const Vector& v) {
*this = Expmap(v); *this = Expmap(v);
} }
/** print with optional string */ /** print with optional string */
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** assert equality up to a tolerance */ /** assert equality up to a tolerance */
bool equals(const Pose2& pose, double tol = 1e-9) const; bool equals(const Pose2& pose, double tol = 1e-9) const;
/** compose syntactic sugar */ /** compose syntactic sugar */
inline Pose2 operator*(const Pose2& p2) const { inline Pose2 operator*(const Pose2& p2) const {
return Pose2(r_*p2.r(), t_ + r_*p2.t()); return Pose2(r_*p2.r(), t_ + r_*p2.t());
} }
/** dimension of the variable - used to autodetect sizes */ /** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return dimension; }
/** Lie requirements */ /** Lie requirements */
/** return DOF, dimensionality of tangent space = 3 */ /** return DOF, dimensionality of tangent space = 3 */
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** /**
* inverse transformation with derivatives * inverse transformation with derivatives
*/ */
Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const; Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
/** /**
* compose this transformation onto another (first *this and then p2) * compose this transformation onto another (first *this and then p2)
* With optional derivatives * With optional derivatives
*/ */
Pose2 compose(const Pose2& p2, Pose2 compose(const Pose2& p2,
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;
/// MATLAB version returns shared pointer /// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> compose_(const Pose2& p2) { boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(new Pose2(compose(p2))); return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
} }
/** 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); }
/** /** Real versions of Expmap/Logmap */
* Inverse of retraction, from SE(2) to se(2) static Pose2 Expmap(const Vector& xi);
*/ static Vector Logmap(const Pose2& p);
static Vector Unretract(const Pose2& p);
/** Real versions of Expmap/Logmap */ /** default implementations of binary functions */
static Pose2 Expmap(const Vector& xi); Pose2 retract(const Vector& v) const;
static Vector Logmap(const Pose2& p); Vector localCoordinates(const Pose2& p2) const;
/** default implementations of binary functions */ /**
inline Pose2 retract(const Vector& v) const { return compose(Retract(v)); } * Return relative pose between p1 and p2, in p1 coordinate frame
inline Vector unretract(const Pose2& p2) const { return Unretract(between(p2));} */
Pose2 between(const Pose2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** non-approximated versions of expmap/logmap */ /// MATLAB version returns shared pointer
inline Pose2 expmap(const Vector& v) const { return compose(Expmap(v)); } boost::shared_ptr<Pose2> between_(const Pose2& p2) {
inline Vector logmap(const Pose2& p2) const { return Logmap(between(p2));} return boost::shared_ptr<Pose2>(new Pose2(between(p2)));
}
/** /** return transformation matrix */
* Return relative pose between p1 and p2, in p1 coordinate frame Matrix matrix() const;
*/
Pose2 between(const Pose2& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// MATLAB version returns shared pointer /**
boost::shared_ptr<Pose2> between_(const Pose2& p2) { * Return point coordinates in pose coordinate frame
return boost::shared_ptr<Pose2>(new Pose2(between(p2))); */
} Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** return transformation matrix */ /**
Matrix matrix() const; * Return point coordinates in global frame
*/
/** Point2 transform_from(const Point2& point,
* Return point coordinates in pose coordinate frame boost::optional<Matrix&> H1=boost::none,
*/ boost::optional<Matrix&> H2=boost::none) const;
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Return point coordinates in global frame
*/
Point2 transform_from(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** /**
* Calculate bearing to a landmark * Calculate bearing to a landmark
@ -212,7 +203,7 @@ namespace gtsam {
*/ */
Matrix AdjointMap() const; Matrix AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const { inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3); assert(xi.size() == 3);
return AdjointMap()*xi; return AdjointMap()*xi;
} }
@ -230,41 +221,41 @@ namespace gtsam {
0., 0., 0.); 0., 0., 0.);
} }
/** get functions for x, y, theta */ /** get functions for x, y, theta */
inline double x() const { return t_.x(); } inline double x() const { return t_.x(); }
inline double y() const { return t_.y(); } inline double y() const { return t_.y(); }
inline double theta() const { return r_.theta(); } inline double theta() const { return r_.theta(); }
/** shorthand access functions */ /** shorthand access functions */
inline const Point2& t() const { return t_; } inline const Point2& t() const { return t_; }
inline const Rot2& r() const { return r_; } inline const Rot2& r() const { return r_; }
/** full access functions required by Pose concept */ /** full access functions required by Pose concept */
inline const Point2& translation() const { return t_; } inline const Point2& translation() const { return t_; }
inline const Rot2& rotation() const { return r_; } inline const Rot2& rotation() const { return r_; }
private: private:
// Serialization function // Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(r_); ar & BOOST_SERIALIZATION_NVP(r_);
} }
}; // Pose2 }; // Pose2
/** specialization for pose2 wedge function (generic template in Lie.h) */ /** specialization for pose2 wedge function (generic template in Lie.h) */
template <> template <>
inline Matrix wedge<Pose2>(const Vector& xi) { inline Matrix wedge<Pose2>(const Vector& xi) {
return Pose2::wedge(xi(0),xi(1),xi(2)); return Pose2::wedge(xi(0),xi(1),xi(2));
} }
/** /**
* Calculate pose between a vector of 2D point correspondences (p,q) * Calculate pose between a vector of 2D point correspondences (p,q)
* where q = Pose2::transform_from(p) = t + R*p * where q = Pose2::transform_from(p) = t + R*p
*/ */
typedef std::pair<Point2,Point2> Point2Pair; typedef std::pair<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs); boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
} // namespace gtsam } // namespace gtsam

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