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
* the function exists and throw compile-time errors.
*
* Returns dimensionality of the tangent space
* 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
* Expmap around identity
* 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
* static Vector Logmap(const T& p);
@ -48,18 +36,13 @@
* between_default()
* 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
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Group.h>
namespace gtsam {
@ -101,32 +84,14 @@ namespace gtsam {
*/
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 */
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 */
Vector logmap_identity_ret = T::Logmap(t);
/** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
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
* 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>
T BCH(const T& X, const T& Y) {
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
@ -185,13 +151,19 @@ namespace gtsam {
} // namespace gtsam
/**
* Macros for using the ManifoldConcept
* Macros for using the LieConcept
* - 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
*/
/// TODO: find better name for "INST" macro, something like "UNIT" or similar
#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
#define GTSAM_CONCEPT_LIE_INST(T) \
template class gtsam::ManifoldConcept<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 {
/** default constructor - should be unnecessary */
LieScalar() {}
/** default constructor */
LieScalar() : d_(0.0) {}
/** wrap a double */
LieScalar(double d) : d_(d) {}
@ -45,55 +45,51 @@ namespace gtsam {
return fabs(expected.d_ - d_) <= tol;
}
/**
* Returns dimensionality of the tangent space
* with member and static versions
*/
// Manifold requirements
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return 1; }
inline static size_t Dim() { return 1; }
/**
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
inline LieScalar expmap(const Vector& v) const { return LieScalar(d_ + v(0)); }
/** Update the LieScalar with a tangent space update */
inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
/** expmap around identity */
static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); }
/**
* Returns Log map
* Default Implementation calls global binary function
*/
inline Vector logmap(const LieScalar& lp) const {
return Vector_(1, lp.d_ - d_);
// Group requirements
/** identity */
inline static LieScalar identity() {
return LieScalar();
}
/** 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 */
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 */
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 */
inline static LieScalar Retract(const Vector& v) { return Expmap(v); }
/**
* 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); }
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); }
private:
double d_;

48
gtsam/base/LieVector.h
View File

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

21
gtsam/base/Manifold.h
View File

@ -26,19 +26,11 @@
* Returns Retraction update of T
* T retract(const Vector& v) const;
*
* Retract around identity
* static T Retract(const Vector& v);
*
* Returns inverse retraction operation
* A default implementation of unretract(*this, lp) is available:
* Vector unretract(const T& lp) const;
*
* Unretract around identity
* static Vector Unretract(const T& p);
* Vector localCoordinates(const T& lp) const;
*
*/
#pragma once
#include <string>
@ -74,16 +66,10 @@ namespace gtsam {
*/
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);
/** Unretract around identity */
Vector unretract_identity_ret = T::Unretract(t);
Vector localCoords_ret = t.localCoordinates(t2);
}
};
@ -97,6 +83,5 @@ namespace gtsam {
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* 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_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>
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 */
template<class T>
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);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.unretract(h(x.retract(d)));
d(j) += delta; Vector hxplus = hx.localCoordinates(h(x.retract(d)));
d(j) -= 2*delta; Vector hxmin = hx.localCoordinates(h(x.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -151,8 +151,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1.retract(d),x2));
d(j) -= 2*delta; Vector hxmin = 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.localCoordinates(h(x1.retract(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -215,8 +215,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1,x2.retract(d)));
d(j) -= 2*delta; Vector hxmin = 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.localCoordinates(h(x1,x2.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -281,8 +281,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1.retract(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = 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.localCoordinates(h(x1.retract(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -346,8 +346,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1, x2.retract(d),x3));
d(j) -= 2*delta; Vector hxmin = 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.localCoordinates(h(x1, x2.retract(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -411,8 +411,8 @@ namespace gtsam {
Vector d = zero(n);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = hx.unretract(h(x1, x2, x3.retract(d)));
d(j) -= 2*delta; Vector hxmin = 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.localCoordinates(h(x1, x2, x3.retract(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
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;
GTSAM_CONCEPT_TESTABLE_INST(LieScalar)
GTSAM_CONCEPT_MANIFOLD_INST(LieScalar)
GTSAM_CONCEPT_LIE_INST(LieScalar)
const double tol=1e-9;
@ -40,10 +39,10 @@ TEST( testLieScalar, construction ) {
}
/* ************************************************************************* */
TEST( testLieScalar, logmap ) {
TEST( testLieScalar, localCoordinates ) {
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;
GTSAM_CONCEPT_TESTABLE_INST(LieVector)
GTSAM_CONCEPT_MANIFOLD_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) ; }
/* ************************************************************************* */
Vector Cal3Bundler::unretract(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(); }
Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { return vector() - T2.vector(); }
}

5
gtsam/geometry/Cal3Bundler.h
View File

@ -52,10 +52,7 @@ public:
Matrix D2d_intrinsic_calibration(const Point2& p) const ;
Cal3Bundler retract(const Vector& d) const ;
Vector unretract(const Cal3Bundler& T2) const ;
static Cal3Bundler Retract(const Vector& v) ;
static Vector Unretract(const Cal3Bundler& p) ;
Vector localCoordinates(const Cal3Bundler& T2) const ;
int dim() const { 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) ; }
/* ************************************************************************* */
Vector Cal3DS2::unretract(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(); }
Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return vector() - T2.vector(); }
}
/* ************************************************************************* */

5
gtsam/geometry/Cal3DS2.h
View File

@ -63,10 +63,7 @@ public:
Matrix D2d_calibration(const Point2& p) const ;
Cal3DS2 retract(const Vector& d) const ;
Vector unretract(const Cal3DS2& T2) const ;
static Cal3DS2 Retract(const Vector& v) ;
static Vector Unretract(const Cal3DS2& p) ;
Vector localCoordinates(const Cal3DS2& T2) const ;
int dim() const { 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));
}
/// Retraction from origin
inline static Cal3_S2 Retract(const Vector& d) {
Cal3_S2 c;
return c.retract(d);
}
/// Unretraction for the calibration
Vector unretract(const Cal3_S2& T2) const {
Vector localCoordinates(const Cal3_S2& T2) const {
return vector() - T2.vector();
}
/// Unretraction from origin
inline static Vector Unretract(const Cal3_S2& T2) {
return T2.vector();
}
private:
/// 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 {
return pose().unretract(T2.pose()) ;
}
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Retract(const Vector& v) {
return CalibratedCamera(Pose3::Retract(v)) ;
}
/* ************************************************************************* */
Vector CalibratedCamera::Unretract(const CalibratedCamera& p) {
return Pose3::Unretract(p.pose()) ;
Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const {
return pose().localCoordinates(T2.pose()) ;
}
/* ************************************************************************* */

8
gtsam/geometry/CalibratedCamera.h
View File

@ -61,13 +61,7 @@ namespace gtsam {
CalibratedCamera retract(const Vector& d) const;
/// Return canonical coordinate
Vector unretract(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);
Vector localCoordinates(const CalibratedCamera& T2) const;
/// Lie group dimensionality
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 {
return CalibratedCameraT(pose().retract(d), k_) ;
}
Vector unretract(const CalibratedCameraT& T2) const {
return pose().unretract(T2.pose()) ;
Vector localCoordinates(const CalibratedCameraT& T2) const {
return pose().localCoordinates(T2.pose()) ;
}
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>
class GeneralCameraT {
private:
Camera calibrated_; // Calibrated camera
Calibration calibration_; // Calibration
private:
Camera calibrated_; // Calibrated camera
Calibration calibration_; // Calibration
public:
GeneralCameraT(){}
GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {}
GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {}
GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {}
GeneralCameraT(const Pose3& pose) : calibrated_(pose) {}
GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {}
public:
GeneralCameraT(){}
GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {}
GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {}
GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {}
GeneralCameraT(const Pose3& pose) : calibrated_(pose) {}
GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {}
// Vector Initialization
GeneralCameraT(const Vector &v) :
calibrated_(sub(v, 0, Camera::Dim())),
calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
// Vector Initialization
GeneralCameraT(const Vector &v) :
calibrated_(sub(v, 0, Camera::Dim())),
calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
inline const Pose3& pose() const { return calibrated_.pose(); }
inline const Camera& calibrated() const { return calibrated_; }
inline const Calibration& calibration() const { return calibration_; }
inline const Pose3& pose() const { return calibrated_.pose(); }
inline const Camera& calibrated() const { return calibrated_; }
inline const Calibration& calibration() const { return calibration_; }
std::pair<Point2,bool> projectSafe(
const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
std::pair<Point2,bool> projectSafe(
const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Point3 cameraPoint = calibrated_.pose().transform_to(P);
return std::pair<Point2, bool>(
project(P,H1,H2),
cameraPoint.z() > 0);
Point3 cameraPoint = calibrated_.pose().transform_to(P);
return std::pair<Point2, bool>(
project(P,H1,H2),
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 {
Point2 intrinsic = calibration_.calibrate(projection);
Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
return calibrated_.pose().transform_from(cameraPoint);
}
Matrix H1_k, H1_pose, H2_pt;
Point2 projection = project(P, H1_pose, H1_k, H2_pt);
if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k);
if ( H2 ) *H2 = H2_pt;
/**
* 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;
}
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 {
// dump functions for compilation for now
bool equals (const GeneralCameraT &camera, double tol = 1e-9) const {
return calibrated_.equals(camera.calibrated_, tol) &&
calibration_.equals(camera.calibration_, tol) ;
}
if (!H1 && !H2) {
Point2 intrinsic = calibrated_.project(P);
return calibration_.uncalibrate(intrinsic) ;
}
void print(const std::string& s = "") const {
calibrated_.pose().print(s + ".camera.") ;
calibration_.print(s + ".calibration.") ;
}
Matrix H1_k, H1_pose, H2_pt;
Point2 projection = project(P, H1_pose, H1_k, H2_pt);
if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k);
if ( H2 ) *H2 = H2_pt;
GeneralCameraT retract(const Vector &v) const {
Vector v1 = sub(v,0,Camera::Dim());
Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
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
bool equals (const GeneralCameraT &camera, double tol = 1e-9) const {
return calibrated_.equals(camera.calibrated_, tol) &&
calibration_.equals(camera.calibration_, tol) ;
}
inline GeneralCameraT compose(const Pose3 &p) const {
return GeneralCameraT( pose().compose(p), calibration_ ) ;
}
void print(const std::string& s = "") const {
calibrated_.pose().print(s + ".camera.") ;
calibration_.print(s + ".calibration.") ;
}
Matrix D2d_camera(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);
GeneralCameraT retract(const Vector &v) const {
Vector v1 = sub(v,0,Camera::Dim());
Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
}
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2) ;
Vector unretract(const GeneralCameraT &C) const {
const Vector v1(calibrated().unretract(C.calibrated())),
v2(calibration().unretract(C.calibration()));
return concatVectors(2,&v1,&v2) ;
}
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
return D;
}
static GeneralCameraT Retract(const Vector& v) {
return GeneralCameraT(
Camera::Retract(sub(v,0,Camera::Dim())),
Calibration::Retract(sub(v,Camera::Dim(), Camera::Dim()+Calibration::Dim()))
);
}
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;
}
static Vector Unretract(const GeneralCameraT& p) {
const Vector v1(Camera::Unretract(p.calibrated())),
v2(Calibration::Unretract(p.calibration()));
return concatVectors(2,&v1,&v2);
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;
inline GeneralCameraT compose(const Pose3 &p) const {
return GeneralCameraT( pose().compose(p), calibration_ ) ;
}
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D2d_camera(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(2,n1+n2+3) ;
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2) ;
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;
}
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
return D;
}
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() ; }
//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:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(calibration_);
}
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(calibration_);
}
};
};
typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera;

28
gtsam/geometry/Point2.cpp
View File

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

189
gtsam/geometry/Point2.h
View File

@ -23,127 +23,122 @@
namespace gtsam {
/**
* A 2D point
* Complies with the Testable Concept
* Functional, so no set functions: once created, a point is constant.
* @ingroup geometry
*/
class Point2 {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
private:
double x_, y_;
public:
Point2(): x_(0), y_(0) {}
Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {}
Point2(double x, double y): x_(x), y_(y) {}
Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); }
/**
* A 2D point
* Complies with the Testable Concept
* Functional, so no set functions: once created, a point is constant.
* @ingroup geometry
*/
class Point2 {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
private:
double x_, y_;
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
public:
Point2(): x_(0), y_(0) {}
Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {}
Point2(double x, double y): x_(x), y_(y) {}
Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); }
/** print with optional string */
void print(const std::string& s = "") const;
/** dimension of the variable - used to autodetect sizes */
inline static size_t Dim() { return dimension; }
/** equals with an tolerance, prints out message if unequal*/
bool equals(const Point2& q, double tol = 1e-9) const;
/** print with optional string */
void print(const std::string& s = "") const;
/** Lie requirements */
/** equals with an tolerance, prints out message if unequal*/
bool equals(const Point2& q, double tol = 1e-9) const;
/** Size of the tangent space of the Lie type */
inline size_t dim() const { return dimension; }
// Group requirements
/** "Compose", just adds the coordinates of two points. With optional derivatives */
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 */
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;
}
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
inline Point2 inverse() const { return Point2(-x_, -y_); }
/** identity */
inline static Point2 identity() {
return Point2();
}
/** Binary expmap - just adds the points */
inline Point2 expmap(const Vector& v) const { return *this + Point2(v); }
/** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */
inline Point2 inverse() const { return Point2(-x_, -y_); }
/** Binary Logmap - just subtracts the points */
inline Vector logmap(const Point2& p2) const { return Logmap(between(p2));}
// Manifold requirements
/** Exponential map around identity - just create a Point2 from a vector */
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/** Size of the tangent space */
inline size_t dim() const { return dimension; }
/** Log map around identity - just return the Point2 as a vector */
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
/** Updates a with tangent space delta */
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
// Manifold requirements
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
inline Point2 retract(const Vector& v) const { return expmap(v); }
/** Lie requirements */
/** expmap around identity */
inline static Point2 Retract(const Vector& v) { return Expmap(v); }
/** Exponential map around identity - just create a Point2 from a vector */
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Point2& t2) const { return logmap(t2); }
/** Log map around identity - just return the Point2 as a vector */
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
/** Unretract around identity */
inline static Vector Unretract(const Point2& t) { return Logmap(t); }
/** "Between", subtracts point coordinates */
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 */
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);
}
/** get functions for x, y */
double x() const {return x_;}
double y() const {return y_;}
/** get functions for x, y */
double x() const {return x_;}
double y() const {return y_;}
/** return vectorized form (column-wise) */
Vector vector() const { return Vector_(2, x_, y_); }
/** return vectorized form (column-wise) */
Vector vector() const { return Vector_(2, x_, y_); }
/** operators */
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 */
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);}
/** norm of point */
double norm() const;
/** norm of point */
double norm() const;
/** creates a unit vector */
Point2 unit() const { return *this/norm(); }
/** creates a unit vector */
Point2 unit() const { return *this/norm(); }
/** distance between two points */
inline double dist(const Point2& p2) const {
return (p2 - *this).norm();
}
/** distance between two points */
inline double dist(const Point2& p2) const {
return (p2 - *this).norm();
}
private:
/** Serialization function */
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:
/** Serialization function */
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;}
/** 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 {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point3);
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point3);
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
/* ************************************************************************* */
void Point3::print(const std::string& s) const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
}
void Point3::print(const std::string& s) const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
}
/* ************************************************************************* */
/* ************************************************************************* */
bool Point3::operator== (const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
bool Point3::operator== (const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
return *this - q;
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_,
z_*q.x_ - x_*q.z_,
x_*q.y_ - y_*q.x_ );
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
}
/* ************************************************************************* */
double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ );
}
/* ************************************************************************* */
/* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = eye(3,3);
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);
if (H2) *H2 = -eye(3,3);
return *this - q;
}
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q) const {
return Point3( y_*q.z_ - z_*q.y_,
z_*q.x_ - x_*q.z_,
x_*q.y_ - y_*q.x_ );
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q) const {
return ( x_*q.x_ + y_*q.y_ + z_*q.z_ );
}
/* ************************************************************************* */
double Point3::norm() const {
return sqrt( x_*x_ + y_*y_ + z_*z_ );
}
/* ************************************************************************* */
} // namespace gtsam

19
gtsam/geometry/Point3.h
View File

@ -60,6 +60,11 @@ namespace gtsam {
/** return DOF, dimensionality of tangent space */
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() */
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 */
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
inline Point3 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Point3 Retract(const Vector& v) { return Expmap(v); }
inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Point3& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Point3& t) { return Logmap(t); }
inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); }
/** Between using the default implementation */
inline Point3 between(const Point3& p2,

521
gtsam/geometry/Pose2.cpp
View File

@ -25,287 +25,278 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
R = stack(2, &R, &Z12);
Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
return collect(2, &R, &T);
}
/* ************************************************************************* */
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
R = stack(2, &R, &Z12);
Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
return collect(2, &R, &T);
}
/* ************************************************************************* */
void Pose2::print(const string& s) const {
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
}
/* ************************************************************************* */
void Pose2::print(const string& s) const {
cout << s << "(" << t_.x() << ", " << t_.y() << ", " << r_.theta() << ")" << endl;
}
/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
}
/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) {
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
if (std::abs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]);
else {
Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t);
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) {
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
if (std::abs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]);
else {
Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t);
}
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return Vector_(3, t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return Vector_(3, v.x(), v.y(), w);
}
}
/* ************************************************************************* */
Vector Pose2::Logmap(const Pose2& p) {
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
if (std::abs(w) < 1e-10)
return Vector_(3, t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return Vector_(3, v.x(), v.y(), w);
}
}
/* ************************************************************************* */
/* ************************************************************************* */
Pose2 Pose2::retract(const Vector& v) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
/* ************************************************************************* */
// 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);
}
return compose(Expmap(v));
#else
/* ************************************************************************* */
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());
}
assert(v.size() == 3);
return compose(Pose2(v[0], v[1], v[2]));
#endif
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
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
);
}
/* ************************************************************************* */
Vector Pose2::localCoordinates(const Pose2& p2) const {
#ifdef SLOW_BUT_CORRECT_EXPMAP
return Logmap(between(p2));
#else
Pose2 r = between(p2);
return Vector_(3, r.x(), r.y(), r.theta());
#endif
}
/* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
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
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
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;
/* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
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 !! :-(
}
/* ************************************************************************* */
// 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;
}
// calculate angle and translation
double theta = atan2(s,c);
Rot2 R = Rot2::fromAngle(theta);
Point2 t = cq - R*cp;
return Pose2(R, t);
}
/* ************************************************************************* */
// 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

277
gtsam/geometry/Pose2.h
View File

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

62
gtsam/geometry/Pose3.cpp
View File

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

19
gtsam/geometry/Pose3.h
View File

@ -91,6 +91,11 @@ namespace gtsam {
/** Dimensionality of the tangent space */
inline size_t dim() const { return dimension; }
/** identity */
inline static Pose3 identity() {
return Pose3();
}
/**
* Derivative of inverse
*/
@ -115,28 +120,16 @@ namespace gtsam {
Point3 transform_to(const Point3& p,
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 */
Pose3 retract(const Vector& d) const;
/** Logarithm map around another pose T1 */
Vector unretract(const Pose3& T2) const;
Vector localCoordinates(const Pose3& T2) const;
/** non-approximated versions of Expmap/Logmap */
static Pose3 Expmap(const Vector& xi);
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
* as well as optionally the derivatives

25
gtsam/geometry/Rot2.h
View File

@ -127,6 +127,11 @@ namespace gtsam {
return dimension;
}
/** identity */
inline static Rot2 identity() {
return Rot2();
}
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
@ -148,30 +153,14 @@ namespace gtsam {
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
inline Rot2 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Rot2 Retract(const Vector& v) { return Expmap(v); }
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Rot2& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Rot2& t) { return Logmap(t); }
inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
/** Between using the default implementation */
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);
}
/** identity */
inline static Rot3 identity() {
return Rot3();
}
// Log map at identity - return the canonical coordinates of this rotation
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
inline Rot3 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static Rot3 Retract(const Vector& v) { return Expmap(v); }
inline Rot3 retract(const Vector& v) const { return compose(Expmap(v)); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const Rot3& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const Rot3& t) { return Logmap(t); }
inline Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); }
// 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;
}
/** 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 {
return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
*camera.K_, tol);
}
// Manifold requirements - use existing expmap/logmap
inline StereoCamera retract(const Vector& v) const { return expmap(v); }
inline static StereoCamera Retract(const Vector& v) { return Expmap(v); }
inline Vector unretract(const StereoCamera& t2) const { return logmap(t2); }
inline static Vector Unretract(const StereoCamera& t) { return Logmap(t); }
inline StereoCamera retract(const Vector& v) const {
return StereoCamera(pose().retract(v), K_);
}
inline Vector localCoordinates(const StereoCamera& t2) const {
return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_));
}
Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none,

24
gtsam/geometry/StereoPoint2.h
View File

@ -86,6 +86,11 @@ namespace gtsam {
return *this + p1;
}
/** identity */
inline static StereoPoint2 identity() {
return StereoPoint2();
}
/** inverse */
inline StereoPoint2 inverse() const {
return StereoPoint2()- (*this);
@ -101,29 +106,14 @@ namespace gtsam {
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
inline StereoPoint2 retract(const Vector& v) const { return expmap(v); }
/** expmap around identity */
inline static StereoPoint2 Retract(const Vector& v) { return Expmap(v); }
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
/**
* Returns inverse retraction
*/
inline Vector unretract(const StereoPoint2& t2) const { return logmap(t2); }
/** Unretract around identity */
inline static Vector Unretract(const StereoPoint2& t) { return Logmap(t); }
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
inline StereoPoint2 between(const StereoPoint2& p2) const {
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
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 ?
}
}

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

@ -23,7 +23,6 @@ using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_MANIFOLD_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), H2));
EXPECT(assert_equal(Point2(5,7), p1.expmap(Vector_(2, 4.,5.))));
EXPECT(assert_equal(Vector_(2, 3.,3.), p1.logmap(p2)));
EXPECT(assert_equal(Point2(5,7), p1.retract(Vector_(2, 4.,5.))));
EXPECT(assert_equal(Vector_(2, 3.,3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */
@ -50,7 +49,7 @@ TEST( Point2, expmap)
Vector d(2);
d(0) = 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));
}

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

@ -21,7 +21,6 @@
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point3)
GTSAM_CONCEPT_MANIFOLD_INST(Point3)
GTSAM_CONCEPT_LIE_INST(Point3)
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), H2));
EXPECT(assert_equal(Point3(5,7,9), p1.expmap(Vector_(3, 4.,5.,6.))));
EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.logmap(p2)));
EXPECT(assert_equal(Point3(5,7,9), p1.retract(Vector_(3, 4.,5.,6.))));
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
GTSAM_CONCEPT_TESTABLE_INST(Pose2)
GTSAM_CONCEPT_MANIFOLD_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) {
Point2 p;
@ -59,10 +53,10 @@ TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 origin;
Vector d12 = t1.unretract(t2);
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.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*origin.retract(d21)));
}
@ -83,7 +77,7 @@ TEST(Pose2, retract) {
TEST(Pose2, expmap) {
Pose2 pose(M_PI_2, Point2(1, 2));
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));
}
@ -91,7 +85,7 @@ TEST(Pose2, expmap) {
TEST(Pose2, expmap2) {
Pose2 pose(M_PI_2, Point2(1, 2));
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));
}
@ -117,12 +111,12 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */
TEST(Pose2, expmap0) {
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);
#else
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
#endif
Pose2 actual = pose * Pose2::Retract(Vector_(3, 0.01, -0.015, 0.018));
//#else
// Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
//#endif
Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -183,7 +177,7 @@ TEST(Pose2, logmap) {
#else
Vector expected = Vector_(3, 0.01, -0.015, 0.018);
#endif
Vector actual = pose0.unretract(pose);
Vector actual = pose0.localCoordinates(pose);
EXPECT(assert_equal(expected, actual, 1e-5));
}
@ -192,7 +186,7 @@ TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
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));
}

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

@ -25,7 +25,6 @@ using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Pose3)
GTSAM_CONCEPT_MANIFOLD_INST(Pose3)
GTSAM_CONCEPT_LIE_INST(Pose3)
static Point3 P(0.2,0.7,-2);
@ -66,9 +65,9 @@ TEST( Pose3, expmap_a_full)
Pose3 id;
Vector v = zero(6);
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;
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;
Vector v = zero(6);
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;
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);
double t = norm_2(w);
if (t < 1e-5)
return Pose3(Rot3(), Point3::Retract(v));
return Pose3(Rot3(), Point3::Expmap(v));
else {
Matrix W = skewSymmetric(w/t);
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 t2 = T3;
Pose3 origin;
Vector d12 = t1.logmap(t2);
EXPECT(assert_equal(t2, t1.expmap(d12)));
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
// EXPECT(assert_equal(t2, retract(origin,d12)*t1));
Vector d21 = t2.logmap(t1);
EXPECT(assert_equal(t1, t2.expmap(d21)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
// 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
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), Point3(2,1,0)), x2.expmap(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(0,0,0), l1), expmap_default<Pose3>(x1, 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)), expmap_default<Pose3>(x3, sqrt(2) * x_step), tol));
}
/* ************************************************************************* */

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

@ -23,7 +23,6 @@
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot2)
GTSAM_CONCEPT_MANIFOLD_INST(Rot2)
GTSAM_CONCEPT_LIE_INST(Rot2)
Rot2 R(Rot2::fromAngle(0.1));
@ -89,7 +88,7 @@ TEST( Rot2, equals)
TEST( Rot2, expmap)
{
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 rot(Rot2::fromAngle(M_PI));
Vector expected = Vector_(1, M_PI_2);
Vector actual = rot0.logmap(rot);
Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual));
}

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

@ -26,7 +26,6 @@
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_MANIFOLD_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3)
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
@ -142,7 +141,7 @@ TEST( Rot3, rodriguez4)
TEST( Rot3, expmap)
{
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;
// log behaves correctly
Vector d12 = gR1.logmap(gR2);
CHECK(assert_equal(gR2, gR1.expmap(d12)));
Vector d12 = gR1.localCoordinates(gR2);
CHECK(assert_equal(gR2, gR1.retract(d12)));
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
Vector d21 = gR2.logmap(gR1);
CHECK(assert_equal(gR1, gR2.expmap(d21)));
Vector d21 = gR2.localCoordinates(gR1);
CHECK(assert_equal(gR1, gR2.retract(d21)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)

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

@ -15,7 +15,6 @@
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_MANIFOLD_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: currently only logmaps elements in both configs
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));
unretract(cp, ordering, delta);
localCoordinates(cp, ordering, delta);
return delta;
}
/* ************************************************************************* */
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;
BOOST_FOREACH(const KeyValue& value, cp) {
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;
/** 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) */
void unretract(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
void localCoordinates(const LieValues& cp, const Ordering& ordering, VectorValues& delta) const;
// 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 {
const size_t p = X::Dim();
if (H1) *H1 = eye(p);
return value_.unretract(x1);
return value_.localCoordinates(x1);
}
/** Print */
@ -628,7 +628,7 @@ public:
const size_t p = X::Dim();
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
return x1.unretract(x2);
return x1.localCoordinates(x2);
}
private:

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -120,7 +120,7 @@ namespace gtsam {
size_t nj = feasible_.dim();
if (allow_error_) {
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)) {
if (H) *H = eye(nj);
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 */
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));
unretract(cp, ordering, delta);
localCoordinates(cp, ordering, delta);
return delta;
}
/** logmap each element */
void unretract(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.unretract(cp.first_, ordering, delta);
second_.unretract(cp.second_, ordering, delta);
void localCoordinates(const TupleValues<VALUES1, VALUES2>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.localCoordinates(cp.first_, ordering, delta);
second_.localCoordinates(cp.second_, ordering, delta);
}
/**
@ -322,14 +322,14 @@ namespace gtsam {
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));
unretract(cp, ordering, delta);
localCoordinates(cp, ordering, delta);
return delta;
}
void unretract(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
first_.unretract(cp.first_, ordering, delta);
void localCoordinates(const TupleValuesEnd<VALUES>& cp, const Ordering& ordering, VectorValues& delta) const {
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&> H2 = boost::none) const {
X hx = x1.between(x2, H1, H2);
return measured_.unretract(hx);
return measured_.localCoordinates(hx);
}
inline const X measured() const {

2
gtsam/slam/BetweenFactor.h
View File

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

9
gtsam/slam/PartialPriorFactor.h
View File

@ -118,16 +118,17 @@ namespace gtsam {
/** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = zeros(this->dim(), p.dim());
Vector full_unretraction = T::Unretract(p);
Vector masked_unretraction = zero(this->dim());
// FIXME: this was originally the generic retraction - may not produce same results
Vector full_logmap = T::Logmap(p);
Vector masked_logmap = zero(this->dim());
size_t masked_idx=0;
for (size_t i=0;i<mask_.size();++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;
++masked_idx;
}
return masked_unretraction - prior_;
return masked_logmap - prior_;
}
// 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 {
if (H) (*H) = eye(p.dim());
// manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.unretract(p);
return prior_.localCoordinates(p);
}
private:

4
gtsam/slam/simulated2DOriented.h
View File

@ -69,7 +69,7 @@ namespace gtsam {
/// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
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,
boost::optional<Matrix&> H1 = boost::none,
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(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 expected = z.unretract(t1.between(t2));
Vector expected = z.localCoordinates(t1.between(t2));
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 prediction.unretract(p2);
return prediction.localCoordinates(p2);
}
};

6
tests/testTupleValues.cpp
View File

@ -203,7 +203,7 @@ TEST(TupleValues, zero_expmap_logmap)
// Check log
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));
}
@ -460,7 +460,7 @@ TEST(TupleValues, expmap)
expected.insert(l2k, Point2(10.3, 11.4));
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));
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)));
}
/* ************************************************************************* */