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

Removed remaining global lie functions on lie objects and configs, switched the Lie base class to a simple concept check function, fixed build script for examples. ISAM2 and MastSLAM verified as compiling.

release/4.3a0
Alex Cunningham 2010-08-26 19:55:40 +00:00
parent 9dd1d6bc10
commit 23a30f8475
70 changed files with 1156 additions and 1121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

427
.cproject
View File

File diff suppressed because it is too large Load Diff

2
Makefile.am
View File

@ -11,7 +11,7 @@ ACLOCAL_AMFLAGS = -I m4
AUTOMAKE_OPTIONS = foreign nostdinc
# All the sub-directories that need to be built
SUBDIRS = CppUnitLite colamd spqr_mini base geometry inference linear nonlinear slam . tests wrap
SUBDIRS = CppUnitLite colamd spqr_mini base geometry inference linear nonlinear slam . tests wrap examples
# And the corresponding libraries produced
SUBLIBS = colamd/libcolamd.la \

25
base/Lie-inl.h
View File

@ -10,29 +10,10 @@
#include <gtsam/base/Lie.h>
#define INSTANTIATE_LIE(T) \
template T between(const T&, const T&); \
template Vector logmap(const T&, const T&); \
template T expmap(const T&, const Vector&); \
template T between_default(const T&, const T&); \
template Vector logmap_default(const T&, const T&); \
template T expmap_default(const T&, const Vector&); \
template bool equal(const T&, const T&, double); \
template bool equal(const T&, const T&); \
template class Lie<T>;
namespace gtsam {
/**
* Returns Exponential mapy
* This is for matlab wrapper
*/
template<class T>
T Lie<T>::expmap(const Vector& v) const {
return gtsam::expmap(*((T*)this),v);
}
/**
* Returns Log map
*/
template<class T>
Vector Lie<T>::logmap(const T& lp) const {
return gtsam::logmap(*((T*)this),lp);
}
}

154
base/Lie.h
View File

@ -18,102 +18,118 @@ namespace gtsam {
* for better performance.
*/
/* Exponential map about identity */
/** Compute l0 s.t. l2=l1*l0 */
template<class T>
T expmap(const Vector& v) { return T::Expmap(v); }
/* Logmap (inverse exponential map) about identity */
template<class T>
Vector logmap(const T& p) { return T::Logmap(p); }
/** Compute l1 s.t. l2=l1*l0 */
template<class T>
inline T between(const T& l1, const T& l2) { return compose(inverse(l1),l2); }
inline T between_default(const T& l1, const T& l2) { return l1.inverse().compose(l2); }
/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
template<class T>
inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); }
inline Vector logmap_default(const T& l0, const T& lp) { return T::Logmap(l0.between(lp)); }
/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
template<class T>
inline T expmap(const T& t, const Vector& d) { return compose(t,expmap<T>(d)); }
inline T expmap_default(const T& t, const Vector& d) { return t.compose(T::Expmap(d)); }
/**
* Base class for Lie group type
* This class uses the Curiously Recurring Template design pattern to allow
* for static polymorphism.
* This class uses the Curiously Recurring Template design pattern to allow for
* concept checking using a private function.
*
* T is the derived Lie type, like Point2, Pose3, etc.
*
* By convention, we use capital letters to designate a static function
*
* FIXME: Need to find a way to check for actual implementations in T
* so that there are no recursive function calls. This could be handled
* by not using the same name
*/
template <class T>
class Lie {
public:
private:
/**
* Returns dimensionality of the tangent space
*/
inline size_t dim() const {
return static_cast<const T*>(this)->dim();
}
/** concept checking function - implement the functions this demands */
static void concept_check(const T& t) {
/**
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
T expmap(const Vector& v) const;
/** assignment */
T t2 = t;
/** expmap around identity */
static T Expmap(const Vector& v) {
return T::Expmap(v);
}
/**
* Returns dimensionality of the tangent space
*/
size_t dim_ret = t.dim();
/**
* Returns Log map
* Default Implementation calls global binary function
*/
Vector logmap(const T& lp) const;
/**
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
T expmap_ret = t.expmap(gtsam::zero(dim_ret));
/** Logmap around identity */
static Vector Logmap(const T& p) {
return T::Logmap(p);
}
/** expmap around identity */
T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
/** compose with another object */
inline T compose(const T& p) const {
return static_cast<const T*>(this)->compose(p);
}
/**
* Returns Log map
* Default Implementation calls global binary function
*/
Vector logmap_ret = t.logmap(t2);
/** invert the object and yield a new one */
inline T inverse() const {
return static_cast<const T*>(this)->inverse();
}
/** 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();
}
/**
* The necessary functions to implement for Lie are defined
* below with additional details as to the interface. The
* concept checking function above 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 */
// 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);
/**
* Compute l0 s.t. l2=l1*l0, where (*this) is l1
* A default implementation of between(*this, lp) is available:
* between_default()
*/
// T between(const T& l2) const;
/** compose with another object */
// inline T compose(const T& p) const;
/** invert the object and yield a new one */
// inline T inverse() const;
};
/** get the dimension of an object with a global function */
template<class T>
inline size_t dim(const T& object) {
return object.dim();
}
/** compose two Lie types */
template<class T>
inline T compose(const T& p1, const T& p2) {
return p1.compose(p2);
}
/** invert an object */
template<class T>
inline T inverse(const T& p) {
return p.inverse();
}
/** Call print on the object */
template<class T>
inline void print(const T& object, const std::string& s = "") {

33
base/LieVector.cpp Normal file
View File

@ -0,0 +1,33 @@
/**
* @file LieVector.cpp
* @brief Implementations for LieVector functions
* @author Alex Cunningham
*/
#include <stdarg.h>
#include <gtsam/base/LieVector.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
LieVector::LieVector(size_t m, const double* const data)
: Vector(Vector_(m,data))
{
}
/* ************************************************************************* */
LieVector::LieVector(size_t m, ...)
: Vector(m)
{
va_list ap;
va_start(ap, m);
for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
}
} // \namespace gtsam

25
base/LieVector.h
View File

@ -23,6 +23,15 @@ namespace gtsam {
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
/** wrap a double */
LieVector(double d) : Vector(Vector_(1, d)) {}
/** constructor with size and initial data, row order ! */
LieVector(size_t m, const double* const data);
/** Specify arguments directly, as in Vector_() - always force these to be doubles */
LieVector(size_t m, ...);
/** get the underlying vector */
inline Vector vector() const {
return static_cast<Vector>(*this);
@ -47,25 +56,33 @@ namespace gtsam {
* Returns Exponential map update of T
* Default implementation calls global binary function
*/
LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
inline LieVector expmap(const Vector& v) const { return LieVector(vector() + v); }
/** expmap around identity */
static LieVector Expmap(const Vector& v) { return LieVector(v); }
static inline LieVector Expmap(const Vector& v) { return LieVector(v); }
/**
* Returns Log map
* Default Implementation calls global binary function
*/
Vector logmap(const LieVector& lp) const { return *this - lp; }
inline Vector logmap(const LieVector& lp) const {
// return Logmap(between(lp)); // works
return lp.vector() - vector();
}
/** Logmap around identity - just returns with default cast back */
static Vector Logmap(const LieVector& p) { return p; }
static inline Vector Logmap(const LieVector& p) { return p; }
/** compose with another object */
inline LieVector compose(const LieVector& p) const {
return LieVector(vector() + p);
}
/** between operation */
inline LieVector between(const LieVector& l2) const {
return LieVector(l2.vector() - vector());
}
/** invert the object and yield a new one */
inline LieVector inverse() const {
return LieVector(-1.0 * vector());

3
base/Makefile.am
View File

@ -26,7 +26,8 @@ endif
headers += Testable.h TestableAssertions.h numericalDerivative.h
# Lie Groups
headers += Lie.h Lie-inl.h LieVector.h
headers += Lie.h Lie-inl.h lieProxies.h
sources += LieVector.cpp
check_PROGRAMS += tests/testLieVector
# Data structures

48
base/lieProxies.h Normal file
View File

@ -0,0 +1,48 @@
/**
* @file lieProxies.h
* @brief Provides convenient mappings of common member functions for testing
* @author Alex Cunningham
*/
#pragma once
/**
* Global functions in a separate testing namespace
*
* These should not be used outside of tests, as they are just remappings
* of the original functions. We use these to avoid needing to do
* too much boost::bind magic or writing a bunch of separate proxy functions.
*
* Don't expect all classes to work for all of these functions.
*/
namespace gtsam {
namespace testing {
/** binary functions */
template<class T>
T between(const T& t1, const T& t2) { return t1.between(t2); }
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(); }
/** rotation functions */
template<class T, class P>
P rotate(const T& r, const P& pt) { return r.rotate(pt); }
template<class T, class P>
P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
}
}

39
base/numericalDerivative.h
View File

@ -83,12 +83,12 @@ namespace gtsam {
Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x, double delta=1e-5) {
Y hx = h(x);
double factor = 1.0/(2.0*delta);
const size_t m = dim(hx), n = dim(x);
const size_t m = hx.dim(), n = x.dim();
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x,d)));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x,d)));
d(j) += delta; Vector hxplus = hx.logmap(h(x.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x.expmap(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -137,12 +137,12 @@ namespace gtsam {
const X1& x1, const X2& x2, double delta=1e-5) {
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = dim(hx), n = dim(x1);
const size_t m = hx.dim(), n = x1.dim();
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2));
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1.expmap(d),x2));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -198,16 +198,15 @@ namespace gtsam {
template<class Y, class X1, class X2>
Matrix numericalDerivative22
(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta=1e-5)
{
const X1& x1, const X2& x2, double delta=1e-5) {
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = dim(hx), n = dim(x2);
const size_t m = hx.dim(), n = x2.dim();
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = logmap(hx, h(x1,expmap(x2,d)));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1,expmap(x2,d)));
d(j) += delta; Vector hxplus = hx.logmap(h(x1,x2.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1,x2.expmap(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -268,12 +267,12 @@ namespace gtsam {
{
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = dim(hx), n = dim(x1);
const size_t m = hx.dim(), n = x1.dim();
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2,x3));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2,x3));
d(j) += delta; Vector hxplus = hx.logmap(h(x1.expmap(d),x2,x3));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1.expmap(d),x2,x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -333,12 +332,12 @@ namespace gtsam {
{
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = dim(hx), n = dim(x2);
const size_t m = hx.dim(), n = x2.dim();
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = logmap(hx, h(x1, expmap(x2,d),x3));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1, expmap(x2,d),x3));
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2.expmap(d),x3));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1, x2.expmap(d),x3));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -398,12 +397,12 @@ namespace gtsam {
{
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = dim(hx), n = dim(x3);
const size_t m = hx.dim(), n = x3.dim();
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = logmap(hx, h(x1, x2, expmap(x3,d)));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1, x2, expmap(x3,d)));
d(j) += delta; Vector hxplus = hx.logmap(h(x1, x2, x3.expmap(d)));
d(j) -= 2*delta; Vector hxmin = hx.logmap(h(x1, x2, x3.expmap(d)));
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}

14
base/tests/testLieVector.cpp
View File

@ -19,6 +19,20 @@ TEST( testLieVector, construction ) {
EXPECT(assert_equal(lie1, lie2));
}
/* ************************************************************************* */
TEST( testLieVector, other_constructors ) {
Vector init = Vector_(2, 10.0, 20.0);
LieVector exp(init);
LieVector a(2,10.0,20.0);
double data[] = {10,20};
LieVector b(2,data);
LieVector c(2.3), c_exp(LieVector(1, 2.3));
EXPECT(assert_equal(exp, a));
EXPECT(assert_equal(exp, b));
EXPECT(assert_equal(b, a));
EXPECT(assert_equal(c_exp, c));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

3
configure.ac
View File

@ -4,7 +4,7 @@
AC_PREREQ(2.59)
AC_INIT(gtsam, 0.0.0, dellaert@cc.gatech.edu)
AM_INIT_AUTOMAKE(gtsam, 0.0.0)
AC_OUTPUT(Makefile CppUnitLite/Makefile colamd/Makefile spqr_mini/Makefile base/Makefile inference/Makefile linear/Makefile geometry/Makefile nonlinear/Makefile slam/Makefile tests/Makefile wrap/Makefile)
AC_OUTPUT(Makefile CppUnitLite/Makefile colamd/Makefile spqr_mini/Makefile base/Makefile inference/Makefile linear/Makefile geometry/Makefile nonlinear/Makefile slam/Makefile tests/Makefile wrap/Makefile examples/Makefile)
AC_CONFIG_MACRO_DIR([m4])
AC_CONFIG_HEADER([config.h])
AC_CONFIG_SRCDIR([CppUnitLite/Test.cpp])
@ -18,6 +18,7 @@ AC_CONFIG_SRCDIR([nonlinear/ConstraintOptimizer.cpp])
AC_CONFIG_SRCDIR([slam/pose2SLAM.cpp])
AC_CONFIG_SRCDIR([tests/testSQP.cpp])
AC_CONFIG_SRCDIR([wrap/wrap.cpp])
AC_CONFIG_SRCDIR([examples/SimpleRotation.cpp])
# Check for OS
AC_CANONICAL_HOST # needs to be called at some point earlier

2
examples/Makefile.am
View File

@ -9,7 +9,6 @@ AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
noinst_PROGRAMS =
# Examples
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
@ -18,6 +17,7 @@ noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation va
# rules to build local programs
#----------------------------------------------------------------------------------------------------
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CPPFLAGS = -I$(boost) -I$(BORG_SRCROOT)
LDADD = ../libgtsam.la
AM_DEFAULT_SOURCE_EXT = .cpp
if USE_LDL

7
examples/SimpleRotation.cpp
View File

@ -10,6 +10,7 @@
* @Author: Alex Cunningham
*/
#include <iostream>
#include <math.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
@ -43,6 +44,7 @@ int main() {
// Create a factor
Rot2 prior1 = Rot2::fromAngle(30 * degree);
prior1.print("Goal Angle");
SharedDiagonal model1 = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key1(1);
PriorFactor<Config, Key> factor1(key1, prior1, model1);
@ -53,11 +55,12 @@ int main() {
// and an initial estimate
Config initialEstimate;
initialEstimate.insert(1, Rot2::fromAngle(20 * degree));
initialEstimate.insert(key1, Rot2::fromAngle(20 * degree));
initialEstimate.print("Initialization");
// create an ordering
Optimizer::shared_config result = Optimizer::optimizeLM(graph, initialEstimate, Optimizer::LAMBDA);
GTSAM_PRINT(*result);
result->print("Final config");
return 0;
}

27
geometry/Cal3_S2.h
View File

@ -99,8 +99,18 @@ namespace gtsam {
return Point2((1/fx_)*(u-u0_ - (s_/fy_)*(v-v0_)), (1/fy_)*(v-v0_));
}
/** friends */
friend Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d);
/**
* return DOF, dimensionality of tangent space
*/
inline size_t dim() const { return 5; }
/**
* Given 5-dim tangent vector, create new calibration
*/
inline Cal3_S2 expmap(const Vector& d) const {
return Cal3_S2(fx_ + d(0), fy_ + d(1),
s_ + d(2), u0_ + d(3), v0_ + d(4));
}
private:
/** Serialization function */
@ -118,17 +128,4 @@ namespace gtsam {
typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
/**
* return DOF, dimensionality of tangent space
*/
inline size_t dim(const Cal3_S2&) { return 5; }
/**
* Given 5-dim tangent vector, create new calibration
*/
inline Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d) {
return Cal3_S2(cal.fx_ + d(0), cal.fy_ + d(1),
cal.s_ + d(2), cal.u0_ + d(3), cal.v0_ + d(4));
}
}

2
geometry/CalibratedCamera.cpp
View File

@ -14,7 +14,7 @@ namespace gtsam {
pose_(pose) {
}
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(expmap<Pose3>(v)) {}
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
CalibratedCamera::~CalibratedCamera() {}

46
geometry/Point2.h
View File

@ -46,11 +46,23 @@ namespace gtsam {
/** Size of the tangent space of the Lie type */
inline size_t dim() const { return dimension; }
/** "Compose", just adds the coordinates of two points. */
Point2 compose(const Point2& p1) const { return *this+p1; }
/** "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() */
Point2 inverse() const { return Point2(-x_, -y_); }
inline Point2 inverse() const { return Point2(-x_, -y_); }
/** Binary expmap - just adds the points */
inline Point2 expmap(const Vector& v) const { return *this + Point2(v); }
/** Binary Logmap - just subtracts the points */
inline Vector logmap(const Point2& p2) const { return Logmap(between(p2));}
/** Exponential map around identity - just create a Point2 from a vector */
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
@ -58,6 +70,15 @@ namespace gtsam {
/** 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()); }
/** "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_;}
@ -92,25 +113,6 @@ namespace gtsam {
}
};
/** Lie group functions */
/** "Compose", just adds the coordinates of two points. */
inline Point2 compose(const Point2& p1, const Point2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
if(H1) *H1 = eye(2);
if(H2) *H2 = eye(2);
return compose(p1, p2);
}
/** "Between", subtracts point coordinates */
inline Point2 between(const Point2& p1, const Point2& p2) { return p2-p1; }
inline Point2 between(const Point2& p1, const Point2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
if(H1) *H1 = -eye(2);
if(H2) *H2 = eye(2);
return between(p1, p2);
}
/** multiply with scalar */
inline Point2 operator*(double s, const Point2& p) {return p*s;}
}

9
geometry/Point3.cpp
View File

@ -46,11 +46,6 @@ namespace gtsam {
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
// Point3 Point3::add(const Point3 &q) const {
// return *this + q;
// }
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -59,10 +54,6 @@ namespace gtsam {
return *this + q;
}
/* ************************************************************************* */
// Point3 Point3::sub(const Point3 &q) const {
// return *this - q;
// }
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = eye(3,3);

22
geometry/Point3.h
View File

@ -51,7 +51,13 @@ namespace gtsam {
inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
/** "Compose" - just adds coordinates of two points */
inline Point3 compose(const Point3& p1) const { return *this+p1; }
inline Point3 compose(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) *H1 = eye(3);
if (H2) *H2 = eye(3);
return *this+p2;
}
/** Exponential map at identity - just create a Point3 from x,y,z */
static inline Point3 Expmap(const Vector& v) { return Point3(v); }
@ -59,6 +65,13 @@ 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);}
/** Between using the default implementation */
inline Point3 between(const Point3& p2) const { return between_default(*this, p2); }
/** return vectorized form (column-wise)*/
Vector vector() const {
//double r[] = { x_, y_, z_ };
@ -113,13 +126,6 @@ namespace gtsam {
}
};
/** "Compose" - just adds coordinates of two points */
inline Point3 compose(const Point3& p1, const Point3& p2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2=boost::none) {
if (H1) *H1 = eye(3);
if (H2) *H2 = eye(3);
return p1.compose(p2);
}
/** Syntactic sugar for multiplying coordinates by a scalar s*p */
inline Point3 operator*(double s, const Point3& p) { return p*s;}

28
geometry/Pose2.cpp
View File

@ -46,7 +46,7 @@ namespace gtsam {
else {
Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - rotate(R,v_ortho)) / w;
Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t);
}
}
@ -60,7 +60,7 @@ namespace gtsam {
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (unrotate(R, t) - t);
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return Vector_(3, v.x(), v.y(), w);
}
@ -93,15 +93,11 @@ namespace gtsam {
}
/* ************************************************************************* */
Pose2 Pose2::inverse() const {
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1) {
if (H1) *H1 = -pose.AdjointMap();
return pose.inverse();
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
@ -118,12 +114,12 @@ namespace gtsam {
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 compose(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = inverse(p2).AdjointMap();
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3;
return p1*p2;
return (*this)*p2;
}
/* ************************************************************************* */
@ -141,10 +137,10 @@ namespace gtsam {
}
/* ************************************************************************* */
Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
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 = p1.r(), R2 = p2.r();
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Calculate delta rotation = between(R1,R2)
@ -152,7 +148,7 @@ namespace gtsam {
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - p1.t();
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);

56
geometry/Pose2.h
View File

@ -76,11 +76,18 @@ namespace gtsam {
/** return DOF, dimensionality of tangent space = 3 */
inline size_t dim() const { return dimension; }
/** inverse of a pose */
Pose2 inverse() const;
/**
* inverse transformation with derivatives
*/
Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
/** compose with another pose */
inline Pose2 compose(const Pose2& p) const { return *this * p; }
/**
* 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;
/** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) { return transform_from(point);}
@ -95,6 +102,17 @@ namespace gtsam {
*/
static Vector Logmap(const Pose2& p);
/** default implementations of binary functions */
inline Pose2 expmap(const Vector& v) const { return gtsam::expmap_default(*this, v); }
inline Vector logmap(const Pose2& p2) const { return gtsam::logmap_default(*this, 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;
@ -102,13 +120,15 @@ namespace gtsam {
* 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;
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;
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate bearing to a landmark
@ -116,7 +136,8 @@ namespace gtsam {
* @return 2D rotation \in SO(2)
*/
Rot2 bearing(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to a landmark
@ -124,7 +145,8 @@ namespace gtsam {
* @return range (double)
*/
double range(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate Adjoint map
@ -173,23 +195,5 @@ namespace gtsam {
return Pose2::wedge(xi(0),xi(1),xi(2));
}
/**
* inverse transformation
*/
Pose2 inverse(const Pose2& pose, boost::optional<Matrix&> H1);
/**
* compose this transformation onto another (first p1 and then p2)
*/
Pose2 compose(const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2 = boost::none);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p1, const Pose2& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
} // namespace gtsam

68
geometry/Pose3.cpp
View File

@ -51,16 +51,16 @@ namespace gtsam {
// get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
double theta = norm(w);
double theta = w.norm();
if (theta < 1e-5) {
static const Rot3 I;
return Pose3(I, v);
}
else {
Point3 n(w/theta); // axis unit vector
Rot3 R = rodriguez(n.vector(),theta);
double vn = dot(n,v); // translation parallel to n
Point3 n_cross_v = cross(n,v); // points towards axis
Rot3 R = Rot3::rodriguez(n.vector(),theta);
double vn = n.dot(v); // translation parallel to n
Point3 n_cross_v = n.cross(v); // points towards axis
Point3 t = (n_cross_v - R*n_cross_v)/theta + vn*n;
return Pose3(R, t);
}
@ -80,11 +80,11 @@ namespace gtsam {
}
Pose3 expmap(const Pose3& T, const Vector& d) {
return compose(T,expmap<Pose3>(d));
return compose(T,Pose3::Expmap(d));
}
Vector logmap(const Pose3& T1, const Pose3& T2) {
return logmap(between(T1,T2));
return Pose3::logmap(between(T1,T2));
}
#else
@ -109,15 +109,15 @@ namespace gtsam {
* pose. Increments the offset and rotation independently given a translation and
* canonical rotation coordinates. Created to match ML derivatives, but
* superseded by the correct exponential map story in .cpp */
Pose3 expmap(const Pose3& p0, const Vector& d) {
return Pose3(expmap(p0.rotation(), sub(d, 0, 3)),
expmap(p0.translation(), sub(d, 3, 6)));
Pose3 Pose3::expmap(const Vector& d) const {
return Pose3(R_.expmap(sub(d, 0, 3)),
t_.expmap(sub(d, 3, 6)));
}
/** Independently computes the logmap of the translation and rotation. */
Vector logmap(const Pose3& p0, const Pose3& pp) {
const Vector r(logmap(p0.rotation(), pp.rotation())),
t(logmap(p0.translation(), pp.translation()));
Vector Pose3::logmap(const Pose3& pp) const {
const Vector r(R_.logmap(pp.rotation())),
t(t_.logmap(pp.translation()));
return concatVectors(2, &r, &t);
}
@ -133,17 +133,12 @@ namespace gtsam {
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(gtsam::inverse(pose.R_));
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
Point3 t = pose.transform_to(t_);
return Pose3(cRv, t);
}
/* ************************************************************************* */
// Point3 Pose3::transform_from(const Pose3& pose, const Point3& p) {
// return pose.rotation() * p + pose.translation();
// }
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) {
@ -161,12 +156,6 @@ namespace gtsam {
return R_ * p + t_;
}
/* ************************************************************************* */
// Point3 Pose3::transform_to(const Pose3& pose, const Point3& p) {
// Point3 sub = p - pose.translation();
// return pose.rotation().unrotate(sub);
// }
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -187,8 +176,8 @@ namespace gtsam {
}
/* ************************************************************************* */
Pose3 compose(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Pose3 Pose3::compose(const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) {
#ifdef CORRECT_POSE3_EXMAP
*H1 = AdjointMap(inverse(p2));
@ -198,7 +187,7 @@ namespace gtsam {
Matrix DR_R1 = R2.transpose(), DR_t1 = Z3;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt;
p1.transform_from(t2, Dt, boost::none);
transform_from(t2, Dt, boost::none);
*H1 = gtsam::stack(2, &DR, &Dt);
#endif
}
@ -207,43 +196,42 @@ namespace gtsam {
*H2 = I6;
#else
Matrix R1 = p1.rotation().matrix();
Matrix R1 = rotation().matrix();
Matrix DR = collect(2, &I3, &Z3);
Matrix Dt = collect(2, &Z3, &R1);
*H2 = gtsam::stack(2, &DR, &Dt);
#endif
}
return p1.compose(p2);
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 inverse(const Pose3& p, boost::optional<Matrix&> H1) {
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1)
#ifdef CORRECT_POSE3_EXMAP
{ *H1 = - AdjointMap(p); }
#else
{
const Rot3& R = p.rotation();
const Point3& t = p.translation();
Matrix Rt = R.transpose();
Matrix DR_R1 = -R.matrix(), DR_t1 = Z3;
Matrix Dt_R1 = -skewSymmetric(R.unrotate(t).vector()), Dt_t1 = -Rt;
Matrix Rt = R_.transpose();
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
*H1 = gtsam::stack(2, &DR, &Dt);
}
#endif
return p.inverse();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt*(-t_));
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Matrix invH;
Pose3 invp1 = inverse(p1, invH);
Pose3 invp1 = inverse(invH);
Matrix composeH1;
Pose3 result = compose(invp1, p2, composeH1, H2);
Pose3 result = invp1.compose(p2, composeH1, H2);
if (H1) *H1 = composeH1 * invH;
return result;
}

55
geometry/Pose3.h
View File

@ -77,14 +77,18 @@ namespace gtsam {
/** Dimensionality of the tangent space */
inline size_t dim() const { return dimension; }
/** Find the inverse pose s.t. inverse(p)*p = Pose3() */
inline Pose3 inverse() const {
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt*(-t_));
}
/**
* Derivative of inverse
*/
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/** composes two poses */
inline Pose3 compose(const Pose3& t) const { return *this * t; }
/**
* composes two poses (first (*this) then p2)
* with optional derivatives
*/
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Point3& p,
@ -105,6 +109,20 @@ namespace gtsam {
* coordinates of a pose. */
static Vector Logmap(const Pose3& p);
/** Exponential map around another pose */
Pose3 expmap(const Vector& d) const;
/** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
@ -137,29 +155,6 @@ namespace gtsam {
}
}; // Pose3 class
/** Exponential map around another pose */
Pose3 expmap(const Pose3& T, const Vector& d);
/** Logarithm map around another pose T1 */
Vector logmap(const Pose3& T1, const Pose3& T2);
/**
* Derivatives of compose
*/
Pose3 compose(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Derivative of inverse
*/
Pose3 inverse(const Pose3& p, boost::optional<Matrix&> H1);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where

9
geometry/Rot2.h
View File

@ -112,6 +112,15 @@ 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));}
/** Between using the default implementation */
inline Rot2 between(const Rot2& p2) const { return between_default(*this, p2); }
/** return 2*2 rotation matrix */
Matrix matrix() const;

20
geometry/Rot3.cpp
View File

@ -173,12 +173,6 @@ namespace gtsam {
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
}
// /* ************************************************************************* */
// Point3 Rot3::unrotate(const Rot3& R, const Point3& p) {
// const Matrix Rt(R.transpose());
// return Rt*p.vector(); // q = Rt*p
// }
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
@ -191,19 +185,19 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 compose (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return R1.compose(R2);
return *this * R2;
}
/* ************************************************************************* */
Rot3 between (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (H1) *H1 = -(R2.transpose()*R1.matrix());
Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between(R1, R2);
return between_default(*this, R2);
}
/* ************************************************************************* */

52
geometry/Rot3.h
View File

@ -142,8 +142,10 @@ namespace gtsam {
/** return DOF, dimensionality of tangent space */
inline size_t dim() const { return dimension; }
/** Compose two rotations */
inline Rot3 compose(const Rot3& R2) const { return *this * R2;}
/** Compose two rotations i.e., R= (*this) * R2
*/
Rot3 compose(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/** Exponential map at identity - create a rotation from canonical coordinates
* using Rodriguez' formula
@ -156,14 +158,26 @@ namespace gtsam {
// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Rot3& R);
/* Find the inverse rotation R^T s.t. inverse(R)*R = I */
inline Rot3 inverse() const {
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
/** 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);}
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const {
if (H1) *H1 = -matrix();
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const {
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
@ -173,15 +187,12 @@ namespace gtsam {
* rotate point from rotated coordinate frame to
* world = R*p
*/
// Point3 rotate(const Point3& p) const
// {return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();}
inline Point3 operator*(const Point3& p) const { return rotate(p);}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
// static inline Point3 rotate(const Rot3& R, const Point3& p) { return R*p;}
Point3 rotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
@ -189,7 +200,6 @@ namespace gtsam {
* rotate point from world to rotated
* frame = R'*p
*/
// static Point3 unrotate(const Rot3& R, const Point3& p);
Point3 unrotate(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
@ -205,24 +215,6 @@ namespace gtsam {
}
};
// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(const Rot3& R, boost::optional<Matrix&> H1) {
if (H1) *H1 = -R.matrix();
return R.inverse();
}
/**
* compose two rotations i.e., R=R1*R2
*/
Rot3 compose (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between (const Rot3& R1, const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'

61
geometry/StereoCamera.cpp
View File

@ -21,18 +21,6 @@ StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2& K, double ba
cy_ = calibration(4);
}
///* ************************************************************************* */
//StereoPoint2 StereoCamera::project(const Point3& point) const {
//
// Point3 cameraPoint = leftCamPose_.transform_to(point);
//
// double d = 1.0 / cameraPoint.z();
// double uL = cx_ + d * fx_ * cameraPoint.x();
// double uR = cx_ + d * fx_ * (cameraPoint.x() - baseline_);
// double v = cy_ + d * fy_ * cameraPoint.y();
// return StereoPoint2(uL,uR,v);
//}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> Dproject_stereo_pose,
@ -110,55 +98,6 @@ Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
//return Matrix_(2, 2, K.fx_, K.s_, 0.000, K.fy_);
}
///* ************************************************************************* */
//Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point) {
// //Point2 intrinsic = project(camera.calibrated_, point); // unused
//
// //Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
// //**** above function call inlined
// Matrix D_cameraPoint_pose;
// Point3 cameraPoint = camera.pose().transform_to(point, D_cameraPoint_pose, boost::none);
// //cout << "D_cameraPoint_pose" << endl;
// //print(D_cameraPoint_pose);
//
// //Point2 intrinsic = project_to_camera(cameraPoint); // unused
// Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
// //cout << "myJacobian" << endl;
// //print(D_intrinsic_cameraPoint);
//
// Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
//
// //****
// //Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
// Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3
//
// Matrix D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
// return D_projection_pose;
//}
///* ************************************************************************* */
//Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point) {
// //Point2 intrinsic = project(camera.calibrated_, point); // unused
//
// //Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
// //**** above function call inlined
// Matrix D_cameraPoint_point;
// Point3 cameraPoint = camera.pose().transform_to(point, boost::none, D_cameraPoint_point);
//
// //Point2 intrinsic = project_to_camera(cameraPoint); // unused
// Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(camera, cameraPoint); // 3x3 Jacobian
//
// Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
//
// //****
// //Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
// Matrix D_projection_intrinsic = Duncalibrate2(camera.K()); // 3x3
//
// Matrix D_projection_point = D_projection_intrinsic * D_intrinsic_point;
// return D_projection_point;
//}
// calibrated cameras
/*
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point) {

22
geometry/StereoCamera.h
View File

@ -47,8 +47,15 @@ namespace gtsam {
boost::optional<Matrix&> Dproject_stereo_pose = boost::none,
boost::optional<Matrix&> Dproject_stereo_point = boost::none) const;
// Matrix Dproject_stereo_pose(const StereoCamera& camera, const Point3& point);
// Matrix Dproject_stereo_point(const StereoCamera& camera, const Point3& point);
/** Dimensionality of the tangent space */
inline size_t dim() const {
return 6;
}
/** Exponential map around p0 */
inline StereoCamera expmap(const Vector& d) const {
return StereoCamera(pose().expmap(d),K(),baseline());
}
private:
/** utility functions */
@ -57,15 +64,4 @@ namespace gtsam {
};
/** Dimensionality of the tangent space */
inline size_t dim(const StereoCamera& obj) {
return 6;
}
/** Exponential map around p0 */
template<> inline StereoCamera expmap<StereoCamera>(const StereoCamera& p0, const Vector& d) {
return StereoCamera(expmap(p0.pose(),d),p0.K(),p0.baseline());
}
}

13
geometry/StereoPoint2.h
View File

@ -96,6 +96,19 @@ namespace gtsam {
static inline Vector Logmap(const StereoPoint2& p) {
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);
}
inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2);
}
};
}

3
geometry/Tensor2.h
View File

@ -35,6 +35,9 @@ public:
T[j] = a(j);
}
/** dimension - TODO: is this right for anything other than 3x3? */
size_t dim() const {return N1 * N2;}
const double & operator()(int i, int j) const {
return T[j](i);
}

7
geometry/tests/testHomography2.cpp
View File

@ -104,7 +104,7 @@ TEST( Homography2, EstimateReverse)
* Assumption is Z is normal to the template, template texture in X-Y plane.
*/
Homography2 patchH(const Pose3& tEc) {
Pose3 cEt = inverse(tEc);
Pose3 cEt = tEc.inverse();
Rot3 cRt = cEt.rotation();
Point3 r1 = cRt.column(1), r2 = cRt.column(2), t = cEt.translation();
@ -128,7 +128,7 @@ Homography2 patchH(const Pose3& tEc) {
/* ************************************************************************* */
namespace gtsam {
size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
Vector toVector(const tensors::Tensor2<3, 3>& H) {
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
@ -162,7 +162,8 @@ TEST( Homography2, patchH)
// GTSAM_PRINT(actual(I,_T));
CHECK(assert_equality(expected(I,_T),actual(I,_T)));
Matrix D = numericalDerivative11<Homography2,Pose3>(patchH, gEc);
// FIXME: this doesn't appear to be tested, and requires that Tensor2 be a Lie object
// Matrix D = numericalDerivative11<Homography2,Pose3>(patchH, gEc);
// print(D,"D");
}

2
geometry/tests/testPoint2.cpp
View File

@ -16,7 +16,7 @@ TEST( Point2, expmap)
Vector d(2);
d(0) = 1;
d(1) = -1;
Point2 a(4, 5), b = expmap(a, d), c(5, 4);
Point2 a(4, 5), b = a.expmap(d), c(5, 4);
CHECK(assert_equal(b,c));
}

217
geometry/tests/testPose2.cpp
View File

@ -7,6 +7,7 @@
#include <gtsam/CppUnitLite/TestHarness.h>
#include <iostream>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
@ -24,7 +25,7 @@ TEST(Pose2, constructors) {
Pose2 origin;
assert_equal(pose,origin);
Pose2 t(M_PI_2+0.018, Point2(1.015, 2.01));
CHECK(assert_equal(t,Pose2(t.matrix())));
EXPECT(assert_equal(t,Pose2(t.matrix())));
}
/* ************************************************************************* */
@ -33,12 +34,12 @@ 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 = logmap(t1,t2);
CHECK(assert_equal(t2, expmap(t1,d12)));
CHECK(assert_equal(t2, t1*expmap(origin,d12)));
Vector d21 = logmap(t2,t1);
CHECK(assert_equal(t1, expmap(t2,d21)));
CHECK(assert_equal(t1, t2*expmap(origin,d21)));
Vector d12 = t1.logmap(t2);
EXPECT(assert_equal(t2, t1.expmap(d12)));
EXPECT(assert_equal(t2, t1*origin.expmap(d12)));
Vector d21 = t2.logmap(t1);
EXPECT(assert_equal(t1, t2.expmap(d21)));
EXPECT(assert_equal(t1, t2*origin.expmap(d21)));
}
/* ************************************************************************* */
@ -50,8 +51,8 @@ TEST(Pose2, expmap) {
#else
Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01));
#endif
Pose2 actual = expmap(pose, Vector_(3, 0.01, -0.015, 0.99));
CHECK(assert_equal(expected, actual, 1e-5));
Pose2 actual = pose.expmap(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
@ -65,9 +66,9 @@ TEST(Pose2, expmap2) {
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4;
Pose2 pose = expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.99));
Pose2 pose = Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.99));
Matrix actual = pose.matrix();
//CHECK(assert_equal(expected, actual));
//EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
@ -79,8 +80,8 @@ TEST(Pose2, expmap0) {
#else
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
#endif
Pose2 actual = pose * expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.018));
CHECK(assert_equal(expected, actual, 1e-5));
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5));
}
#ifdef SLOW_BUT_CORRECT_EXPMAP
@ -96,9 +97,9 @@ namespace screw {
TEST(Pose3, expmap_c)
{
CHECK(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, expmap<Pose2>(screw::xi),1e-6));
CHECK(assert_equal(screw::xi, logmap(screw::expected),1e-6));
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, logmap(screw::expected),1e-6));
}
#endif
@ -112,8 +113,8 @@ TEST(Pose2, logmap) {
#else
Vector expected = Vector_(3, 0.01, -0.015, 0.018);
#endif
Vector actual = logmap(pose0,pose);
CHECK(assert_equal(expected, actual, 1e-5));
Vector actual = pose0.logmap(pose);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
@ -134,15 +135,15 @@ TEST( Pose2, transform_to )
// actual
Matrix actualH1, actualH2;
Point2 actual = pose.transform_to(point, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
EXPECT(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
EXPECT(assert_equal(expectedH2,actualH2));
Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
@ -158,18 +159,18 @@ TEST (Pose2, transform_from)
Point2 actual = pose.transform_from(pt, H1, H2);
Point2 expected(0., 2.);
CHECK(assert_equal(expected, actual));
EXPECT(assert_equal(expected, actual));
Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt, 1e-5);
CHECK(assert_equal(H1_expected, H1));
CHECK(assert_equal(H1_expected, numericalH1));
EXPECT(assert_equal(H1_expected, H1));
EXPECT(assert_equal(H1_expected, numericalH1));
Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt, 1e-5);
CHECK(assert_equal(H2_expected, H2));
CHECK(assert_equal(H2_expected, numericalH2));
EXPECT(assert_equal(H2_expected, H2));
EXPECT(assert_equal(H2_expected, numericalH2));
}
/* ************************************************************************* */
@ -181,30 +182,30 @@ TEST(Pose2, compose_a)
Matrix actualDcompose1;
Matrix actualDcompose2;
Pose2 actual = compose(pose1, pose2, actualDcompose1, actualDcompose2);
Pose2 actual = pose1.compose(pose2, actualDcompose1, actualDcompose2);
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
CHECK(assert_equal(expected, actual));
EXPECT(assert_equal(expected, actual));
Matrix expectedH1 = Matrix_(3,3,
0.0, 1.0, 0.0,
-1.0, 0.0, 2.0,
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
);
Matrix expectedH2 = eye(3);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
CHECK(assert_equal(expectedH1,actualDcompose1));
CHECK(assert_equal(numericalH1,actualDcompose1));
CHECK(assert_equal(expectedH2,actualDcompose2));
CHECK(assert_equal(numericalH2,actualDcompose2));
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
EXPECT(assert_equal(expectedH1,actualDcompose1));
EXPECT(assert_equal(numericalH1,actualDcompose1));
EXPECT(assert_equal(expectedH2,actualDcompose2));
EXPECT(assert_equal(numericalH2,actualDcompose2));
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = (pose1 * pose2).transform_to(point);
Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point));
CHECK(assert_equal(expected_point, actual_point1));
CHECK(assert_equal(expected_point, actual_point2));
EXPECT(assert_equal(expected_point, actual_point1));
EXPECT(assert_equal(expected_point, actual_point2));
}
/* ************************************************************************* */
@ -218,15 +219,15 @@ TEST(Pose2, compose_b)
Pose2 pose_actual_op = pose1 * pose2;
Matrix actualDcompose1, actualDcompose2;
Pose2 pose_actual_fcn = compose(pose1, pose2, actualDcompose1, actualDcompose2);
Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
CHECK(assert_equal(numericalH1,actualDcompose1,1e-5));
CHECK(assert_equal(numericalH2,actualDcompose2));
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
EXPECT(assert_equal(numericalH2,actualDcompose2));
CHECK(assert_equal(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
EXPECT(assert_equal(pose_expected, pose_actual_op));
EXPECT(assert_equal(pose_expected, pose_actual_fcn));
}
/* ************************************************************************* */
@ -240,15 +241,15 @@ TEST(Pose2, compose_c)
Pose2 pose_actual_op = pose1 * pose2;
Matrix actualDcompose1, actualDcompose2;
Pose2 pose_actual_fcn = compose(pose1,pose2, actualDcompose1, actualDcompose2);
Pose2 pose_actual_fcn = pose1.compose(pose2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(compose, pose1, pose2, 1e-5);
CHECK(assert_equal(numericalH1,actualDcompose1,1e-5));
CHECK(assert_equal(numericalH2,actualDcompose2));
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2, 1e-5);
EXPECT(assert_equal(numericalH1,actualDcompose1,1e-5));
EXPECT(assert_equal(numericalH2,actualDcompose2));
CHECK(assert_equal(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
EXPECT(assert_equal(pose_expected, pose_actual_op));
EXPECT(assert_equal(pose_expected, pose_actual_fcn));
}
/* ************************************************************************* */
@ -257,19 +258,19 @@ TEST(Pose2, inverse )
Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y
Pose2 identity, lTg = inverse(gTl);
CHECK(assert_equal(identity,compose(lTg,gTl)));
CHECK(assert_equal(identity,compose(gTl,lTg)));
Pose2 identity, lTg = gTl.inverse();
EXPECT(assert_equal(identity,lTg.compose(gTl)));
EXPECT(assert_equal(identity,gTl.compose(lTg)));
Point2 l(4,5), g(-4,6);
CHECK(assert_equal(g,gTl*l));
CHECK(assert_equal(l,lTg*g));
EXPECT(assert_equal(g,gTl*l));
EXPECT(assert_equal(l,lTg*g));
// Check derivative
Matrix numericalH = numericalDerivative11<Pose2,Pose2>(inverse, lTg, 1e-5);
Matrix numericalH = numericalDerivative11<Pose2,Pose2>(testing::inverse, lTg, 1e-5);
Matrix actualDinverse;
inverse(lTg, actualDinverse);
CHECK(assert_equal(numericalH,actualDinverse));
lTg.inverse(actualDinverse);
EXPECT(assert_equal(numericalH,actualDinverse));
}
/* ************************************************************************* */
@ -277,6 +278,7 @@ Vector homogeneous(const Point2& p) {
return Vector_(3, p.x(), p.y(), 1.0);
}
/* ************************************************************************* */
Matrix matrix(const Pose2& gTl) {
Matrix gRl = gTl.r().matrix();
Point2 gt = gTl.t();
@ -286,31 +288,32 @@ Matrix matrix(const Pose2& gTl) {
0.0, 0.0, 1.0);
}
/* ************************************************************************* */
TEST( Pose2, matrix )
{
Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl);
CHECK(assert_equal(Matrix_(3,3,
EXPECT(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
gMl));
Rot2 gR1 = gTl.r();
CHECK(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
Point2 x_axis(1,0), y_axis(0,1);
CHECK(assert_equal(Matrix_(2,2,
EXPECT(assert_equal(Matrix_(2,2,
0.0, -1.0,
1.0, 0.0),
gR1.matrix()));
CHECK(assert_equal(Point2(0,1),gR1*x_axis));
CHECK(assert_equal(Point2(-1,0),gR1*y_axis));
CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));
EXPECT(assert_equal(Point2(0,1),gR1*x_axis));
EXPECT(assert_equal(Point2(-1,0),gR1*y_axis));
EXPECT(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
EXPECT(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));
// check inverse pose
Matrix lMg = matrix(inverse(gTl));
CHECK(assert_equal(Matrix_(3,3,
Matrix lMg = matrix(gTl.inverse());
EXPECT(assert_equal(Matrix_(3,3,
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
@ -323,7 +326,7 @@ TEST( Pose2, compose_matrix )
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); // RIGHT DOES NOT
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
}
/* ************************************************************************* */
@ -339,30 +342,30 @@ TEST( Pose2, between )
Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2));
Pose2 actual1 = between(gT1,gT2);
Pose2 actual2 = between(gT1,gT2,actualH1,actualH2);
CHECK(assert_equal(expected,actual1));
CHECK(assert_equal(expected,actual2));
Pose2 actual1 = gT1.between(gT2);
Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
EXPECT(assert_equal(expected,actual1));
EXPECT(assert_equal(expected,actual2));
Matrix expectedH1 = Matrix_(3,3,
0.0,-1.0,-2.0,
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
);
Matrix numericalH1 = numericalDerivative21(between<Pose2>, gT1, gT2, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, gT1, gT2, 1e-5);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1));
// Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
CHECK(assert_equal(-between(gT2,gT1).AdjointMap(),actualH1));
EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
Matrix expectedH2 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
);
Matrix numericalH2 = numericalDerivative22(between<Pose2>, gT1, gT2, 1e-5);
CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(numericalH2,actualH2));
Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, gT1, gT2, 1e-5);
EXPECT(assert_equal(expectedH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2));
}
@ -374,11 +377,11 @@ TEST( Pose2, between2 )
Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2;
between(p1,p2,actualH1,actualH2);
Matrix numericalH1 = numericalDerivative21(between<Pose2>, p1, p2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(between<Pose2>, p1, p2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
p1.between(p2,actualH1,actualH2);
Matrix numericalH1 = numericalDerivative21<Pose2,Pose2,Pose2>(testing::between, p1, p2, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22<Pose2,Pose2,Pose2>(testing::between, p1, p2, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
@ -386,15 +389,15 @@ TEST( Pose2, round_trip )
{
Pose2 p1(1.23, 2.30, 0.2);
Pose2 odo(0.53, 0.39, 0.15);
Pose2 p2 = compose(p1, odo);
CHECK(assert_equal(odo, between(p1, p2)));
Pose2 p2 = p1.compose(odo);
EXPECT(assert_equal(odo, p1.between(p2)));
}
/* ************************************************************************* */
TEST(Pose2, members)
{
Pose2 pose;
CHECK(pose.dim() == 3);
EXPECT(pose.dim() == 3);
}
/* ************************************************************************* */
@ -412,65 +415,65 @@ TEST( Pose2, bearing )
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish bearing is indeed zero
CHECK(assert_equal(Rot2(),x1.bearing(l1)));
EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
// establish bearing is indeed 45 degrees
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(l2)));
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(l2)));
// establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(bearing_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
// establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(bearing_proxy, x3, l4, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
}
/* ************************************************************************* */
LieVector range_proxy(const Pose2& pose, const Point2& point) {
return LieVector(Vector_(1, pose.range(point)));
return LieVector(pose.range(point));
}
TEST( Pose2, range )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish range is indeed zero
DOUBLES_EQUAL(1,x1.range(l1),1e-9);
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// establish range is indeed 45 degrees
DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9);
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9);
// Another pair
double actual23 = x2.range(l3, actualH1, actualH2);
DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(range_proxy, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
// Another test
double actual34 = x3.range(l4, actualH1, actualH2);
DOUBLES_EQUAL(2,actual34,1e-9);
EXPECT_DOUBLES_EQUAL(2,actual34,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(range_proxy, x3, l4, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */

82
geometry/tests/testPose3.cpp
View File

@ -6,6 +6,7 @@
#include <math.h>
#include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Pose3.h>
using namespace std;
@ -33,22 +34,21 @@ TEST( Pose3, expmap_a)
Vector v(6);
fill(v.begin(), v.end(), 0);
v(0) = 0.3;
CHECK(assert_equal(expmap(id,v), Pose3(R, Point3())));
CHECK(assert_equal(id.expmap(v), Pose3(R, Point3())));
#ifdef CORRECT_POSE3_EXMAP
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
#else
v(3)=0.2;v(4)=0.7;v(5)=-2;
#endif
CHECK(assert_equal(Pose3(R, P),expmap(id,v),1e-5));
CHECK(assert_equal(Pose3(R, P),id.expmap(v),1e-5));
}
/* ************************************************************************* */
TEST(Pose3, expmap_b)
{
Pose3 p1(Rot3(), Point3(100, 0, 0));
Pose3 p2 = expmap(p1, Vector_(6,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 p2 = p1.expmap(Vector_(6,0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
CHECK(assert_equal(expected, p2));
}
@ -68,24 +68,24 @@ namespace screw {
TEST(Pose3, expmap_c)
{
CHECK(assert_equal(screw::expected, expm<Pose3>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, expmap<Pose3>(screw::xi),1e-6));
CHECK(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6));
}
/* ************************************************************************* */
// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
TEST(Pose3, Adjoint)
{
Pose3 expected = T * expmap<Pose3>(screw::xi) * inverse(T);
Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T);
Vector xiprime = Adjoint(T, screw::xi);
CHECK(assert_equal(expected, expmap<Pose3>(xiprime), 1e-6));
CHECK(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6));
Pose3 expected2 = T2 * expmap<Pose3>(screw::xi) * inverse(T2);
Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2);
Vector xiprime2 = Adjoint(T2, screw::xi);
CHECK(assert_equal(expected2, expmap<Pose3>(xiprime2), 1e-6));
CHECK(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6));
Pose3 expected3 = T3 * expmap<Pose3>(screw::xi) * inverse(T3);
Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3);
Vector xiprime3 = Adjoint(T3, screw::xi);
CHECK(assert_equal(expected3, expmap<Pose3>(xiprime3), 1e-6));
CHECK(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
}
/* ************************************************************************* */
@ -100,7 +100,7 @@ Pose3 Agrawal06iros(const Vector& xi) {
else {
Matrix W = skewSymmetric(w/t);
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(expmap<Rot3> (w), expmap<Point3> (A * v));
return Pose3(Rot3::Expmap (w), expmap<Point3> (A * v));
}
}
@ -109,7 +109,7 @@ TEST(Pose3, expmaps_galore)
{
Vector xi; Pose3 actual;
xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
actual = expmap<Pose3>(xi);
actual = Pose3::Expmap(xi);
CHECK(assert_equal(expm<Pose3>(xi), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, logmap(actual),1e-6));
@ -117,17 +117,17 @@ TEST(Pose3, expmaps_galore)
xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6);
for (double theta=1.0;0.3*theta<=M_PI;theta*=2) {
Vector txi = xi*theta;
actual = expmap<Pose3>(txi);
actual = Pose3::Expmap(txi);
CHECK(assert_equal(expm<Pose3>(txi,30), actual,1e-6));
CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6));
Vector log = logmap(actual);
CHECK(assert_equal(actual, expmap<Pose3>(log),1e-6));
CHECK(assert_equal(actual, Pose3::Expmap(log),1e-6));
CHECK(assert_equal(txi,log,1e-6)); // not true once wraps
}
// Works with large v as well, but expm needs 10 iterations!
xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0);
actual = expmap<Pose3>(xi);
actual = Pose3::Expmap(xi);
CHECK(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
CHECK(assert_equal(Agrawal06iros(xi), actual,1e-6));
CHECK(assert_equal(xi, logmap(actual),1e-6));
@ -140,9 +140,9 @@ TEST(Pose3, Adjoint_compose)
// T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
const Pose3& T1 = T;
Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8);
Pose3 expected = T1 * expmap<Pose3>(x) * T2;
Pose3 expected = T1 * Pose3::Expmap(x) * T2;
Vector y = Adjoint(inverse(T2), x);
Pose3 actual = T1 * T2 * expmap<Pose3>(y);
Pose3 actual = T1 * T2 * Pose3::Expmap(y);
CHECK(assert_equal(expected, actual, 1e-6));
}
#endif // SLOW_BUT_CORRECT_EXMAP
@ -155,12 +155,12 @@ TEST( Pose3, compose )
CHECK(assert_equal(actual,expected,1e-8));
Matrix actualDcompose1, actualDcompose2;
compose(T2, T2, actualDcompose1, actualDcompose2);
T2.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T2, T2, 1e-5);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T2, T2, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T2, T2, 1e-5);
CHECK(assert_equal(numericalH2,actualDcompose2));
}
@ -173,12 +173,12 @@ TEST( Pose3, compose2 )
CHECK(assert_equal(actual,expected,1e-8));
Matrix actualDcompose1, actualDcompose2;
compose(T1, T2, actualDcompose1, actualDcompose2);
T1.compose(T2, actualDcompose1, actualDcompose2);
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
Matrix numericalH1 = numericalDerivative21(testing::compose<Pose3>, T1, T2, 1e-5);
CHECK(assert_equal(numericalH1,actualDcompose1,5e-5));
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T1, T2, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::compose<Pose3>, T1, T2, 1e-5);
CHECK(assert_equal(numericalH2,actualDcompose2));
}
@ -186,11 +186,11 @@ TEST( Pose3, compose2 )
TEST( Pose3, inverse)
{
Matrix actualDinverse;
Matrix actual = inverse(T, actualDinverse).matrix();
Matrix actual = T.inverse(actualDinverse).matrix();
Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T, 1e-5);
CHECK(assert_equal(numericalH,actualDinverse));
}
@ -201,16 +201,16 @@ TEST( Pose3, inverseDerivatives2)
Point3 t(3.5,-8.2,4.2);
Pose3 T(R,t);
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T, 1e-5);
Matrix actualDinverse;
inverse(T, actualDinverse);
T.inverse(actualDinverse);
CHECK(assert_equal(numericalH,actualDinverse,5e-5));
}
/* ************************************************************************* */
TEST( Pose3, compose_inverse)
{
Matrix actual = (T*inverse(T)).matrix();
Matrix actual = (T*T.inverse()).matrix();
Matrix expected = eye(4,4);
CHECK(assert_equal(actual,expected,1e-8));
}
@ -408,12 +408,12 @@ TEST(Pose3, manifold)
Pose3 t1 = T;
Pose3 t2 = T3;
Pose3 origin;
Vector d12 = logmap(t1, t2);
CHECK(assert_equal(t2, expmap(t1,d12)));
Vector d12 = t1.logmap(t2);
CHECK(assert_equal(t2, t1.expmap(d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
// CHECK(assert_equal(t2, expmap(origin,d12)*t1));
Vector d21 = logmap(t2, t1);
CHECK(assert_equal(t1, expmap(t2,d21)));
Vector d21 = t2.logmap(t1);
CHECK(assert_equal(t1, t2.expmap(d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
// CHECK(assert_equal(t1, expmap(origin,d21)*t2));
@ -427,11 +427,11 @@ TEST(Pose3, manifold)
// lines in canonical coordinates correspond to Abelian subgroups in SE(3)
Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(expmap<Pose3>(-d),inverse(expmap<Pose3>(d))));
CHECK(assert_equal(Pose3::Expmap(-d),inverse(Pose3::Expmap(d))));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Pose3 T2 = expmap<Pose3>(2*d);
Pose3 T3 = expmap<Pose3>(3*d);
Pose3 T5 = expmap<Pose3>(5*d);
Pose3 T2 = Pose3::Expmap(2*d);
Pose3 T3 = Pose3::Expmap(3*d);
Pose3 T5 = Pose3::Expmap(5*d);
CHECK(assert_equal(T5,T2*T3));
CHECK(assert_equal(T5,T3*T2));
@ -441,15 +441,15 @@ TEST(Pose3, manifold)
/* ************************************************************************* */
TEST( Pose3, between )
{
Pose3 expected = inverse(T2) * T3;
Pose3 expected = T2.inverse() * T3;
Matrix actualDBetween1,actualDBetween2;
Pose3 actual = between(T2, T3, actualDBetween1,actualDBetween2);
Pose3 actual = T2.between(T3, actualDBetween1,actualDBetween2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Pose3> , T2, T3, 1e-5);
Matrix numericalH1 = numericalDerivative21(testing::between<Pose3> , T2, T3, 1e-5);
CHECK(assert_equal(numericalH1,actualDBetween1,5e-5));
Matrix numericalH2 = numericalDerivative22(between<Pose3> , T2, T3, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::between<Pose3> , T2, T3, 1e-5);
CHECK(assert_equal(numericalH2,actualDBetween2));
}

6
geometry/tests/testRot2.cpp
View File

@ -25,7 +25,7 @@ TEST( Rot2, constructors_and_angle)
/* ************************************************************************* */
TEST( Rot2, transpose)
{
CHECK(assert_equal(inverse(R).matrix(),R.transpose()));
CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
}
/* ************************************************************************* */
@ -47,7 +47,7 @@ TEST( Rot2, equals)
TEST( Rot2, expmap)
{
Vector v = zero(1);
CHECK(assert_equal(expmap(R,v), R));
CHECK(assert_equal(R.expmap(v), R));
}
/* ************************************************************************* */
@ -56,7 +56,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 = logmap(rot0, rot);
Vector actual = rot0.logmap(rot);
CHECK(assert_equal(expected, actual));
}

82
geometry/tests/testRot3.cpp
View File

@ -7,6 +7,7 @@
#include <gtsam/CppUnitLite/TestHarness.h>
#include <boost/math/constants/constants.hpp>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
@ -56,7 +57,7 @@ TEST( Rot3, transpose)
{
Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9);
CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3)));
CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
}
/* ************************************************************************* */
@ -126,7 +127,7 @@ TEST( Rot3, expmap)
{
Vector v(3);
fill(v.begin(), v.end(), 0);
CHECK(assert_equal(expmap(R,v), R));
CHECK(assert_equal(R.expmap(v), R));
}
/* ************************************************************************* */
@ -134,35 +135,35 @@ TEST(Rot3, log)
{
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
Rot3 R1 = Rot3::rodriguez(w1);
CHECK(assert_equal(w1, logmap(R1)));
CHECK(assert_equal(w1, Rot3::Logmap(R1)));
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
Rot3 R2 = Rot3::rodriguez(w2);
CHECK(assert_equal(w2, logmap(R2)));
CHECK(assert_equal(w2, Rot3::Logmap(R2)));
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
Rot3 R3 = Rot3::rodriguez(w3);
CHECK(assert_equal(w3, logmap(R3)));
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
Vector w = Vector_(3, 0.1, 0.4, 0.2);
Rot3 R = Rot3::rodriguez(w);
CHECK(assert_equal(w, logmap(R)));
CHECK(assert_equal(w, Rot3::Logmap(R)));
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
Rot3 R5 = Rot3::rodriguez(w5);
CHECK(assert_equal(w5, logmap(R5)));
CHECK(assert_equal(w5, Rot3::Logmap(R5)));
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3 R6 = Rot3::rodriguez(w6);
CHECK(assert_equal(w6, logmap(R6)));
CHECK(assert_equal(w6, Rot3::Logmap(R6)));
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3 R7 = Rot3::rodriguez(w7);
CHECK(assert_equal(w7, logmap(R7)));
CHECK(assert_equal(w7, Rot3::Logmap(R7)));
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
Rot3 R8 = Rot3::rodriguez(w8);
CHECK(assert_equal(w8, logmap(R8)));
CHECK(assert_equal(w8, Rot3::Logmap(R8)));
}
/* ************************************************************************* */
@ -173,12 +174,12 @@ TEST(Rot3, manifold)
Rot3 origin;
// log behaves correctly
Vector d12 = logmap(gR1, gR2);
CHECK(assert_equal(gR2, expmap(gR1,d12)));
CHECK(assert_equal(gR2, gR1*expmap<Rot3>(d12)));
Vector d21 = logmap(gR2, gR1);
CHECK(assert_equal(gR1, expmap(gR2,d21)));
CHECK(assert_equal(gR1, gR2*expmap<Rot3>(d21)));
Vector d12 = gR1.logmap(gR2);
CHECK(assert_equal(gR2, gR1.expmap(d12)));
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
Vector d21 = gR2.logmap(gR1);
CHECK(assert_equal(gR1, gR2.expmap(d21)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
@ -186,11 +187,11 @@ TEST(Rot3, manifold)
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = Vector_(3, 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(expmap<Rot3>(-d),inverse(expmap<Rot3>(d))));
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = expmap<Rot3> (2 * d);
Rot3 R3 = expmap<Rot3> (3 * d);
Rot3 R5 = expmap<Rot3> (5 * d);
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
@ -216,30 +217,28 @@ TEST(Rot3, BCH)
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = expmap<Rot3> (w1.vector()), R2 = expmap<Rot3> (w2.vector());
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R3 = R1 * R2;
Vector expected = logmap(R3);
Vector expected = Rot3::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
CHECK(assert_equal(expected, actual,1e-5));
}
/* ************************************************************************* */
inline Point3 rotate_(const Rot3& r, const Point3& pt) { return r.rotate(pt); }
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(rotate_, R, P);
Matrix numerical2 = numericalDerivative21(rotate_, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(rotate_, R, P);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
inline Point3 unrotate_(const Rot3& r, const Point3& pt) { return r.unrotate(pt); }
TEST( Rot3, unrotate)
{
Point3 w = R * P;
@ -247,10 +246,10 @@ TEST( Rot3, unrotate)
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(unrotate_, R, w);
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
@ -262,30 +261,29 @@ TEST( Rot3, compose )
Rot3 expected = R1 * R2;
Matrix actualH1, actualH2;
Rot3 actual = compose(R1, R2, actualH1, actualH2);
Rot3 actual = R1.compose(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21<Rot3, Rot3, Rot3> (compose, R1,
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
R2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22<Rot3, Rot3, Rot3> (compose, R1,
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, inverse )
{
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Matrix actualH;
CHECK(assert_equal(I,R*inverse(R, actualH)));
CHECK(assert_equal(I,inverse(R)*R));
CHECK(assert_equal(I,R*R.inverse(actualH)));
CHECK(assert_equal(I,R.inverse()*R));
Matrix numericalH = numericalDerivative11<Rot3, Rot3> (inverse, R, 1e-5);
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R, 1e-5);
CHECK(assert_equal(numericalH,actualH));
}
@ -294,21 +292,21 @@ TEST( Rot3, between )
{
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
CHECK(assert_equal(R, between(origin,R)));
CHECK(assert_equal(inverse(R), between(R,origin)));
CHECK(assert_equal(R, origin.between(R)));
CHECK(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = inverse(R1) * R2;
Rot3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
Rot3 actual = between(R1, R2, actualH1, actualH2);
Rot3 actual = R1.between(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Rot3> , R1, R2, 1e-5);
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(between<Rot3> , R1, R2, 1e-5);
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}

6
inference/ISAM2-inl.h
View File

@ -147,7 +147,7 @@ namespace gtsam {
//// 1 - relinearize selected variables
if (relinFromLast) {
theta_ = expmap(theta_, deltaMarked_);
theta_ = theta_.expmap(deltaMarked_);
}
//// 2 - Add new factors (for later relinearization)
@ -274,7 +274,7 @@ namespace gtsam {
}
// not part of the formal algorithm, but needed to allow initialization of new variables outside by the user
thetaFuture_ = expmap(thetaFuture_, deltaMarked_);
thetaFuture_ = thetaFuture_.expmap(deltaMarked_);
}
}
@ -366,7 +366,7 @@ namespace gtsam {
}
// 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
theta_ = expmap(theta_, deltaMarked);
theta_ = theta_.expmap(deltaMarked);
// 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.

4
inference/ISAM2.h
View File

@ -80,9 +80,9 @@ namespace gtsam {
// needed to create initial estimates (note that this will be the linearization point in the next step!)
const Config getLinearizationPoint() const {return thetaFuture_;}
// estimate based on incomplete delta (threshold!)
const Config calculateEstimate() const {return expmap(theta_, delta_);}
const Config calculateEstimate() const {return theta_.expmap(delta_);}
// estimate based on full delta (note that this is based on the actual current linearization point)
const Config calculateBestEstimate() const {return expmap(theta_, optimize2(*this, 0.));}
const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));}
const std::list<Symbol>& getMarkedUnsafe() const { return marked_; }

4
inference/graph-inl.h
View File

@ -140,7 +140,7 @@ public:
typename Config::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename Config::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, compose((*config_)[key_from],relativePose));
config_->insert(key_to, (*config_)[key_from].compose(relativePose));
}
};
@ -190,7 +190,7 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
if (found1)
boost::put(boost::edge_weight, g, edge12, l1Xl2);
else if (found2)
boost::put(boost::edge_weight, g, edge21, inverse(l1Xl2));
boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse());
}
// compose poses

230
linear/VectorMap.h
View File

@ -18,156 +18,156 @@
#include <gtsam/inference/SymbolMap.h>
namespace gtsam {
/** Factor Graph Configuration */
class VectorMap : public Testable<VectorMap> {
protected:
/** Map from string indices to values */
SymbolMap<Vector> values;
/** Factor Graph Configuration */
class VectorMap : public Testable<VectorMap> {
public:
typedef SymbolMap<Vector>::iterator iterator;
typedef SymbolMap<Vector>::const_iterator const_iterator;
protected:
/** Map from string indices to values */
SymbolMap<Vector> values;
VectorMap() {}
VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {}
VectorMap(const Symbol& j, const Vector& a) { insert(j,a); }
virtual ~VectorMap() {}
public:
typedef SymbolMap<Vector>::iterator iterator;
typedef SymbolMap<Vector>::const_iterator const_iterator;
/** return all the nodes in the graph **/
std::vector<Symbol> get_names() const;
VectorMap() {}
VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {}
VectorMap(const Symbol& j, const Vector& a) { insert(j,a); }
/** convert into a single large vector */
Vector vector() const;
virtual ~VectorMap() {}
/** Insert a value into the configuration with a given index */
VectorMap& insert(const Symbol& name, const Vector& v);
/** return all the nodes in the graph **/
std::vector<Symbol> get_names() const;
/** Insert or add a value with given index */
VectorMap& insertAdd(const Symbol& j, const Vector& v);
/** convert into a single large vector */
Vector vector() const;
/** Insert a config into another config */
void insert(const VectorMap& config);
/** Insert a value into the configuration with a given index */
VectorMap& insert(const Symbol& name, const Vector& v);
/** Insert a config into another config, add if key already exists */
void insertAdd(const VectorMap& config);
/** Insert or add a value with given index */
VectorMap& insertAdd(const Symbol& j, const Vector& v);
const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();}
iterator begin() {return values.begin();}
iterator end() {return values.end();}
/** Insert a config into another config */
void insert(const VectorMap& config);
/** Vector access in VectorMap is via a Vector reference */
Vector& operator[](const Symbol& j);
const Vector& operator[](const Symbol& j) const;
/** Insert a config into another config, add if key already exists */
void insertAdd(const VectorMap& config);
/** [set] and [get] provided for access via MATLAB */
inline Vector& get(const Symbol& j) { return (*this)[j];}
void set(const Symbol& j, const Vector& v) { (*this)[j] = v; }
inline const Vector& get(const Symbol& j) const { return (*this)[j];}
const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();}
iterator begin() {return values.begin();}
iterator end() {return values.end();}
bool contains(const Symbol& name) const {
const_iterator it = values.find(name);
return (it!=values.end());
}
/** Vector access in VectorMap is via a Vector reference */
Vector& operator[](const Symbol& j);
const Vector& operator[](const Symbol& j) const;
/** Nr of vectors */
size_t size() const { return values.size();}
/** [set] and [get] provided for access via MATLAB */
inline Vector& get(const Symbol& j) { return (*this)[j];}
void set(const Symbol& j, const Vector& v) { (*this)[j] = v; }
inline const Vector& get(const Symbol& j) const { return (*this)[j];}
/** Total dimensionality */
size_t dim() const;
bool contains(const Symbol& name) const {
const_iterator it = values.find(name);
return (it!=values.end());
}
/** max of the vectors */
inline double max() const {
double m = -std::numeric_limits<double>::infinity();
for(const_iterator it=begin(); it!=end(); it++)
m = std::max(m, gtsam::max(it->second));
return m;
}
/** Nr of vectors */
size_t size() const { return values.size();}
/** Scale */
VectorMap scale(double s) const;
VectorMap operator*(double s) const;
/** Total dimensionality */
size_t dim() const;
/** Negation */
VectorMap operator-() const;
/** max of the vectors */
inline double max() const {
double m = -std::numeric_limits<double>::infinity();
for(const_iterator it=begin(); it!=end(); it++)
m = std::max(m, gtsam::max(it->second));
return m;
}
/** Add in place */
void operator+=(const VectorMap &b);
/** Scale */
VectorMap scale(double s) const;
VectorMap operator*(double s) const;
/** Add */
VectorMap operator+(const VectorMap &b) const;
/** Negation */
VectorMap operator-() const;
/** Subtract */
VectorMap operator-(const VectorMap &b) const;
/** Add in place */
void operator+=(const VectorMap &b);
/** print */
void print(const std::string& name = "") const;
/** Add */
VectorMap operator+(const VectorMap &b) const;
/** equals, for unit testing */
bool equals(const VectorMap& expected, double tol=1e-9) const;
/** Subtract */
VectorMap operator-(const VectorMap &b) const;
void clear() {values.clear();}
/** Dot product */
double dot(const VectorMap& b) const;
/** print */
void print(const std::string& name = "") const;
/** Set all vectors to zero */
VectorMap& zero();
/** equals, for unit testing */
bool equals(const VectorMap& expected, double tol=1e-9) const;
/** Create a clone of x with exactly same structure, except with zero values */
static VectorMap zero(const VectorMap& x);
void clear() {values.clear();}
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorMap, this is just addition.
*/
friend VectorMap expmap(const VectorMap& original, const VectorMap& delta);
/** Dot product */
double dot(const VectorMap& b) const;
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
friend VectorMap expmap(const VectorMap& original, const Vector& delta);
/** Set all vectors to zero */
VectorMap& zero();
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(values);
}
}; // VectorMap
/** Create a clone of x with exactly same structure, except with zero values */
static VectorMap zero(const VectorMap& x);
/** scalar product */
inline VectorMap operator*(double s, const VectorMap& x) {return x*s;}
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorMap, this is just addition.
*/
friend VectorMap expmap(const VectorMap& original, const VectorMap& delta);
/** Dot product */
double dot(const VectorMap&, const VectorMap&);
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
friend VectorMap expmap(const VectorMap& original, const Vector& delta);
/**
* BLAS Level 1 scal: x <- alpha*x
*/
void scal(double alpha, VectorMap& x);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(values);
}
}; // VectorMap
/**
* BLAS Level 1 axpy: y <- alpha*x + y
* UNSAFE !!!! Only works if x and y laid out in exactly same shape
* Used in internal loop in iterative for fast conjugate gradients
* Consider using other functions if this is not in hotspot
*/
void axpy(double alpha, const VectorMap& x, VectorMap& y);
/** scalar product */
inline VectorMap operator*(double s, const VectorMap& x) {return x*s;}
/** dim function (for iterative::CGD) */
inline double dim(const VectorMap& x) { return x.dim();}
/** Dot product */
double dot(const VectorMap&, const VectorMap&);
/** max of the vectors */
inline double max(const VectorMap& x) { return x.max();}
/**
* BLAS Level 1 scal: x <- alpha*x
*/
void scal(double alpha, VectorMap& x);
/** print with optional string */
void print(const VectorMap& v, const std::string& s = "");
/**
* BLAS Level 1 axpy: y <- alpha*x + y
* UNSAFE !!!! Only works if x and y laid out in exactly same shape
* Used in internal loop in iterative for fast conjugate gradients
* Consider using other functions if this is not in hotspot
*/
void axpy(double alpha, const VectorMap& x, VectorMap& y);
/** dim function (for iterative::CGD) */
inline double dim(const VectorMap& x) { return x.dim();}
/** max of the vectors */
inline double max(const VectorMap& x) { return x.max();}
/** print with optional string */
void print(const VectorMap& v, const std::string& s = "");
} // gtsam

60
nonlinear/LieConfig-inl.h
View File

@ -1,8 +1,6 @@
/*
* LieConfig.cpp
*
* Created on: Jan 8, 2010
* @Author: Richard Roberts
/**
* @file LieConfig.cpp
* @author Richard Roberts
*/
#pragma once
@ -21,15 +19,16 @@
#define INSTANTIATE_LIE_CONFIG(J) \
/*INSTANTIATE_LIE(T);*/ \
template LieConfig<J> expmap(const LieConfig<J>&, const VectorConfig&); \
template LieConfig<J> expmap(const LieConfig<J>&, const Vector&); \
template VectorConfig logmap(const LieConfig<J>&, const LieConfig<J>&); \
/*template LieConfig<J> expmap(const LieConfig<J>&, const VectorConfig&);*/ \
/*template LieConfig<J> expmap(const LieConfig<J>&, const Vector&);*/ \
/*template VectorConfig logmap(const LieConfig<J>&, const LieConfig<J>&);*/ \
template class LieConfig<J>;
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class J>
void LieConfig<J>::print(const string &s) const {
cout << "LieConfig " << s << ", size " << values_.size() << "\n";
@ -38,6 +37,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class J>
bool LieConfig<J>::equals(const LieConfig<J>& expected, double tol) const {
if (values_.size() != expected.values_.size()) return false;
@ -49,6 +49,7 @@ namespace gtsam {
return true;
}
/* ************************************************************************* */
template<class J>
const typename J::Value_t& LieConfig<J>::at(const J& j) const {
const_iterator it = values_.find(j);
@ -56,33 +57,38 @@ namespace gtsam {
else return it->second;
}
/* ************************************************************************* */
template<class J>
size_t LieConfig<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const typename Values::value_type& value, values_)
n += gtsam::dim(value.second);
n += value.second.dim();
return n;
}
/* ************************************************************************* */
template<class J>
VectorConfig LieConfig<J>::zero() const {
VectorConfig z;
BOOST_FOREACH(const typename Values::value_type& value, values_)
z.insert(value.first,gtsam::zero(gtsam::dim(value.second)));
z.insert(value.first,gtsam::zero(value.second.dim()));
return z;
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::insert(const J& name, const typename J::Value_t& val) {
values_.insert(make_pair(name, val));
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::insert(const LieConfig<J>& cfg) {
BOOST_FOREACH(const typename Values::value_type& v, cfg.values_)
insert(v.first, v.second);
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::update(const LieConfig<J>& cfg) {
BOOST_FOREACH(const typename Values::value_type& v, values_) {
@ -91,11 +97,13 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::update(const J& j, const typename J::Value_t& val) {
values_[j] = val;
}
/* ************************************************************************* */
template<class J>
std::list<J> LieConfig<J>::keys() const {
std::list<J> ret;
@ -104,68 +112,72 @@ namespace gtsam {
return ret;
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
template<class J>
void LieConfig<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
dim = gtsam::dim(it->second);
dim = it->second.dim();
values_.erase(it);
}
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
LieConfig<J> expmap(const LieConfig<J>& c, const VectorConfig& delta) {
LieConfig<J> LieConfig<J>::expmap(const VectorConfig& delta) const {
LieConfig<J> newConfig;
typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, c) {
BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first;
const typename J::Value_t& pj = value.second;
Symbol jkey = (Symbol)j;
if (delta.contains(jkey)) {
const Vector& dj = delta[jkey];
newConfig.insert(j, expmap(pj,dj));
newConfig.insert(j, pj.expmap(dj));
} else
newConfig.insert(j, pj);
}
return newConfig;
}
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
LieConfig<J> expmap(const LieConfig<J>& c, const Vector& delta) {
if(delta.size() != dim(c)) {
cout << "LieConfig::dim (" << dim(c) << ") <> delta.size (" << delta.size() << ")" << endl;
LieConfig<J> LieConfig<J>::expmap(const Vector& delta) const {
if(delta.size() != dim()) {
cout << "LieConfig::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
throw invalid_argument("Delta vector length does not match config dimensionality.");
}
LieConfig<J> newConfig;
int delta_offset = 0;
typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, c) {
BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first;
const typename J::Value_t& pj = value.second;
int cur_dim = dim(pj);
newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim)));
int cur_dim = pj.dim();
newConfig.insert(j,pj.expmap(sub(delta, delta_offset, delta_offset+cur_dim)));
delta_offset += cur_dim;
}
return newConfig;
}
/* ************************************************************************* */
// todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs
template<class J>
VectorConfig logmap(const LieConfig<J>& c0, const LieConfig<J>& cp) {
VectorConfig LieConfig<J>::logmap(const LieConfig<J>& cp) const {
VectorConfig delta;
typedef pair<J,typename J::Value_t> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) {
if(c0.exists(value.first))
delta.insert(value.first,
logmap(c0[value.first], value.second));
if(this->exists(value.first))
delta.insert(value.first, this->at(value.first).logmap(value.second));
}
return delta;
}

43
nonlinear/LieConfig.h
View File

@ -76,16 +76,16 @@ namespace gtsam {
const Value& at(const J& j) const;
/** operator[] syntax for get */
const Value& operator[](const J& j) const { return at(j); }
const Value& operator[](const J& j) const { return at(j); }
/** Check if a variable exists */
bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
/** Check if a variable exists */
bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
/** Check if a variable exists and return it if so */
boost::optional<Value> exists_(const J& i) const {
const_iterator it = values_.find(i);
if (it==values_.end()) return boost::none; else return it->second;
}
/** Check if a variable exists and return it if so */
boost::optional<Value> exists_(const J& i) const {
const_iterator it = values_.find(i);
if (it==values_.end()) return boost::none; else return it->second;
}
/** The number of variables in this config */
size_t size() const { return values_.size(); }
@ -104,6 +104,17 @@ namespace gtsam {
iterator begin() { return values_.begin(); }
iterator end() { return values_.end(); }
// Lie operations
/** Add a delta config to current config and returns a new config */
LieConfig expmap(const VectorConfig& delta) const;
/** Add a delta vector to current config and returns a new config, uses iterator order */
LieConfig expmap(const Vector& delta) const;
/** Get a delta config about a linearization point c0 (*this) */
VectorConfig logmap(const LieConfig& cp) const;
// imperative methods:
/** Add a variable with the given j - does not replace existing values */
@ -153,21 +164,5 @@ namespace gtsam {
};
/** Dimensionality of the tangent space */
template<class J>
inline size_t dim(const LieConfig<J>& c) { return c.dim(); }
/** Add a delta config */
template<class J>
LieConfig<J> expmap(const LieConfig<J>& c, const VectorConfig& delta);
/** Add a delta vector, uses iterator order */
template<class J>
LieConfig<J> expmap(const LieConfig<J>& c, const Vector& delta);
/** Get a delta config about a linearization point c0 */
template<class J>
VectorConfig logmap(const LieConfig<J>& c0, const LieConfig<J>& cp);
}

4
nonlinear/NonlinearConstraint.h
View File

@ -434,7 +434,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 logmap(value_, x1);
return value_.logmap(x1);
}
};
@ -466,7 +466,7 @@ public:
const size_t p = X::Dim();
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
return logmap(x1, x2);
return x1.logmap(x2);
}
};

10
nonlinear/NonlinearEquality.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
* Constructor - forces exact evaluation
*/
NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible),
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()),
compare_(compare) {
}
@ -69,7 +69,7 @@ namespace gtsam {
* Constructor - allows inexact evaluation
*/
NonlinearEquality(const Key& j, const T& feasible, double error_gain, bool (*compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(dim(feasible)), j), feasible_(feasible),
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain),
compare_(compare) {
}
@ -77,7 +77,7 @@ namespace gtsam {
void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << dim(feasible_) << std::endl;
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
}
/** Check if two factors are equal */
@ -102,10 +102,10 @@ namespace gtsam {
/** error function */
inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
size_t nj = dim(feasible_);
size_t nj = feasible_.dim();
if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
return logmap(xj, feasible_);
return xj.logmap(feasible_);
} else if (compare_(feasible_,xj)) {
if (H) *H = eye(nj);
return zero(nj); // set error to zero if equal

4
nonlinear/NonlinearOptimizer-inl.h
View File

@ -86,7 +86,7 @@ namespace gtsam {
delta.print("delta");
// take old config and update it
shared_config newConfig(new C(expmap(*config_,delta)));
shared_config newConfig(new C(config_->expmap(delta)));
// maybe show output
if (verbosity >= CONFIG)
@ -154,7 +154,7 @@ namespace gtsam {
delta.print("delta");
// update config
shared_config newConfig(new C(expmap(*config_,delta))); // TODO: updateConfig
shared_config newConfig(new C(config_->expmap(delta))); // TODO: updateConfig
// if (verbosity >= TRYCONFIG)
// newConfig->print("config");

60
nonlinear/TupleConfig.h
View File

@ -173,12 +173,12 @@ namespace gtsam {
/** Expmap */
TupleConfig<Config1, Config2> expmap(const VectorConfig& delta) const {
return TupleConfig(gtsam::expmap(first_, delta), second_.expmap(delta));
return TupleConfig(first_.expmap(delta), second_.expmap(delta));
}
/** logmap each element */
VectorConfig logmap(const TupleConfig<Config1, Config2>& cp) const {
VectorConfig ret(gtsam::logmap(first_, cp.first_));
VectorConfig ret(first_.logmap(cp.first_));
ret.insert(second_.logmap(cp.second_));
return ret;
}
@ -267,11 +267,11 @@ namespace gtsam {
size_t dim() const { return first_.dim(); }
TupleConfigEnd<Config> expmap(const VectorConfig& delta) const {
return TupleConfigEnd(gtsam::expmap(first_, delta));
return TupleConfigEnd(first_.expmap(delta));
}
VectorConfig logmap(const TupleConfigEnd<Config>& cp) const {
VectorConfig ret(gtsam::logmap(first_, cp.first_));
VectorConfig ret(first_.logmap(cp.first_));
return ret;
}
@ -283,18 +283,6 @@ namespace gtsam {
}
};
/** Exmap static functions */
template<class Config1, class Config2>
inline TupleConfig<Config1, Config2> expmap(const TupleConfig<Config1, Config2>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
/** logmap static functions */
template<class Config1, class Config2>
inline VectorConfig logmap(const TupleConfig<Config1, Config2>& c0, const TupleConfig<Config1, Config2>& cp) {
return c0.logmap(cp);
}
/**
* Wrapper classes to act as containers for configs. Note that these can be cascaded
* recursively, as they are TupleConfigs, and are primarily a short form of the config
@ -321,16 +309,6 @@ namespace gtsam {
inline const Config1& first() const { return this->config(); }
};
template<class C1>
TupleConfig1<C1> expmap(const TupleConfig1<C1>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class C1>
VectorConfig logmap(const TupleConfig1<C1>& c1, const TupleConfig1<C1>& c2) {
return c1.logmap(c2);
}
template<class C1, class C2>
class TupleConfig2 : public TupleConfig<C1, TupleConfigEnd<C2> > {
public:
@ -351,16 +329,6 @@ namespace gtsam {
inline const Config2& second() const { return this->rest().config(); }
};
template<class C1, class C2>
TupleConfig2<C1, C2> expmap(const TupleConfig2<C1, C2>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class C1, class C2>
VectorConfig logmap(const TupleConfig2<C1, C2>& c1, const TupleConfig2<C1, C2>& c2) {
return c1.logmap(c2);
}
template<class C1, class C2, class C3>
class TupleConfig3 : public TupleConfig<C1, TupleConfig<C2, TupleConfigEnd<C3> > > {
public:
@ -380,11 +348,6 @@ namespace gtsam {
inline const Config3& third() const { return this->rest().rest().config(); }
};
template<class C1, class C2, class C3>
TupleConfig3<C1, C2, C3> expmap(const TupleConfig3<C1, C2, C3>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class C1, class C2, class C3, class C4>
class TupleConfig4 : public TupleConfig<C1, TupleConfig<C2,TupleConfig<C3, TupleConfigEnd<C4> > > > {
public:
@ -409,11 +372,6 @@ namespace gtsam {
inline const Config4& fourth() const { return this->rest().rest().rest().config(); }
};
template<class C1, class C2, class C3, class C4>
TupleConfig4<C1, C2, C3, C4> expmap(const TupleConfig4<C1, C2, C3, C4>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class C1, class C2, class C3, class C4, class C5>
class TupleConfig5 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfigEnd<C5> > > > > {
public:
@ -438,11 +396,6 @@ namespace gtsam {
inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); }
};
template<class C1, class C2, class C3, class C4, class C5>
TupleConfig5<C1, C2, C3, C4, C5> expmap(const TupleConfig5<C1, C2, C3, C4, C5>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
template<class C1, class C2, class C3, class C4, class C5, class C6>
class TupleConfig6 : public TupleConfig<C1, TupleConfig<C2, TupleConfig<C3, TupleConfig<C4, TupleConfig<C5, TupleConfigEnd<C6> > > > > > {
public:
@ -468,9 +421,4 @@ namespace gtsam {
inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); }
};
template<class C1, class C2, class C3, class C4, class C5, class C6>
TupleConfig6<C1, C2, C3, C4, C5, C6> expmap(const TupleConfig6<C1, C2, C3, C4, C5, C6>& c, const VectorConfig& delta) {
return c.expmap(delta);
}
}

6
nonlinear/tests/testLieConfig.cpp
View File

@ -126,7 +126,7 @@ TEST(LieConfig, expmap_a)
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, expmap(config0, increment)));
CHECK(assert_equal(expected, config0.expmap(increment)));
}
/* ************************************************************************* */
@ -143,7 +143,7 @@ TEST(LieConfig, expmap_b)
expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, expmap(config0, increment)));
CHECK(assert_equal(expected, config0.expmap(increment)));
}
/* ************************************************************************* */
@ -161,7 +161,7 @@ TEST(LieConfig, expmap_c)
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, expmap(config0, increment)));
CHECK(assert_equal(expected, config0.expmap(increment)));
}
/* ************************************************************************* */

2
slam/BearingFactor.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 hx = pose.bearing(point, H1, H2);
return logmap(between(z_, hx));
return Rot2::Logmap(z_.between(hx));
}
/** return the measured */

2
slam/BearingRangeFactor.h
View File

@ -44,7 +44,7 @@ namespace gtsam {
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Rot2 y1 = pose.bearing(point, H11_, H12_);
Vector e1 = logmap(between(bearing_, y1));
Vector e1 = Rot2::Logmap(bearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - range_);

4
slam/BetweenConstraint.h
View File

@ -35,8 +35,8 @@ namespace gtsam {
Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
X hx = between(x1, x2, H1, H2);
return logmap(measured_, hx);
X hx = x1.between(x2, H1, H2);
return measured_.logmap(hx);
}
inline const X measured() const {

4
slam/BetweenFactor.h
View File

@ -63,9 +63,9 @@ namespace gtsam {
/** vector of errors */
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
T hx = between(p1, p2, H1, H2); // h(x)
T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
return logmap(measured_, hx);
return measured_.logmap(hx);
}
/** return the measured */

2
slam/LinearApproxFactor.h
View File

@ -70,7 +70,7 @@ public:
VectorConfig delta;
BOOST_FOREACH(const Key& key, nonlinearKeys_) {
X newPt = c[key], linPt = lin_points_[key];
Vector d = logmap(linPt, newPt);
Vector d = linPt.logmap(newPt);
delta.insert(key, d);
}

2
slam/Pose2SLAMOptimizer.cpp
View File

@ -49,7 +49,7 @@ namespace gtsam {
/* ************************************************************************* */
void Pose2SLAMOptimizer::update(const Vector& x) {
VectorConfig X = system_->assembleConfig(x, *solver_.ordering());
*theta_ = expmap(*theta_, X);
*theta_ = theta_->expmap(X);
linearize();
}

4
slam/PriorFactor.h
View File

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

2
slam/TransformConstraint.h
View File

@ -54,7 +54,7 @@ public:
Point dummy;
*Dlocal = -1* eye(dummy.dim());
}
return gtsam::logmap(gtsam::between<Point>(local, newlocal));
return Point::Logmap(local.between(newlocal));
}
};
} // \namespace gtsam

4
slam/dataset.cpp
View File

@ -132,7 +132,7 @@ pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
}
if (addNoise)
l1Xl2 = expmap(l1Xl2,(*model)->sample());
l1Xl2 = l1Xl2.expmap((*model)->sample());
// Insert vertices if pure odometry file
if (!poses->exists(id1)) poses->insert(id1, Pose2());
@ -170,7 +170,7 @@ void save2D(const Pose2Graph& graph, const Pose2Config& config, const SharedDiag
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
if (!factor) continue;
pose = inverse(factor->measured());
pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)

2
slam/simulated2DOriented.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
/* ************************************************************************* */
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
return between(x1, x2, H1, H2);
return x1.between(x2, H1, H2);
}
/* ************************************************************************* */

6
slam/simulated2DOriented.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
* odometry between two poses, and optional derivative version
*/
inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
return between(x1, x2);
return x1.between(x2);
}
Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
@ -58,7 +58,7 @@ namespace gtsam {
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const {
return logmap(z_, prior(x, H));
return z_.logmap(prior(x, H));
}
};
@ -77,7 +77,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 logmap(z_, odo(x1, x2, H1, H2));
return z_.logmap(odo(x1, x2, H1, H2));
}
};

2
slam/tests/testPose2Config.cpp
View File

@ -37,7 +37,7 @@ TEST( Pose2Config, expmap )
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0);
Pose2Config actual = expmap(pose2SLAM::circle(4,1.0),delta);
Pose2Config actual = pose2SLAM::circle(4,1.0).expmap(delta);
CHECK(assert_equal(expected,actual));
}

6
slam/tests/testPose2Factor.cpp
View File

@ -36,7 +36,7 @@ TEST( Pose2Factor, error )
x0.insert(2, p2);
// Create factor
Pose2 z = between(p1,p2);
Pose2 z = p1.between(p2);
Pose2Factor factor(1, 2, z, covariance);
// Actual linearization
@ -53,7 +53,7 @@ TEST( Pose2Factor, error )
// Check error after increasing p2
VectorConfig plus = delta;
plus.insertAdd("x2", Vector_(3, 0.1, 0.0, 0.0));
Pose2Config x1 = expmap(x0, plus);
Pose2Config x1 = x0.expmap(plus);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -78,7 +78,7 @@ TEST( Pose2Factor, rhs )
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
// Check RHS
Pose2 hx0 = between(p1,p2);
Pose2 hx0 = p1.between(p2);
CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0));
Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
CHECK(assert_equal(expected_b,-factor.whitenedError(x0)));

2
slam/tests/testPose2Prior.cpp
View File

@ -43,7 +43,7 @@ TEST( Pose2Prior, error )
// Check error after increasing p2
VectorConfig plus = delta + VectorConfig("x1", Vector_(3, 0.1, 0.0, 0.0));
Pose2Config x1 = expmap(x0, plus);
Pose2Config x1 = x0.expmap(plus);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));

20
slam/tests/testPose2SLAM.cpp
View File

@ -126,7 +126,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addHardConstraint(0, p0);
Pose2 delta = between(p0,p1);
Pose2 delta = p0.between(p1);
fg->addConstraint(0, 1, delta, covariance);
fg->addConstraint(1, 2, delta, covariance);
fg->addConstraint(2, 0, delta, covariance);
@ -134,8 +134,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
// Create initial config
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
initial->insert(0, p0);
initial->insert(1, expmap(hexagon[1],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, expmap(hexagon[2],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -164,7 +164,7 @@ TEST(Pose2Graph, optimizeCircle) {
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addHardConstraint(0, p0);
Pose2 delta = between(p0,p1);
Pose2 delta = p0.between(p1);
fg->addConstraint(0, 1, delta, covariance);
fg->addConstraint(1,2, delta, covariance);
fg->addConstraint(2,3, delta, covariance);
@ -175,11 +175,11 @@ TEST(Pose2Graph, optimizeCircle) {
// Create initial config
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
initial->insert(0, p0);
initial->insert(1, expmap(hexagon[1],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, expmap(hexagon[2],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(3, expmap(hexagon[3],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(4, expmap(hexagon[4],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(5, expmap(hexagon[5],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(1, hexagon[1].expmap(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].expmap(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].expmap(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].expmap(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].expmap(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering
shared_ptr<Ordering> ordering(new Ordering);
@ -197,7 +197,7 @@ TEST(Pose2Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta,between(actual[5],actual[0])));
CHECK(assert_equal(delta,actual[5].between(actual[0])));
// Pose2SLAMOptimizer myOptimizer("3");

2
slam/tests/testPose3Config.cpp
View File

@ -56,7 +56,7 @@ TEST( Pose3Config, expmap )
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0);
Pose3Config actual = expmap(pose3SLAM::circle(4,1.0),delta);
Pose3Config actual = pose3SLAM::circle(4,1.0).expmap(delta);
CHECK(assert_equal(expected,actual));
}

2
slam/tests/testPose3Factor.cpp
View File

@ -33,7 +33,7 @@ TEST( Pose3Factor, error )
// Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2))
Vector actual = factor.unwhitenedError(x);
Vector expected = logmap(z,between(t1,t2));
Vector expected = z.logmap(t1.between(t2));
CHECK(assert_equal(expected,actual));
}

14
slam/tests/testPose3SLAM.cpp
View File

@ -38,7 +38,7 @@ TEST(Pose3Graph, optimizeCircle) {
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph);
fg->addHardConstraint(0, gT0);
Pose3 _0T1 = between(gT0,gT1); // inv(gT0)*gT1
Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1
double theta = M_PI/3.0;
CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1));
fg->addConstraint(0,1, _0T1, covariance);
@ -51,11 +51,11 @@ TEST(Pose3Graph, optimizeCircle) {
// Create initial config
boost::shared_ptr<Pose3Config> initial(new Pose3Config());
initial->insert(0, gT0);
initial->insert(1, expmap(hexagon[1],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(2, expmap(hexagon[2],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(3, expmap(hexagon[3],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(4, expmap(hexagon[4],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(5, expmap(hexagon[5],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(1, hexagon[1].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(2, hexagon[2].expmap(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(3, hexagon[3].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(4, hexagon[4].expmap(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(5, hexagon[5].expmap(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
@ -73,7 +73,7 @@ TEST(Pose3Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure
CHECK(assert_equal(_0T1,between(actual[5],actual[0]),1e-5));
CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5));
}
/* ************************************************************************* */

2
slam/tests/testVSLAMConfig.cpp
View File

@ -31,7 +31,7 @@ TEST( Config, update_with_large_delta) {
delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));
delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1));
delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
Config actual = expmap(init, delta);
Config actual = init.expmap(delta);
CHECK(assert_equal(expected,actual));
}

48
slam/tests/testVSLAMFactor.cpp
View File

@ -36,29 +36,29 @@ TEST( ProjectionFactor, error )
Point2 z(323.,240.);
int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<ProjectionFactor>
factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK));
// For the following configuration, the factor predicts 320,240
Config config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1);
// Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
// For the following configuration, the factor predicts 320,240
Config config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1);
// Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config)));
// Which yields an error of 3^2/2 = 4.5
DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
// Which yields an error of 3^2/2 = 4.5
DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
// Check linearize
Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
Vector b = Vector_(2,3.,0.);
SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1);
GaussianFactor::shared_ptr actual = factor->linearize(config);
CHECK(assert_equal(expected,*actual,1e-3));
// Check linearize
Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
Vector b = Vector_(2,3.,0.);
SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1);
GaussianFactor::shared_ptr actual = factor->linearize(config);
CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph
Graph graph;
// linearize graph
Graph graph;
graph.push_back(factor);
GaussianFactorGraph expected_lfg;
expected_lfg.push_back(actual);
@ -69,11 +69,11 @@ TEST( ProjectionFactor, error )
VectorConfig delta;
delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.));
delta.insert("l1",Vector_(3, 1.,2.,3.));
Config actual_config = expmap(config, delta);
Config expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2);
CHECK(assert_equal(expected_config,actual_config,1e-9));
Config actual_config = config.expmap(delta);
Config expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2);
CHECK(assert_equal(expected_config,actual_config,1e-9));
}
/* ************************************************************************* */

2
tests/testGraph.cpp
View File

@ -73,7 +73,7 @@ TEST( Graph, composePoses )
Pose2Graph graph;
Matrix cov = eye(3);
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Pose2 p12=between(p1,p2), p23=between(p2,p3), p43=between(p4,p3);
Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
graph.addConstraint(1,2, p12, cov);
graph.addConstraint(2,3, p23, cov);
graph.addConstraint(4,3, p43, cov);

2
tests/testNonlinearEqualityConstraint.cpp
View File

@ -168,7 +168,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// odometry constraint
eq2D::OdoEqualityConstraint::shared_ptr constraint2(
new eq2D::OdoEqualityConstraint(
gtsam::between(truth_pt1, truth_pt2), key1, key2));
truth_pt1.between(truth_pt2), key1, key2));
shared_graph graph(new Graph());
graph->push_back(constraint1);

2
tests/testTransformConstraint.cpp
View File

@ -230,7 +230,7 @@ TEST( TransformConstraint, converge_global ) {
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealForeign = inverse(trans).transform_from(local);
Point2 idealForeign = trans.inverse().transform_from(local);
// perturb the initial estimate
// Point2 global = idealForeign; // Ideal - works

37
tests/testTupleConfig.cpp
View File

@ -16,7 +16,6 @@
#include <gtsam/base/Vector.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/VectorConfig.h>
#include <gtsam/nonlinear/TupleConfig-inl.h>
using namespace gtsam;
@ -179,24 +178,24 @@ TEST(TupleConfig, zero_expmap_logmap)
delta.insert("l2", Vector_(2, 1.3, 1.4));
Config expected;
expected.insert(PoseKey(1), expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(PoseKey(2), expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(PoseKey(1), x1.expmap(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(PoseKey(2), x2.expmap(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(PointKey(1), Point2(5.0, 6.1));
expected.insert(PointKey(2), Point2(10.3, 11.4));
Config actual = expmap(config1, delta);
Config actual = config1.expmap(delta);
CHECK(assert_equal(expected, actual));
// Check log
VectorConfig expected_log = delta;
VectorConfig actual_log = logmap(config1,actual);
VectorConfig actual_log = config1.logmap(actual);
CHECK(assert_equal(expected_log, actual_log));
}
/* ************************************************************************* */
// some key types
typedef TypedSymbol<Vector, 'L'> LamKey;
typedef TypedSymbol<LieVector, 'L'> LamKey;
typedef TypedSymbol<Pose3, 'a'> Pose3Key;
typedef TypedSymbol<Point3, 'b'> Point3Key;
typedef TypedSymbol<Point3, 'c'> Point3Key2;
@ -261,7 +260,7 @@ TEST(TupleConfig, basic_functions) {
LamKey L1(1);
Pose2 pose1(1.0, 2.0, 0.3);
Point2 point1(2.0, 3.0);
Vector lam1 = Vector_(1, 2.3);
LieVector lam1 = LieVector(2.3);
// Insert
configA.insert(x1, pose1);
@ -331,7 +330,7 @@ TEST(TupleConfig, insert_config) {
LamKey L1(1), L2(2);
Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
Point2 point1(2.0, 3.0), point2(5.0, 6.0);
Vector lam1 = Vector_(1, 2.3), lam2 = Vector_(1, 4.5);
LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5);
config1.insert(x1, pose1);
config1.insert(l1, point1);
@ -438,13 +437,13 @@ TEST(TupleConfig, expmap)
delta.insert("l2", Vector_(2, 1.3, 1.4));
ConfigA expected;
expected.insert(x1k, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(l1k, Point2(5.0, 6.1));
expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, expmap(config1, delta)));
CHECK(assert_equal(delta, logmap(config1, expected)));
CHECK(assert_equal(expected, config1.expmap(delta)));
CHECK(assert_equal(delta, config1.logmap(expected)));
}
/* ************************************************************************* */
@ -467,13 +466,13 @@ TEST(TupleConfig, expmap_typedefs)
delta.insert("l1", Vector_(2, 1.0, 1.1));
delta.insert("l2", Vector_(2, 1.3, 1.4));
expected.insert(x1k, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(x1k, x1.expmap(Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(x2k, x2.expmap(Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(l1k, Point2(5.0, 6.1));
expected.insert(l2k, Point2(10.3, 11.4));
CHECK(assert_equal(expected, expmap(config1, delta)));
//CHECK(assert_equal(delta, logmap(config1, expected)));
CHECK(assert_equal(expected, TupleConfig2<PoseConfig, PointConfig>(config1.expmap(delta))));
//CHECK(assert_equal(delta, config1.logmap(expected)));
}
/* ************************************************************************* */
@ -494,7 +493,7 @@ TEST( TupleConfig, pairconfig_style )
LamKey L1(1);
Pose2 pose1(1.0, 2.0, 0.3);
Point2 point1(2.0, 3.0);
Vector lam1 = Vector_(1, 2.3);
LieVector lam1 = LieVector(2.3);
PoseConfig config1; config1.insert(x1, pose1);
PointConfig config2; config2.insert(l1, point1);
@ -519,7 +518,7 @@ TEST(TupleConfig, insert_config_typedef) {
LamKey L1(1), L2(2);
Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
Point2 point1(2.0, 3.0), point2(5.0, 6.0);
Vector lam1 = Vector_(1, 2.3), lam2 = Vector_(1, 4.5);
LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5);
config1.insert(x1, pose1);
config1.insert(l1, point1);
@ -550,7 +549,7 @@ TEST(TupleConfig, partial_insert) {
LamKey L1(1), L2(2);
Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
Point2 point1(2.0, 3.0), point2(5.0, 6.0);
Vector lam1 = Vector_(1, 2.3), lam2 = Vector_(1, 4.5);
LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5);
init.insert(x1, pose1);
init.insert(l1, point1);