From 23a30f8475aa5f73ae786cb5ce6d6e2033a5b6c2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 26 Aug 2010 19:55:40 +0000 Subject: [PATCH] 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. --- .cproject | 227 ++++++++++------- Makefile.am | 2 +- base/Lie-inl.h | 25 +- base/Lie.h | 166 +++++++------ base/LieVector.cpp | 33 +++ base/LieVector.h | 25 +- base/Makefile.am | 3 +- base/lieProxies.h | 48 ++++ base/numericalDerivative.h | 39 ++- base/tests/testLieVector.cpp | 14 ++ configure.ac | 3 +- examples/Makefile.am | 2 +- examples/SimpleRotation.cpp | 7 +- geometry/Cal3_S2.h | 27 +- geometry/CalibratedCamera.cpp | 2 +- geometry/Point2.h | 46 ++-- geometry/Point3.cpp | 9 - geometry/Point3.h | 22 +- geometry/Pose2.cpp | 28 +-- geometry/Pose2.h | 56 +++-- geometry/Pose3.cpp | 68 +++-- geometry/Pose3.h | 55 ++-- geometry/Rot2.h | 9 + geometry/Rot3.cpp | 20 +- geometry/Rot3.h | 52 ++-- geometry/StereoCamera.cpp | 61 ----- geometry/StereoCamera.h | 22 +- geometry/StereoPoint2.h | 13 + geometry/Tensor2.h | 3 + geometry/tests/testHomography2.cpp | 7 +- geometry/tests/testPoint2.cpp | 2 +- geometry/tests/testPose2.cpp | 217 ++++++++-------- geometry/tests/testPose3.cpp | 82 +++--- geometry/tests/testRot2.cpp | 6 +- geometry/tests/testRot3.cpp | 82 +++--- inference/ISAM2-inl.h | 6 +- inference/ISAM2.h | 4 +- inference/graph-inl.h | 4 +- linear/VectorMap.h | 290 +++++++++++----------- nonlinear/LieConfig-inl.h | 60 +++-- nonlinear/LieConfig.h | 43 ++-- nonlinear/NonlinearConstraint.h | 4 +- nonlinear/NonlinearEquality.h | 10 +- nonlinear/NonlinearOptimizer-inl.h | 4 +- nonlinear/TupleConfig.h | 60 +---- nonlinear/tests/testLieConfig.cpp | 6 +- slam/BearingFactor.h | 2 +- slam/BearingRangeFactor.h | 2 +- slam/BetweenConstraint.h | 4 +- slam/BetweenFactor.h | 4 +- slam/LinearApproxFactor.h | 2 +- slam/Pose2SLAMOptimizer.cpp | 2 +- slam/PriorFactor.h | 4 +- slam/TransformConstraint.h | 2 +- slam/dataset.cpp | 4 +- slam/simulated2DOriented.cpp | 2 +- slam/simulated2DOriented.h | 6 +- slam/tests/testPose2Config.cpp | 2 +- slam/tests/testPose2Factor.cpp | 6 +- slam/tests/testPose2Prior.cpp | 2 +- slam/tests/testPose2SLAM.cpp | 20 +- slam/tests/testPose3Config.cpp | 2 +- slam/tests/testPose3Factor.cpp | 2 +- slam/tests/testPose3SLAM.cpp | 14 +- slam/tests/testVSLAMConfig.cpp | 2 +- slam/tests/testVSLAMFactor.cpp | 56 ++--- tests/testGraph.cpp | 2 +- tests/testNonlinearEqualityConstraint.cpp | 2 +- tests/testTransformConstraint.cpp | 2 +- tests/testTupleConfig.cpp | 37 ++- 70 files changed, 1096 insertions(+), 1061 deletions(-) create mode 100644 base/LieVector.cpp create mode 100644 base/lieProxies.h diff --git a/.cproject b/.cproject index 42bf725382..f54873f0eb 100644 --- a/.cproject +++ b/.cproject @@ -302,7 +302,6 @@ make - all true true @@ -310,7 +309,6 @@ make - clean true true @@ -318,7 +316,6 @@ make - check true true @@ -326,7 +323,6 @@ make - testGaussianConditional.run true true @@ -334,7 +330,6 @@ make - testGaussianFactor.run true true @@ -342,7 +337,6 @@ make - timeGaussianFactor.run true true @@ -350,7 +344,6 @@ make - timeVectorConfig.run true true @@ -358,7 +351,6 @@ make - testVectorBTree.run true true @@ -366,7 +358,6 @@ make - testVectorMap.run true true @@ -374,7 +365,6 @@ make - testNoiseModel.run true true @@ -382,7 +372,6 @@ make - testBayesNetPreconditioner.run true true @@ -390,7 +379,6 @@ make - testErrors.run true false @@ -398,6 +386,7 @@ make + check true true @@ -405,6 +394,7 @@ make + tests/testSPQRUtil.run true true @@ -412,6 +402,7 @@ make + clean true true @@ -419,7 +410,6 @@ make - check true true @@ -427,7 +417,6 @@ make - tests/testGaussianJunctionTree.run true true @@ -435,6 +424,7 @@ make + check true true @@ -442,6 +432,7 @@ make + clean true true @@ -449,6 +440,7 @@ make + testBTree.run true true @@ -456,6 +448,7 @@ make + testDSF.run true true @@ -463,6 +456,7 @@ make + testDSFVector.run true true @@ -470,6 +464,7 @@ make + testMatrix.run true true @@ -477,6 +472,7 @@ make + testSPQRUtil.run true true @@ -484,6 +480,7 @@ make + testVector.run true true @@ -491,6 +488,7 @@ make + timeMatrix.run true true @@ -498,6 +496,7 @@ make + all true true @@ -505,7 +504,6 @@ make - all true true @@ -513,7 +511,6 @@ make - clean true true @@ -529,7 +526,6 @@ make - testBayesTree.run true false @@ -537,7 +533,6 @@ make - testBinaryBayesNet.run true false @@ -545,7 +540,6 @@ make - testFactorGraph.run true true @@ -553,7 +547,6 @@ make - testISAM.run true true @@ -561,7 +554,6 @@ make - testJunctionTree.run true true @@ -569,7 +561,6 @@ make - testKey.run true true @@ -577,7 +568,6 @@ make - testOrdering.run true true @@ -585,7 +575,6 @@ make - testSymbolicBayesNet.run true false @@ -593,7 +582,6 @@ make - testSymbolicFactor.run true false @@ -601,7 +589,6 @@ make - testSymbolicFactorGraph.run true false @@ -609,7 +596,6 @@ make - timeSymbolMaps.run true true @@ -617,7 +603,6 @@ make - check true true @@ -625,7 +610,6 @@ make - testClusterTree.run true true @@ -633,7 +617,6 @@ make - testJunctionTree.run true true @@ -641,7 +624,6 @@ make - testEliminationTree.run true true @@ -649,6 +631,7 @@ make + check true true @@ -656,6 +639,7 @@ make + testGaussianFactorGraph.run true true @@ -663,6 +647,7 @@ make + testGaussianISAM.run true true @@ -670,6 +655,7 @@ make + testGaussianISAM2.run true true @@ -677,6 +663,7 @@ make + testGraph.run true false @@ -684,6 +671,7 @@ make + testIterative.run true true @@ -691,6 +679,7 @@ make + testNonlinearEquality.run true true @@ -698,6 +687,7 @@ make + testNonlinearFactor.run true true @@ -705,6 +695,7 @@ make + testNonlinearFactorGraph.run true true @@ -712,6 +703,7 @@ make + testNonlinearOptimizer.run true true @@ -719,6 +711,7 @@ make + testSQP.run true true @@ -726,6 +719,7 @@ make + testSubgraphPreconditioner.run true true @@ -733,6 +727,7 @@ make + testTupleConfig.run true true @@ -740,6 +735,7 @@ make + timeGaussianFactorGraph.run true true @@ -747,6 +743,7 @@ make + testBayesNetPreconditioner.run true true @@ -754,6 +751,7 @@ make + testConstraintOptimizer.run true true @@ -761,6 +759,7 @@ make + testInference.run true false @@ -768,6 +767,7 @@ make + testGaussianBayesNet.run true false @@ -775,6 +775,7 @@ make + testGaussianFactor.run true false @@ -782,6 +783,7 @@ make + testJunctionTree.run true false @@ -789,6 +791,7 @@ make + testSymbolicBayesNet.run true false @@ -796,6 +799,7 @@ make + testSymbolicFactorGraph.run true false @@ -803,7 +807,6 @@ make - clean true true @@ -811,7 +814,6 @@ make - all true true @@ -819,7 +821,6 @@ make - testNonlinearConstraint.run true true @@ -827,7 +828,6 @@ make - testLieConfig.run true true @@ -835,7 +835,6 @@ make - testConstraintOptimizer.run true true @@ -843,7 +842,6 @@ make - install true true @@ -851,7 +849,6 @@ make - clean true true @@ -859,6 +856,7 @@ make + all true true @@ -866,6 +864,7 @@ make + clean true true @@ -873,6 +872,7 @@ make + all true true @@ -880,6 +880,7 @@ make + clean true true @@ -887,6 +888,7 @@ make + all true true @@ -894,6 +896,7 @@ make + clean true true @@ -901,7 +904,6 @@ make - all true true @@ -909,7 +911,6 @@ make - clean true true @@ -917,6 +918,7 @@ make + clean all true true @@ -924,6 +926,7 @@ make + all true true @@ -931,6 +934,7 @@ make + check true true @@ -938,6 +942,7 @@ make + clean true true @@ -945,6 +950,7 @@ make + testPlanarSLAM.run true true @@ -952,6 +958,7 @@ make + testPose2Config.run true true @@ -959,6 +966,7 @@ make + testPose2Factor.run true true @@ -966,6 +974,7 @@ make + testPose2Prior.run true true @@ -973,6 +982,7 @@ make + testPose2SLAM.run true true @@ -980,6 +990,7 @@ make + testPose3Config.run true true @@ -987,6 +998,7 @@ make + testPose3SLAM.run true true @@ -994,6 +1006,7 @@ make + testSimulated2DOriented.run true false @@ -1001,6 +1014,7 @@ make + testVSLAMConfig.run true true @@ -1008,6 +1022,7 @@ make + testVSLAMFactor.run true true @@ -1015,6 +1030,7 @@ make + testVSLAMGraph.run true true @@ -1022,6 +1038,7 @@ make + testPose3Factor.run true true @@ -1029,6 +1046,7 @@ make + testSimulated2D.run true false @@ -1036,6 +1054,7 @@ make + testSimulated3D.run true false @@ -1043,6 +1062,7 @@ make + check true true @@ -1050,7 +1070,6 @@ make - timeCalibratedCamera.run true true @@ -1058,7 +1077,6 @@ make - timeRot3.run true true @@ -1066,19 +1084,42 @@ make - clean true true true + + make + + SimpleRotation.run + true + true + true + make + check true true true + + make + + check + true + true + true + + + make + tests/testLieConfig.run + true + true + true + make -j2 @@ -1097,29 +1138,54 @@ make - clean true true true - + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + make + -j2 check true true true - + make - tests/testLieConfig.run + all + true + true + true + + + make + + dist true true true make + testRot3.run true true @@ -1127,6 +1193,7 @@ make + testRot2.run true true @@ -1134,6 +1201,7 @@ make + testPose3.run true true @@ -1141,6 +1209,7 @@ make + timeRot3.run true true @@ -1148,6 +1217,7 @@ make + testPose2.run true true @@ -1155,6 +1225,7 @@ make + testCal3_S2.run true true @@ -1162,6 +1233,7 @@ make + testSimpleCamera.run true true @@ -1169,6 +1241,7 @@ make + testHomography2.run true true @@ -1176,6 +1249,7 @@ make + testCalibratedCamera.run true true @@ -1183,6 +1257,7 @@ make + check true true @@ -1190,6 +1265,7 @@ make + clean true true @@ -1197,52 +1273,14 @@ make + testPoint2.run true true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - all - true - true - true - - - make - dist - true - true - true - make - check true true @@ -1250,7 +1288,6 @@ make - testGaussianJunctionTree.run true true @@ -1258,7 +1295,6 @@ make - testGaussianFactorGraph.run true true @@ -1266,7 +1302,6 @@ make - timeGaussianFactorGraph.run true true @@ -1274,6 +1309,7 @@ make + testTupleConfig.run true true @@ -1281,14 +1317,22 @@ make + testFusionTupleConfig.run true true true - + make + SimpleRotation.run + true + true + true + + + make check true true @@ -1296,7 +1340,6 @@ make - clean true true @@ -1304,7 +1347,6 @@ make - install true true @@ -1312,7 +1354,6 @@ make - all true true diff --git a/Makefile.am b/Makefile.am index 03b02cf632..77b3337f9e 100644 --- a/Makefile.am +++ b/Makefile.am @@ -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 \ diff --git a/base/Lie-inl.h b/base/Lie-inl.h index 002d817404..28cb13164f 100644 --- a/base/Lie-inl.h +++ b/base/Lie-inl.h @@ -10,29 +10,10 @@ #include #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; -namespace gtsam { - /** - * Returns Exponential mapy - * This is for matlab wrapper - */ - template - T Lie::expmap(const Vector& v) const { - return gtsam::expmap(*((T*)this),v); - } - - /** - * Returns Log map - */ - template - Vector Lie::logmap(const T& lp) const { - return gtsam::logmap(*((T*)this),lp); - } - -} diff --git a/base/Lie.h b/base/Lie.h index aacd0a5310..40f579dd03 100644 --- a/base/Lie.h +++ b/base/Lie.h @@ -18,102 +18,118 @@ namespace gtsam { * for better performance. */ - /* Exponential map about identity */ + /** Compute l0 s.t. l2=l1*l0 */ template - T expmap(const Vector& v) { return T::Expmap(v); } - - /* Logmap (inverse exponential map) about identity */ - template - Vector logmap(const T& p) { return T::Logmap(p); } - - /** Compute l1 s.t. l2=l1*l0 */ - template - 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 - 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 - inline T expmap(const T& t, const Vector& d) { return compose(t,expmap(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 Lie { - public: - - /** - * Returns dimensionality of the tangent space - */ - inline size_t dim() const { - return static_cast(this)->dim(); - } - - /** - * Returns Exponential map update of T - * Default implementation calls global binary function - */ - T expmap(const Vector& v) const; - - /** expmap around identity */ - static T Expmap(const Vector& v) { - return T::Expmap(v); - } - - /** - * Returns Log map - * Default Implementation calls global binary function - */ - Vector logmap(const T& lp) const; - - /** Logmap around identity */ - static Vector Logmap(const T& p) { - return T::Logmap(p); - } - - /** compose with another object */ - inline T compose(const T& p) const { - return static_cast(this)->compose(p); - } - - /** invert the object and yield a new one */ - inline T inverse() const { - return static_cast(this)->inverse(); - } + private: - }; - - /** get the dimension of an object with a global function */ - template - inline size_t dim(const T& object) { - return object.dim(); - } + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** compose two Lie types */ - template - inline T compose(const T& p1, const T& p2) { - return p1.compose(p2); - } + /** assignment */ + T t2 = t; - /** invert an object */ - template - inline T inverse(const T& p) { - return p.inverse(); - } + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); + + /** + * Returns Exponential map update of T + * Default implementation calls global binary function + */ + T expmap_ret = t.expmap(gtsam::zero(dim_ret)); + + /** expmap around identity */ + T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); + + /** + * Returns Log map + * Default Implementation calls global binary function + */ + Vector logmap_ret = t.logmap(t2); + /** Logmap around identity */ + Vector logmap_identity_ret = T::Logmap(t); + + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = t.between(t2); + + /** compose with another object */ + T compose_ret = t.compose(t2); + + /** invert the object and yield a new one */ + T inverse_ret = t.inverse(); + } + + /** + * 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; + + }; + /** Call print on the object */ template inline void print(const T& object, const std::string& s = "") { diff --git a/base/LieVector.cpp b/base/LieVector.cpp new file mode 100644 index 0000000000..77d3e57a60 --- /dev/null +++ b/base/LieVector.cpp @@ -0,0 +1,33 @@ +/** + * @file LieVector.cpp + * @brief Implementations for LieVector functions + * @author Alex Cunningham + */ + +#include +#include + +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 diff --git a/base/LieVector.h b/base/LieVector.h index 9b981036b8..21e8868bf0 100644 --- a/base/LieVector.h +++ b/base/LieVector.h @@ -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(*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()); diff --git a/base/Makefile.am b/base/Makefile.am index a40f49997c..9db9942505 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -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 diff --git a/base/lieProxies.h b/base/lieProxies.h new file mode 100644 index 0000000000..38cfcdb1a8 --- /dev/null +++ b/base/lieProxies.h @@ -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 + T between(const T& t1, const T& t2) { return t1.between(t2); } + + template + T compose(const T& t1, const T& t2) { return t1.compose(t2); } + + /** expmap and logmap */ + template + Vector logmap(const T& t1, const T& t2) { return t1.logmap(t2); } + + template + T expmap(const T& t1, const Vector& t2) { return t1.expmap(t2); } + + /** unary functions */ + template + T inverse(const T& t) { return t.inverse(); } + + /** rotation functions */ + template + P rotate(const T& r, const P& pt) { return r.rotate(pt); } + + template + P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } + +} +} + diff --git a/base/numericalDerivative.h b/base/numericalDerivative.h index 98a3836316..e7f74b70e0 100644 --- a/base/numericalDerivative.h +++ b/base/numericalDerivative.h @@ -83,12 +83,12 @@ namespace gtsam { Matrix numericalDerivative11(boost::function 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 Matrix numericalDerivative22 (boost::function 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 #include #include #include @@ -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 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; } diff --git a/geometry/Cal3_S2.h b/geometry/Cal3_S2.h index a0f1a14bca..e92657fb2c 100644 --- a/geometry/Cal3_S2.h +++ b/geometry/Cal3_S2.h @@ -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 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)); - } - } diff --git a/geometry/CalibratedCamera.cpp b/geometry/CalibratedCamera.cpp index 951cf49e70..fd72d10846 100644 --- a/geometry/CalibratedCamera.cpp +++ b/geometry/CalibratedCamera.cpp @@ -14,7 +14,7 @@ namespace gtsam { pose_(pose) { } - CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(expmap(v)) {} + CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} CalibratedCamera::~CalibratedCamera() {} diff --git a/geometry/Point2.h b/geometry/Point2.h index 0910c90624..c26c82a6ac 100644 --- a/geometry/Point2.h +++ b/geometry/Point2.h @@ -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 H1=boost::none, + boost::optional 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 H1=boost::none, + boost::optional 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 H1, boost::optional 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 H1, boost::optional 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;} } diff --git a/geometry/Point3.cpp b/geometry/Point3.cpp index 25543c3ec0..2ea2db1682 100644 --- a/geometry/Point3.cpp +++ b/geometry/Point3.cpp @@ -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 H1, boost::optional 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 H1, boost::optional H2) const { if (H1) *H1 = eye(3,3); diff --git a/geometry/Point3.h b/geometry/Point3.h index 98f81a4f33..1fcfa380fd 100644 --- a/geometry/Point3.h +++ b/geometry/Point3.h @@ -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 H1=boost::none, + boost::optional 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 H1, boost::optional 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;} diff --git a/geometry/Pose2.cpp b/geometry/Pose2.cpp index 0888b34018..277c9229ca 100644 --- a/geometry/Pose2.cpp +++ b/geometry/Pose2.cpp @@ -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 H1) const { + if (H1) *H1 = -AdjointMap(); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } - Pose2 inverse(const Pose2& pose, boost::optional 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 H1, - boost::optional H2) { + Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, + boost::optional 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 H1, - boost::optional H2) { + Pose2 Pose2::between(const Pose2& p2, boost::optional H1, + boost::optional 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); diff --git a/geometry/Pose2.h b/geometry/Pose2.h index 31dc6a26a2..27b1e915b3 100644 --- a/geometry/Pose2.h +++ b/geometry/Pose2.h @@ -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 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 H1 = boost::none, + boost::optional 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 H1=boost::none, + boost::optional 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 H1=boost::none, boost::optional H2=boost::none) const; + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; /** * Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + boost::optional H1=boost::none, + boost::optional 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 H1=boost::none, boost::optional H2=boost::none) const; + boost::optional H1=boost::none, + boost::optional 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 H1=boost::none, boost::optional H2=boost::none) const; + boost::optional H1=boost::none, + boost::optional 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 H1); - - /** - * compose this transformation onto another (first p1 and then p2) - */ - Pose2 compose(const Pose2& p1, const Pose2& p2, - boost::optional H1, - boost::optional H2 = boost::none); - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p1, const Pose2& p2, - boost::optional H1, boost::optional H2); - } // namespace gtsam diff --git a/geometry/Pose3.cpp b/geometry/Pose3.cpp index 6e76e0d860..5f550bf58f 100644 --- a/geometry/Pose3.cpp +++ b/geometry/Pose3.cpp @@ -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(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 H1, boost::optional 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 H1, boost::optional H2) const { @@ -187,8 +176,8 @@ namespace gtsam { } /* ************************************************************************* */ - Pose3 compose(const Pose3& p1, const Pose3& p2, - boost::optional H1, boost::optional H2) { + Pose3 Pose3::compose(const Pose3& p2, + boost::optional H1, boost::optional 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 H1) { + Pose3 Pose3::inverse(boost::optional 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 H1, - boost::optional H2) { + Pose3 Pose3::between(const Pose3& p2, boost::optional H1, + boost::optional 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; } diff --git a/geometry/Pose3.h b/geometry/Pose3.h index ded52a7189..ea4d1e8daa 100644 --- a/geometry/Pose3.h +++ b/geometry/Pose3.h @@ -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 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 H1=boost::none, + boost::optional 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 H1=boost::none, + boost::optional 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 H1, boost::optional H2); - /** - * Derivative of inverse - */ - Pose3 inverse(const Pose3& p, boost::optional 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 H1, boost::optional H2); - /** * wedge for Pose3: * @param xi 6-dim twist (omega,v) where diff --git a/geometry/Rot2.h b/geometry/Rot2.h index a16b93c594..c7f121ee24 100644 --- a/geometry/Rot2.h +++ b/geometry/Rot2.h @@ -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; diff --git a/geometry/Rot3.cpp b/geometry/Rot3.cpp index fc78474443..075be911f7 100644 --- a/geometry/Rot3.cpp +++ b/geometry/Rot3.cpp @@ -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 H1, boost::optional H2) { + Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional 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 H1, boost::optional H2) { - if (H1) *H1 = -(R2.transpose()*R1.matrix()); + Rot3 Rot3::between (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; - return between(R1, R2); + return between_default(*this, R2); } /* ************************************************************************* */ diff --git a/geometry/Rot3.h b/geometry/Rot3.h index aca4f05b89..30203638aa 100644 --- a/geometry/Rot3.h +++ b/geometry/Rot3.h @@ -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 H1=boost::none, boost::optional 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 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 H1=boost::none, + boost::optional 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 H1=boost::none, boost::optional 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 H1=boost::none, boost::optional 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 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 H1, boost::optional 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 H1, boost::optional 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' diff --git a/geometry/StereoCamera.cpp b/geometry/StereoCamera.cpp index dc6bc91dc2..beba3f160b 100644 --- a/geometry/StereoCamera.cpp +++ b/geometry/StereoCamera.cpp @@ -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 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) { diff --git a/geometry/StereoCamera.h b/geometry/StereoCamera.h index 6bd60402a1..d473dc76bc 100644 --- a/geometry/StereoCamera.h +++ b/geometry/StereoCamera.h @@ -47,8 +47,15 @@ namespace gtsam { boost::optional Dproject_stereo_pose = boost::none, boost::optional 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(const StereoCamera& p0, const Vector& d) { - return StereoCamera(expmap(p0.pose(),d),p0.K(),p0.baseline()); - } - - } diff --git a/geometry/StereoPoint2.h b/geometry/StereoPoint2.h index 78e298add3..67b2e80324 100644 --- a/geometry/StereoPoint2.h +++ b/geometry/StereoPoint2.h @@ -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); + } }; } diff --git a/geometry/Tensor2.h b/geometry/Tensor2.h index 8dd7d5b088..a5e26f8cc3 100644 --- a/geometry/Tensor2.h +++ b/geometry/Tensor2.h @@ -35,6 +35,9 @@ class Tensor2 { 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); } diff --git a/geometry/tests/testHomography2.cpp b/geometry/tests/testHomography2.cpp index 68fc9f3a58..aac1e47383 100644 --- a/geometry/tests/testHomography2.cpp +++ b/geometry/tests/testHomography2.cpp @@ -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(patchH, gEc); + // FIXME: this doesn't appear to be tested, and requires that Tensor2 be a Lie object +// Matrix D = numericalDerivative11(patchH, gEc); // print(D,"D"); } diff --git a/geometry/tests/testPoint2.cpp b/geometry/tests/testPoint2.cpp index 9dee8f05d8..cc90cefef3 100644 --- a/geometry/tests/testPoint2.cpp +++ b/geometry/tests/testPoint2.cpp @@ -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)); } diff --git a/geometry/tests/testPose2.cpp b/geometry/tests/testPose2.cpp index 871fb5c7e1..6588890d24 100644 --- a/geometry/tests/testPose2.cpp +++ b/geometry/tests/testPose2.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -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(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(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(screw::xi),1e-6)); - CHECK(assert_equal(screw::expected, expmap(screw::xi),1e-6)); - CHECK(assert_equal(screw::xi, logmap(screw::expected),1e-6)); + EXPECT(assert_equal(screw::expected, expm(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(compose, pose1, pose2, 1e-5); - Matrix numericalH2 = numericalDerivative22(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(testing::compose, pose1, pose2, 1e-5); + Matrix numericalH2 = numericalDerivative22(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(compose, pose1, pose2, 1e-5); - Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); - CHECK(assert_equal(numericalH1,actualDcompose1,1e-5)); - CHECK(assert_equal(numericalH2,actualDcompose2)); + Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2, 1e-5); + Matrix numericalH2 = numericalDerivative22(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(compose, pose1, pose2, 1e-5); - Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); - CHECK(assert_equal(numericalH1,actualDcompose1,1e-5)); - CHECK(assert_equal(numericalH2,actualDcompose2)); + Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2, 1e-5); + Matrix numericalH2 = numericalDerivative22(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(inverse, lTg, 1e-5); + Matrix numericalH = numericalDerivative11(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, gT1, gT2, 1e-5); - CHECK(assert_equal(expectedH1,actualH1)); - CHECK(assert_equal(numericalH1,actualH1)); + Matrix numericalH1 = numericalDerivative21(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, gT1, gT2, 1e-5); - CHECK(assert_equal(expectedH2,actualH2)); - CHECK(assert_equal(numericalH2,actualH2)); + Matrix numericalH2 = numericalDerivative22(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, p1, p2, 1e-5); - CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5); - CHECK(assert_equal(numericalH2,actualH2)); + p1.between(p2,actualH1,actualH2); + Matrix numericalH1 = numericalDerivative21(testing::between, p1, p2, 1e-5); + EXPECT(assert_equal(numericalH1,actualH1)); + Matrix numericalH2 = numericalDerivative22(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)); } /* ************************************************************************* */ diff --git a/geometry/tests/testPose3.cpp b/geometry/tests/testPose3.cpp index d26b00b4e6..867f39fc28 100644 --- a/geometry/tests/testPose3.cpp +++ b/geometry/tests/testPose3.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include 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(screw::xi),1e-6)); - CHECK(assert_equal(screw::expected, expmap(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(screw::xi) * inverse(T); + Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); Vector xiprime = Adjoint(T, screw::xi); - CHECK(assert_equal(expected, expmap(xiprime), 1e-6)); + CHECK(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - Pose3 expected2 = T2 * expmap(screw::xi) * inverse(T2); + Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); Vector xiprime2 = Adjoint(T2, screw::xi); - CHECK(assert_equal(expected2, expmap(xiprime2), 1e-6)); + CHECK(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - Pose3 expected3 = T3 * expmap(screw::xi) * inverse(T3); + Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); Vector xiprime3 = Adjoint(T3, screw::xi); - CHECK(assert_equal(expected3, expmap(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 (w), expmap (A * v)); + return Pose3(Rot3::Expmap (w), expmap (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(xi); + actual = Pose3::Expmap(xi); CHECK(assert_equal(expm(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(txi); + actual = Pose3::Expmap(txi); CHECK(assert_equal(expm(txi,30), actual,1e-6)); CHECK(assert_equal(Agrawal06iros(txi), actual,1e-6)); Vector log = logmap(actual); - CHECK(assert_equal(actual, expmap(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(xi); + actual = Pose3::Expmap(xi); CHECK(assert_equal(expm(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(x) * T2; + Pose3 expected = T1 * Pose3::Expmap(x) * T2; Vector y = Adjoint(inverse(T2), x); - Pose3 actual = T1 * T2 * expmap(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(compose, T2, T2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2, 1e-5); CHECK(assert_equal(numericalH1,actualDcompose1,5e-5)); - Matrix numericalH2 = numericalDerivative22(compose, T2, T2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::compose, 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(compose, T1, T2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2, 1e-5); CHECK(assert_equal(numericalH1,actualDcompose1,5e-5)); - Matrix numericalH2 = numericalDerivative22(compose, T1, T2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::compose, 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(inverse, T, 1e-5); + Matrix numericalH = numericalDerivative11(testing::inverse, 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(inverse, T, 1e-5); + Matrix numericalH = numericalDerivative11(testing::inverse, 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(-d),inverse(expmap(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(2*d); - Pose3 T3 = expmap(3*d); - Pose3 T5 = expmap(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 , T2, T3, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3, 1e-5); CHECK(assert_equal(numericalH1,actualDBetween1,5e-5)); - Matrix numericalH2 = numericalDerivative22(between , T2, T3, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3, 1e-5); CHECK(assert_equal(numericalH2,actualDBetween2)); } diff --git a/geometry/tests/testRot2.cpp b/geometry/tests/testRot2.cpp index b5a88d69ce..f728c3a470 100644 --- a/geometry/tests/testRot2.cpp +++ b/geometry/tests/testRot2.cpp @@ -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)); } diff --git a/geometry/tests/testRot3.cpp b/geometry/tests/testRot3.cpp index 41730c2ae4..99e6a73d06 100644 --- a/geometry/tests/testRot3.cpp +++ b/geometry/tests/testRot3.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -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(), 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(), 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()); 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(d12))); - Vector d21 = logmap(gR2, gR1); - CHECK(assert_equal(gR1, expmap(gR2,d21))); - CHECK(assert_equal(gR1, gR2*expmap(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(-d),inverse(expmap(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 (2 * d); - Rot3 R3 = expmap (3 * d); - Rot3 R5 = expmap (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 (w1.vector()), R2 = expmap (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, R, P); + Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); + Matrix numerical3 = numericalDerivative22(testing::rotate, 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, R, w); CHECK(assert_equal(numerical1,H1,error)); - Matrix numerical2 = numericalDerivative22(unrotate_, R, w); + Matrix numerical2 = numericalDerivative22(testing::unrotate, 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 (compose, R1, + Matrix numericalH1 = numericalDerivative21(testing::compose, R1, R2, 1e-5); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22 (compose, R1, + Matrix numericalH2 = numericalDerivative22(testing::compose, 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 (inverse, R, 1e-5); + Matrix numericalH = numericalDerivative11(testing::inverse, 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 , R1, R2, 1e-5); + Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2, 1e-5); CHECK(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(between , R1, R2, 1e-5); + Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2, 1e-5); CHECK(assert_equal(numericalH2,actualH2)); } diff --git a/inference/ISAM2-inl.h b/inference/ISAM2-inl.h index 7fb73b6df4..4d02246fb2 100644 --- a/inference/ISAM2-inl.h +++ b/inference/ISAM2-inl.h @@ -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. diff --git a/inference/ISAM2.h b/inference/ISAM2.h index 569ab5f661..81cbe2565c 100644 --- a/inference/ISAM2.h +++ b/inference/ISAM2.h @@ -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& getMarkedUnsafe() const { return marked_; } diff --git a/inference/graph-inl.h b/inference/graph-inl.h index 3d5476ab70..45b47c5d3a 100644 --- a/inference/graph-inl.h +++ b/inference/graph-inl.h @@ -140,7 +140,7 @@ class compose_key_visitor : public boost::default_bfs_visitor { 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 composePoses(const G& graph, const PredecessorMap namespace gtsam { - - /** Factor Graph Configuration */ - class VectorMap : public Testable { - protected: - /** Map from string indices to values */ - SymbolMap values; +/** Factor Graph Configuration */ +class VectorMap : public Testable { - public: - typedef SymbolMap::iterator iterator; - typedef SymbolMap::const_iterator const_iterator; +protected: + /** Map from string indices to values */ + SymbolMap 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::iterator iterator; + typedef SymbolMap::const_iterator const_iterator; - /** return all the nodes in the graph **/ - std::vector get_names() const; - - /** convert into a single large vector */ - Vector vector() const; - - /** Insert a value into the configuration with a given index */ - VectorMap& insert(const Symbol& name, const Vector& v); + VectorMap() {} + VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {} + VectorMap(const Symbol& j, const Vector& a) { insert(j,a); } - /** Insert or add a value with given index */ - VectorMap& insertAdd(const Symbol& j, const Vector& v); + virtual ~VectorMap() {} - /** Insert a config into another config */ - void insert(const VectorMap& config); - - /** Insert a config into another config, add if key already exists */ - void insertAdd(const VectorMap& config); - - const_iterator begin() const {return values.begin();} - const_iterator end() const {return values.end();} - iterator begin() {return values.begin();} - iterator end() {return values.end();} - - /** Vector access in VectorMap is via a Vector reference */ - Vector& operator[](const Symbol& j); - const Vector& operator[](const Symbol& j) const; - - /** [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];} - - bool contains(const Symbol& name) const { - const_iterator it = values.find(name); - return (it!=values.end()); - } - - /** Nr of vectors */ - size_t size() const { return values.size();} - - /** Total dimensionality */ - size_t dim() const; - - /** max of the vectors */ - inline double max() const { - double m = -std::numeric_limits::infinity(); - for(const_iterator it=begin(); it!=end(); it++) - m = std::max(m, gtsam::max(it->second)); - return m; - } - - /** Scale */ - VectorMap scale(double s) const; - VectorMap operator*(double s) const; - - /** Negation */ - VectorMap operator-() const; - - /** Add in place */ - void operator+=(const VectorMap &b); - - /** Add */ - VectorMap operator+(const VectorMap &b) const; - - /** Subtract */ - VectorMap operator-(const VectorMap &b) const; - - /** print */ - void print(const std::string& name = "") const; - - /** equals, for unit testing */ - bool equals(const VectorMap& expected, double tol=1e-9) const; - - void clear() {values.clear();} - - /** Dot product */ - double dot(const VectorMap& b) const; - - /** Set all vectors to zero */ - VectorMap& zero(); - - /** Create a clone of x with exactly same structure, except with zero values */ - static VectorMap zero(const VectorMap& x); - - /** - * Add a delta config, needed for use in NonlinearOptimizer - * For VectorMap, this is just addition. - */ - friend VectorMap expmap(const VectorMap& original, const VectorMap& delta); - - /** - * 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); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(values); - } - }; // VectorMap - - /** scalar product */ - inline VectorMap operator*(double s, const VectorMap& x) {return x*s;} - - /** Dot product */ - double dot(const VectorMap&, const VectorMap&); - - /** - * BLAS Level 1 scal: x <- alpha*x - */ - void scal(double alpha, VectorMap& x); - - /** - * 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 = ""); + /** return all the nodes in the graph **/ + std::vector get_names() const; + + /** convert into a single large vector */ + Vector vector() const; + + /** Insert a value into the configuration with a given index */ + VectorMap& insert(const Symbol& name, const Vector& v); + + /** Insert or add a value with given index */ + VectorMap& insertAdd(const Symbol& j, const Vector& v); + + /** Insert a config into another config */ + void insert(const VectorMap& config); + + /** Insert a config into another config, add if key already exists */ + void insertAdd(const VectorMap& config); + + const_iterator begin() const {return values.begin();} + const_iterator end() const {return values.end();} + iterator begin() {return values.begin();} + iterator end() {return values.end();} + + /** Vector access in VectorMap is via a Vector reference */ + Vector& operator[](const Symbol& j); + const Vector& operator[](const Symbol& j) const; + + /** [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];} + + bool contains(const Symbol& name) const { + const_iterator it = values.find(name); + return (it!=values.end()); + } + + /** Nr of vectors */ + size_t size() const { return values.size();} + + /** Total dimensionality */ + size_t dim() const; + + /** max of the vectors */ + inline double max() const { + double m = -std::numeric_limits::infinity(); + for(const_iterator it=begin(); it!=end(); it++) + m = std::max(m, gtsam::max(it->second)); + return m; + } + + /** Scale */ + VectorMap scale(double s) const; + VectorMap operator*(double s) const; + + /** Negation */ + VectorMap operator-() const; + + /** Add in place */ + void operator+=(const VectorMap &b); + + /** Add */ + VectorMap operator+(const VectorMap &b) const; + + /** Subtract */ + VectorMap operator-(const VectorMap &b) const; + + /** print */ + void print(const std::string& name = "") const; + + /** equals, for unit testing */ + bool equals(const VectorMap& expected, double tol=1e-9) const; + + void clear() {values.clear();} + + /** Dot product */ + double dot(const VectorMap& b) const; + + /** Set all vectors to zero */ + VectorMap& zero(); + + /** Create a clone of x with exactly same structure, except with zero values */ + static VectorMap zero(const VectorMap& x); + + /** + * Add a delta config, needed for use in NonlinearOptimizer + * For VectorMap, this is just addition. + */ + friend VectorMap expmap(const VectorMap& original, const VectorMap& delta); + + /** + * 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); + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(values); + } +}; // VectorMap + +/** scalar product */ +inline VectorMap operator*(double s, const VectorMap& x) {return x*s;} + +/** Dot product */ +double dot(const VectorMap&, const VectorMap&); + +/** + * BLAS Level 1 scal: x <- alpha*x + */ +void scal(double alpha, VectorMap& x); + +/** + * 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 diff --git a/nonlinear/LieConfig-inl.h b/nonlinear/LieConfig-inl.h index 31c96dcda4..00f5b061f3 100644 --- a/nonlinear/LieConfig-inl.h +++ b/nonlinear/LieConfig-inl.h @@ -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 expmap(const LieConfig&, const VectorConfig&); \ - template LieConfig expmap(const LieConfig&, const Vector&); \ - template VectorConfig logmap(const LieConfig&, const LieConfig&); \ + /*template LieConfig expmap(const LieConfig&, const VectorConfig&);*/ \ + /*template LieConfig expmap(const LieConfig&, const Vector&);*/ \ + /*template VectorConfig logmap(const LieConfig&, const LieConfig&);*/ \ template class LieConfig; using namespace std; namespace gtsam { +/* ************************************************************************* */ template void LieConfig::print(const string &s) const { cout << "LieConfig " << s << ", size " << values_.size() << "\n"; @@ -38,6 +37,7 @@ namespace gtsam { } } + /* ************************************************************************* */ template bool LieConfig::equals(const LieConfig& expected, double tol) const { if (values_.size() != expected.values_.size()) return false; @@ -49,6 +49,7 @@ namespace gtsam { return true; } + /* ************************************************************************* */ template const typename J::Value_t& LieConfig::at(const J& j) const { const_iterator it = values_.find(j); @@ -56,33 +57,38 @@ namespace gtsam { else return it->second; } + /* ************************************************************************* */ template size_t LieConfig::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 VectorConfig LieConfig::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 void LieConfig::insert(const J& name, const typename J::Value_t& val) { values_.insert(make_pair(name, val)); } + /* ************************************************************************* */ template void LieConfig::insert(const LieConfig& cfg) { BOOST_FOREACH(const typename Values::value_type& v, cfg.values_) insert(v.first, v.second); } + /* ************************************************************************* */ template void LieConfig::update(const LieConfig& cfg) { BOOST_FOREACH(const typename Values::value_type& v, values_) { @@ -91,11 +97,13 @@ namespace gtsam { } } + /* ************************************************************************* */ template void LieConfig::update(const J& j, const typename J::Value_t& val) { values_[j] = val; } + /* ************************************************************************* */ template std::list LieConfig::keys() const { std::list ret; @@ -104,68 +112,72 @@ namespace gtsam { return ret; } + /* ************************************************************************* */ template void LieConfig::erase(const J& j) { size_t dim; // unused erase(j, dim); } + /* ************************************************************************* */ template void LieConfig::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 - LieConfig expmap(const LieConfig& c, const VectorConfig& delta) { + LieConfig LieConfig::expmap(const VectorConfig& delta) const { LieConfig newConfig; typedef pair 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 - LieConfig expmap(const LieConfig& c, const Vector& delta) { - if(delta.size() != dim(c)) { - cout << "LieConfig::dim (" << dim(c) << ") <> delta.size (" << delta.size() << ")" << endl; + LieConfig LieConfig::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 newConfig; int delta_offset = 0; typedef pair 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 - VectorConfig logmap(const LieConfig& c0, const LieConfig& cp) { + VectorConfig LieConfig::logmap(const LieConfig& cp) const { VectorConfig delta; typedef pair 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; } diff --git a/nonlinear/LieConfig.h b/nonlinear/LieConfig.h index d428c0ddb7..79679c93a3 100644 --- a/nonlinear/LieConfig.h +++ b/nonlinear/LieConfig.h @@ -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 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 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 - inline size_t dim(const LieConfig& c) { return c.dim(); } - - /** Add a delta config */ - template - LieConfig expmap(const LieConfig& c, const VectorConfig& delta); - - /** Add a delta vector, uses iterator order */ - template - LieConfig expmap(const LieConfig& c, const Vector& delta); - - /** Get a delta config about a linearization point c0 */ - template - VectorConfig logmap(const LieConfig& c0, const LieConfig& cp); - } diff --git a/nonlinear/NonlinearConstraint.h b/nonlinear/NonlinearConstraint.h index 242699153b..0c59ebe255 100644 --- a/nonlinear/NonlinearConstraint.h +++ b/nonlinear/NonlinearConstraint.h @@ -434,7 +434,7 @@ class NonlinearEquality1 : public NonlinearEqualityConstraint1 { Vector evaluateError(const X& x1, boost::optional 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 @@ class NonlinearEquality2 : public NonlinearEqualityConstraint2 const size_t p = X::Dim(); if (H1) *H1 = -eye(p); if (H2) *H2 = eye(p); - return logmap(x1, x2); + return x1.logmap(x2); } }; diff --git a/nonlinear/NonlinearEquality.h b/nonlinear/NonlinearEquality.h index fac5385f91..bfa6813318 100644 --- a/nonlinear/NonlinearEquality.h +++ b/nonlinear/NonlinearEquality.h @@ -60,7 +60,7 @@ namespace gtsam { * Constructor - forces exact evaluation */ NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare) : - 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::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) : - 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 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 diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index 8a66722f52..60ab68c7da 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -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"); diff --git a/nonlinear/TupleConfig.h b/nonlinear/TupleConfig.h index 54baa58e44..2d98c69388 100644 --- a/nonlinear/TupleConfig.h +++ b/nonlinear/TupleConfig.h @@ -173,12 +173,12 @@ namespace gtsam { /** Expmap */ TupleConfig 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& 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 expmap(const VectorConfig& delta) const { - return TupleConfigEnd(gtsam::expmap(first_, delta)); + return TupleConfigEnd(first_.expmap(delta)); } VectorConfig logmap(const TupleConfigEnd& 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 - inline TupleConfig expmap(const TupleConfig& c, const VectorConfig& delta) { - return c.expmap(delta); - } - - /** logmap static functions */ - template - inline VectorConfig logmap(const TupleConfig& c0, const TupleConfig& 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 - TupleConfig1 expmap(const TupleConfig1& c, const VectorConfig& delta) { - return c.expmap(delta); - } - - template - VectorConfig logmap(const TupleConfig1& c1, const TupleConfig1& c2) { - return c1.logmap(c2); - } - template class TupleConfig2 : public TupleConfig > { public: @@ -351,16 +329,6 @@ namespace gtsam { inline const Config2& second() const { return this->rest().config(); } }; - template - TupleConfig2 expmap(const TupleConfig2& c, const VectorConfig& delta) { - return c.expmap(delta); - } - - template - VectorConfig logmap(const TupleConfig2& c1, const TupleConfig2& c2) { - return c1.logmap(c2); - } - template class TupleConfig3 : public TupleConfig > > { public: @@ -380,11 +348,6 @@ namespace gtsam { inline const Config3& third() const { return this->rest().rest().config(); } }; - template - TupleConfig3 expmap(const TupleConfig3& c, const VectorConfig& delta) { - return c.expmap(delta); - } - template class TupleConfig4 : public TupleConfig > > > { public: @@ -409,11 +372,6 @@ namespace gtsam { inline const Config4& fourth() const { return this->rest().rest().rest().config(); } }; - template - TupleConfig4 expmap(const TupleConfig4& c, const VectorConfig& delta) { - return c.expmap(delta); - } - template class TupleConfig5 : public TupleConfig > > > > { public: @@ -438,11 +396,6 @@ namespace gtsam { inline const Config5& fifth() const { return this->rest().rest().rest().rest().config(); } }; - template - TupleConfig5 expmap(const TupleConfig5& c, const VectorConfig& delta) { - return c.expmap(delta); - } - template class TupleConfig6 : public TupleConfig > > > > > { public: @@ -468,9 +421,4 @@ namespace gtsam { inline const Config6& sixth() const { return this->rest().rest().rest().rest().rest().config(); } }; - template - TupleConfig6 expmap(const TupleConfig6& c, const VectorConfig& delta) { - return c.expmap(delta); - } - } diff --git a/nonlinear/tests/testLieConfig.cpp b/nonlinear/tests/testLieConfig.cpp index f512ebd3fa..434d892ff9 100644 --- a/nonlinear/tests/testLieConfig.cpp +++ b/nonlinear/tests/testLieConfig.cpp @@ -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))); } /* ************************************************************************* */ diff --git a/slam/BearingFactor.h b/slam/BearingFactor.h index 539fbf676b..838789f89d 100644 --- a/slam/BearingFactor.h +++ b/slam/BearingFactor.h @@ -35,7 +35,7 @@ namespace gtsam { Vector evaluateError(const Pose2& pose, const Point2& point, boost::optional H1, boost::optional H2) const { Rot2 hx = pose.bearing(point, H1, H2); - return logmap(between(z_, hx)); + return Rot2::Logmap(z_.between(hx)); } /** return the measured */ diff --git a/slam/BearingRangeFactor.h b/slam/BearingRangeFactor.h index aa60be5d6b..64842ba931 100644 --- a/slam/BearingRangeFactor.h +++ b/slam/BearingRangeFactor.h @@ -44,7 +44,7 @@ namespace gtsam { boost::optional H22_ = H2 ? boost::optional(H22) : boost::optional(); 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_); diff --git a/slam/BetweenConstraint.h b/slam/BetweenConstraint.h index 9beb91a858..540d7d6226 100644 --- a/slam/BetweenConstraint.h +++ b/slam/BetweenConstraint.h @@ -35,8 +35,8 @@ namespace gtsam { Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional 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 { diff --git a/slam/BetweenFactor.h b/slam/BetweenFactor.h index 5c7e66ca46..1d57a635e4 100644 --- a/slam/BetweenFactor.h +++ b/slam/BetweenFactor.h @@ -63,9 +63,9 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional 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 */ diff --git a/slam/LinearApproxFactor.h b/slam/LinearApproxFactor.h index 021cc34ea8..5786e5e374 100644 --- a/slam/LinearApproxFactor.h +++ b/slam/LinearApproxFactor.h @@ -70,7 +70,7 @@ class LinearApproxFactor : public NonlinearFactor { 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); } diff --git a/slam/Pose2SLAMOptimizer.cpp b/slam/Pose2SLAMOptimizer.cpp index 2a66720254..bad299ab94 100644 --- a/slam/Pose2SLAMOptimizer.cpp +++ b/slam/Pose2SLAMOptimizer.cpp @@ -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(); } diff --git a/slam/PriorFactor.h b/slam/PriorFactor.h index 7238055eae..fd9dbce8c6 100644 --- a/slam/PriorFactor.h +++ b/slam/PriorFactor.h @@ -63,9 +63,9 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional 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); } }; diff --git a/slam/TransformConstraint.h b/slam/TransformConstraint.h index 77548c9b30..101fef5e4e 100644 --- a/slam/TransformConstraint.h +++ b/slam/TransformConstraint.h @@ -54,7 +54,7 @@ class TransformConstraint : public NonlinearEqualityConstraint3(local, newlocal)); + return Point::Logmap(local.between(newlocal)); } }; } // \namespace gtsam diff --git a/slam/dataset.cpp b/slam/dataset.cpp index 763410cbd7..8f0de954cf 100644 --- a/slam/dataset.cpp +++ b/slam/dataset.cpp @@ -132,7 +132,7 @@ pair 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 factor = boost::dynamic_pointer_cast(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) diff --git a/slam/simulated2DOriented.cpp b/slam/simulated2DOriented.cpp index a840f8bd33..73fe29a3c6 100644 --- a/slam/simulated2DOriented.cpp +++ b/slam/simulated2DOriented.cpp @@ -27,7 +27,7 @@ namespace gtsam { /* ************************************************************************* */ Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1, boost::optional H2) { - return between(x1, x2, H1, H2); + return x1.between(x2, H1, H2); } /* ************************************************************************* */ diff --git a/slam/simulated2DOriented.h b/slam/simulated2DOriented.h index 28c4969e78..3e63e5912c 100644 --- a/slam/simulated2DOriented.h +++ b/slam/simulated2DOriented.h @@ -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 H1 = boost::none, boost::optional H2 = boost::none); @@ -58,7 +58,7 @@ namespace gtsam { Vector evaluateError(const Pose2& x, boost::optional 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 H2 = boost::none) const { - return logmap(z_, odo(x1, x2, H1, H2)); + return z_.logmap(odo(x1, x2, H1, H2)); } }; diff --git a/slam/tests/testPose2Config.cpp b/slam/tests/testPose2Config.cpp index 25bc2ef38c..61e112798c 100644 --- a/slam/tests/testPose2Config.cpp +++ b/slam/tests/testPose2Config.cpp @@ -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)); } diff --git a/slam/tests/testPose2Factor.cpp b/slam/tests/testPose2Factor.cpp index 0730e5f52d..ac06576388 100644 --- a/slam/tests/testPose2Factor.cpp +++ b/slam/tests/testPose2Factor.cpp @@ -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 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))); diff --git a/slam/tests/testPose2Prior.cpp b/slam/tests/testPose2Prior.cpp index 4260e4b98b..32810714ca 100644 --- a/slam/tests/testPose2Prior.cpp +++ b/slam/tests/testPose2Prior.cpp @@ -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))); diff --git a/slam/tests/testPose2SLAM.cpp b/slam/tests/testPose2SLAM.cpp index 460d9c65ce..c6686e3ebc 100644 --- a/slam/tests/testPose2SLAM.cpp +++ b/slam/tests/testPose2SLAM.cpp @@ -126,7 +126,7 @@ TEST(Pose2Graph, optimizeThreePoses) { // create a Pose graph with one equality constraint and one measurement shared_ptr 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 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(new Ordering); @@ -164,7 +164,7 @@ TEST(Pose2Graph, optimizeCircle) { // create a Pose graph with one equality constraint and one measurement shared_ptr 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 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(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"); diff --git a/slam/tests/testPose3Config.cpp b/slam/tests/testPose3Config.cpp index 43a509d292..65da1be5c9 100644 --- a/slam/tests/testPose3Config.cpp +++ b/slam/tests/testPose3Config.cpp @@ -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)); } diff --git a/slam/tests/testPose3Factor.cpp b/slam/tests/testPose3Factor.cpp index 4ed208775e..c95ce295dc 100644 --- a/slam/tests/testPose3Factor.cpp +++ b/slam/tests/testPose3Factor.cpp @@ -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)); } diff --git a/slam/tests/testPose3SLAM.cpp b/slam/tests/testPose3SLAM.cpp index e4c388b5a5..94c3464334 100644 --- a/slam/tests/testPose3SLAM.cpp +++ b/slam/tests/testPose3SLAM.cpp @@ -38,7 +38,7 @@ TEST(Pose3Graph, optimizeCircle) { // create a Pose graph with one equality constraint and one measurement shared_ptr 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 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(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)); } /* ************************************************************************* */ diff --git a/slam/tests/testVSLAMConfig.cpp b/slam/tests/testVSLAMConfig.cpp index 087bce41e0..16d8c4d616 100644 --- a/slam/tests/testVSLAMConfig.cpp +++ b/slam/tests/testVSLAMConfig.cpp @@ -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)); } diff --git a/slam/tests/testVSLAMFactor.cpp b/slam/tests/testVSLAMFactor.cpp index 86fc4318cf..0b180f86db 100644 --- a/slam/tests/testVSLAMFactor.cpp +++ b/slam/tests/testVSLAMFactor.cpp @@ -36,29 +36,29 @@ TEST( ProjectionFactor, error ) Point2 z(323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - 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))); - - // 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)); - - // linearize graph - Graph graph; + 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))); + + // 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)); + + // 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)); } /* ************************************************************************* */ diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 8260cc7ad6..c99fff1e33 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -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); diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp index a9bea9a1fc..7740eb02fb 100644 --- a/tests/testNonlinearEqualityConstraint.cpp +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -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); diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp index 621c4f6caf..0d0a88ad57 100644 --- a/tests/testTransformConstraint.cpp +++ b/tests/testTransformConstraint.cpp @@ -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 diff --git a/tests/testTupleConfig.cpp b/tests/testTupleConfig.cpp index ceeb0e33e6..ebbeb9e266 100644 --- a/tests/testTupleConfig.cpp +++ b/tests/testTupleConfig.cpp @@ -16,7 +16,6 @@ #include #include -#include #include 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 LamKey; +typedef TypedSymbol LamKey; typedef TypedSymbol Pose3Key; typedef TypedSymbol Point3Key; typedef TypedSymbol 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(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);