diff --git a/base/LieVector.h b/base/LieVector.h index f9eb95427..9b981036b 100644 --- a/base/LieVector.h +++ b/base/LieVector.h @@ -6,22 +6,69 @@ #pragma once +#include #include +#include namespace gtsam { -/** temporarily using the old system */ -// The rest of the file makes double and Vector behave as a Lie type (with + as compose) + /** + * LieVector is a wrapper around vector to allow it to be a Lie type + */ + struct LieVector : public Vector, public Lie, Testable { -// Vector group operations -inline Vector compose(const Vector& p1,const Vector& p2) { return p1+p2;} -inline Vector inverse(const Vector& p) { return -p;} -inline Vector between(const Vector& p1,const Vector& p2) { return p2-p1;} + /** default constructor - should be unnecessary */ + LieVector() {} -// Vector is a trivial Lie group -template<> inline Vector expmap(const Vector& d) { return d;} -template<> inline Vector expmap(const Vector& p,const Vector& d) { return p+d;} -template<> inline Vector logmap(const Vector& p) { return p;} -template<> inline Vector logmap(const Vector& p1,const Vector& p2) { return p2-p1;} + /** initialize from a normal vector */ + LieVector(const Vector& v) : Vector(v) {} + /** get the underlying vector */ + inline Vector vector() const { + return static_cast(*this); + } + + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + gtsam::print(vector(), name); + } + + /** equality up to tolerance */ + inline bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } + + /** + * Returns dimensionality of the tangent space + */ + inline size_t dim() const { return this->size(); } + + /** + * Returns Exponential map update of T + * Default implementation calls global binary function + */ + LieVector expmap(const Vector& v) const { return LieVector(vector() + v); } + + /** expmap around identity */ + static 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; } + + /** Logmap around identity - just returns with default cast back */ + static Vector Logmap(const LieVector& p) { return p; } + + /** compose with another object */ + inline LieVector compose(const LieVector& p) const { + return LieVector(vector() + p); + } + + /** invert the object and yield a new one */ + inline LieVector inverse() const { + return LieVector(-1.0 * vector()); + } + }; } // \namespace gtsam diff --git a/base/Makefile.am b/base/Makefile.am index f4e56ff6f..a40f49997 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -27,6 +27,7 @@ headers += Testable.h TestableAssertions.h numericalDerivative.h # Lie Groups headers += Lie.h Lie-inl.h LieVector.h +check_PROGRAMS += tests/testLieVector # Data structures headers += BTree.h DSF.h diff --git a/base/numericalDerivative.h b/base/numericalDerivative.h index 53fb41d8b..98a383631 100644 --- a/base/numericalDerivative.h +++ b/base/numericalDerivative.h @@ -12,6 +12,7 @@ #include #include +#include #include //#define LINEARIZE_AT_IDENTITY @@ -40,6 +41,11 @@ namespace gtsam { * http://www.boost.org/doc/libs/1_43_0/libs/bind/bind.html */ + + /** global functions for converting to a LieVector for use with numericalDerivative */ + LieVector makeLieVector(const Vector& v) { return LieVector(v); } + LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); } + /** * Numerically compute gradient of scalar function * Class X is the input argument @@ -89,31 +95,32 @@ namespace gtsam { return H; } + /** use a raw C++ function pointer */ template Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { return numericalDerivative11(boost::bind(h, _1), x, delta); } - /** pseudo-template specialization for double Y values */ + /** remapping for double valued functions */ template Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - double hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = 1, n = dim(x); - Vector d(n,0.0); - Matrix H = zeros(m,n); - for (size_t j=0;j(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); } template Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); + return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); + } + + /** remapping for vector valued functions */ + template + Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); + } + + template + Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { + return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); } /** @@ -142,6 +149,7 @@ namespace gtsam { return H; } + /** use a raw C++ function pointer */ template inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { @@ -152,24 +160,30 @@ namespace gtsam { template Matrix numericalDerivative21(boost::function h, const X1& x1, const X2& x2, double delta=1e-5) { - double hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = 1, n = dim(x1); - Vector d(n,0.0); - Matrix H = zeros(m,n); - for (size_t j=0;j( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); } template - inline Matrix numericalDerivative21(double (*h)(const X1&, const X2&), + Matrix numericalDerivative21(double (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); + return numericalDerivative21( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } + + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative21(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } + + template + inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative21( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); } /** @@ -199,33 +213,42 @@ namespace gtsam { } return H; } + + /** use a raw C++ function pointer */ template inline Matrix numericalDerivative22 (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); } - /** pseudo-specialization for double Y values */ + /** pseudo-partial template specialization for double return values */ template Matrix numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta=1e-5) { - double hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = 1, n = dim(x2); - Vector d(n,0.0); - Matrix H = zeros(m,n); - for (size_t j=0;j( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); } + template - inline Matrix numericalDerivative22 (double (*h)(const X1&, const X2&), + inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); + return numericalDerivative22( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); + } + + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); + } + + template + inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta=1e-5) { + return numericalDerivative22( + boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); } /** @@ -263,7 +286,46 @@ namespace gtsam { return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } - // arg 2 + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative31(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + template + inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative31(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + template + inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative31( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + /** + * Compute numerical derivative in argument 2 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ template Matrix numericalDerivative32 (boost::function h, @@ -289,7 +351,46 @@ namespace gtsam { return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } - // arg 3 + /** pseudo-partial template specialization for double return values */ + template + Matrix numericalDerivative32(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + template + inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + /** pseudo-partial template specialization for vector return values */ + template + Matrix numericalDerivative32(boost::function h, + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + template + inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { + return numericalDerivative32( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); + } + + /** + * Compute numerical derivative in argument 3 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ template Matrix numericalDerivative33 (boost::function h, @@ -315,82 +416,34 @@ namespace gtsam { return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); } - - /** - * specializations for double outputs - */ + /** pseudo-partial template specialization for double return values */ template - Matrix numericalDerivative31 - (boost::function h, + Matrix numericalDerivative33(boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - double hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = 1, n = dim(x1); - Vector d(n,0.0); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative31 - (double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + return numericalDerivative33( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); } - // arg 2 template - Matrix numericalDerivative32 - (boost::function h, + inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - double hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = 1, n = dim(x2); - Vector d(n,0.0); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative32 - (double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + return numericalDerivative33( + boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); } - // arg 3 + /** pseudo-partial template specialization for vector return values */ template - Matrix numericalDerivative33 - (boost::function h, + Matrix numericalDerivative33(boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - double hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = 1, n = dim(x3); - Vector d(n,0.0); - Matrix H = zeros(m,n); - for (size_t j=0;j( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); } + template - inline Matrix numericalDerivative33 - (double (*h)(const X1&, const X2&, const X3&), + inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); + return numericalDerivative33( + boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); } } diff --git a/base/tests/testLieVector.cpp b/base/tests/testLieVector.cpp new file mode 100644 index 000000000..00e0c3c46 --- /dev/null +++ b/base/tests/testLieVector.cpp @@ -0,0 +1,26 @@ +/** + * @file testLieVector.cpp + * @author Alex Cunningham + */ + +#include + +#include + +using namespace gtsam; + +/* ************************************************************************* */ +TEST( testLieVector, construction ) { + Vector v = Vector_(3, 1.0, 2.0, 3.0); + LieVector lie1(v), lie2(v); + + EXPECT(lie1.dim() == 3); + EXPECT(assert_equal(v, lie1.vector())); + EXPECT(assert_equal(lie1, lie2)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/geometry/tests/testPose2.cpp b/geometry/tests/testPose2.cpp index 51bf2f60d..871fb5c7e 100644 --- a/geometry/tests/testPose2.cpp +++ b/geometry/tests/testPose2.cpp @@ -439,8 +439,8 @@ TEST( Pose2, bearing ) } /* ************************************************************************* */ -double range_proxy(const Pose2& pose, const Point2& point) { - return pose.range(point); +LieVector range_proxy(const Pose2& pose, const Point2& point) { + return LieVector(Vector_(1, pose.range(point))); } TEST( Pose2, range ) { diff --git a/nonlinear/tests/testLieConfig.cpp b/nonlinear/tests/testLieConfig.cpp index 3b17c1f46..f512ebd3f 100644 --- a/nonlinear/tests/testLieConfig.cpp +++ b/nonlinear/tests/testLieConfig.cpp @@ -18,7 +18,7 @@ using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); -typedef TypedSymbol VecKey; +typedef TypedSymbol VecKey; typedef LieConfig Config; VecKey key1(1), key2(2), key3(3), key4(4); @@ -211,7 +211,7 @@ TEST(LieConfig, exists_) config0.insert(key1, Vector_(1, 1.)); config0.insert(key2, Vector_(1, 2.)); - boost::optional v = config0.exists_(key1); + boost::optional v = config0.exists_(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); } @@ -237,7 +237,7 @@ TEST(LieConfig, update) /* ************************************************************************* */ TEST(LieConfig, dummy_initialization) { - typedef TypedSymbol Key; + typedef TypedSymbol Key; typedef LieConfig Config1; Config1 init1; diff --git a/slam/Simulated3D.cpp b/slam/Simulated3D.cpp index 3123a0435..8db46593d 100644 --- a/slam/Simulated3D.cpp +++ b/slam/Simulated3D.cpp @@ -5,54 +5,35 @@ **/ #include +#include +#include namespace gtsam { + +using namespace simulated3D; +INSTANTIATE_LIE_CONFIG(PointKey) +INSTANTIATE_LIE_CONFIG(PoseKey) +INSTANTIATE_TUPLE_CONFIG2(PoseConfig, PointConfig) + namespace simulated3D { -Vector prior (const Vector& x) -{ +Point3 prior (const Point3& x, boost::optional H) { + if (H) *H = eye(3); return x; } -Matrix Dprior(const Vector& x) -{ - Matrix H = eye((int) x.size()); - return H; -} - -Vector odo(const Vector& x1, const Vector& x2) -{ +Point3 odo(const Point3& x1, const Point3& x2, + boost::optional H1, boost::optional H2) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); return x2 - x1; } -Matrix Dodo1(const Vector& x1, const Vector& x2) -{ - Matrix H = -1 * eye((int) x1.size()); - return H; -} - -Matrix Dodo2(const Vector& x1, const Vector& x2) -{ - Matrix H = eye((int) x1.size()); - return H; -} - - -Vector mea(const Vector& x, const Vector& l) -{ +Point3 mea(const Point3& x, const Point3& l, + boost::optional H1, boost::optional H2) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); return l - x; } -Matrix Dmea1(const Vector& x, const Vector& l) -{ - Matrix H = -1 * eye((int) x.size()); - return H; -} - -Matrix Dmea2(const Vector& x, const Vector& l) -{ - Matrix H = eye((int) x.size()); - return H; -} - }} // namespace gtsam::simulated3D diff --git a/slam/Simulated3D.h b/slam/Simulated3D.h index c5f504252..791c47e8a 100644 --- a/slam/Simulated3D.h +++ b/slam/Simulated3D.h @@ -9,73 +9,72 @@ #pragma once #include -#include +#include #include #include #include +#include // \namespace namespace gtsam { namespace simulated3D { - typedef VectorConfig VectorConfig; + typedef gtsam::TypedSymbol PoseKey; + typedef gtsam::TypedSymbol PointKey; - typedef gtsam::TypedSymbol PoseKey; - typedef gtsam::TypedSymbol PointKey; + typedef LieConfig PoseConfig; + typedef LieConfig PointConfig; + typedef TupleConfig2 Config; /** * Prior on a single pose */ - Vector prior(const Vector& x); - Matrix Dprior(const Vector& x); + Point3 prior(const Point3& x, boost::optional H = boost::none); /** * odometry between two poses */ - Vector odo(const Vector& x1, const Vector& x2); - Matrix Dodo1(const Vector& x1, const Vector& x2); - Matrix Dodo2(const Vector& x1, const Vector& x2); + Point3 odo(const Point3& x1, const Point3& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none); /** * measurement between landmark and pose */ - Vector mea(const Vector& x, const Vector& l); - Matrix Dmea1(const Vector& x, const Vector& l); - Matrix Dmea2(const Vector& x, const Vector& l); + Point3 mea(const Point3& x, const Point3& l, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none); - struct Point2Prior3D: public NonlinearFactor1 { + struct PointPrior3D: public NonlinearFactor1 { - Vector z_; + Point3 z_; - Point2Prior3D(const Vector& z, + PointPrior3D(const Point3& z, const SharedGaussian& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + NonlinearFactor1 (model, j), z_(z) { } - Vector evaluateError(const Vector& x, boost::optional H = + Vector evaluateError(const Point3& x, boost::optional H = boost::none) { - if (H) *H = Dprior(x); - return prior(x) - z_; + return (prior(x, H) - z_).vector(); } }; - struct Simulated3DMeasurement: public NonlinearFactor2 { - Vector z_; + Point3 z_; - Simulated3DMeasurement(const Vector& z, + Simulated3DMeasurement(const Point3& z, const SharedGaussian& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( + NonlinearFactor2 ( model, j1, j2), z_(z) { } - Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< + Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = Dmea1(x1, x2); - if (H2) *H2 = Dmea2(x1, x2); - return mea(x1, x2) - z_; + return (mea(x1, x2, H1, H2) - z_).vector(); } }; diff --git a/slam/TransformConstraint.h b/slam/TransformConstraint.h index 975714698..77548c9b3 100644 --- a/slam/TransformConstraint.h +++ b/slam/TransformConstraint.h @@ -49,21 +49,12 @@ public: boost::optional Dforeign = boost::none, boost::optional Dtrans = boost::none, boost::optional Dlocal = boost::none) const { - if (Dforeign) { - Matrix Af; - trans.transform_from(foreign, boost::none, Af); - *Dforeign = Af; - } - if (Dtrans) { - Matrix At; - trans.transform_from(foreign, At, boost::none); - *Dtrans = At; - } + Point newlocal = trans.transform_from(foreign, Dtrans, Dforeign); if (Dlocal) { Point dummy; *Dlocal = -1* eye(dummy.dim()); } - return gtsam::logmap(gtsam::between(local, trans.transform_from(foreign))); + return gtsam::logmap(gtsam::between(local, newlocal)); } }; } // \namespace gtsam diff --git a/slam/tests/testPose2Factor.cpp b/slam/tests/testPose2Factor.cpp index dd0429b8b..0730e5f52 100644 --- a/slam/tests/testPose2Factor.cpp +++ b/slam/tests/testPose2Factor.cpp @@ -8,6 +8,7 @@ #define GTSAM_MAGIC_KEY +#include #include #include @@ -87,8 +88,8 @@ TEST( Pose2Factor, rhs ) /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // Hence i.e., b = approximates z-h(x0) = error_vector(x0) -Vector h(const Pose2& p1,const Pose2& p2) { - return covariance->whiten(factor.evaluateError(p1,p2)); +LieVector h(const Pose2& p1,const Pose2& p2) { + return LieVector(covariance->whiten(factor.evaluateError(p1,p2))); } /* ************************************************************************* */ diff --git a/slam/tests/testPose2Prior.cpp b/slam/tests/testPose2Prior.cpp index 8309a5f9c..4260e4b98 100644 --- a/slam/tests/testPose2Prior.cpp +++ b/slam/tests/testPose2Prior.cpp @@ -8,6 +8,7 @@ #define GTSAM_MAGIC_KEY +#include #include #include @@ -56,8 +57,8 @@ static Pose2Prior factor(1,prior, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // Hence i.e., b = approximates z-h(x0) = error_vector(x0) -Vector h(const Pose2& p1) { - return sigmas->whiten(factor.evaluateError(p1)); +LieVector h(const Pose2& p1) { + return LieVector(sigmas->whiten(factor.evaluateError(p1))); } /* ************************************************************************* */ diff --git a/slam/tests/testSimulated3D.cpp b/slam/tests/testSimulated3D.cpp index f18e5340d..16c642ac4 100644 --- a/slam/tests/testSimulated3D.cpp +++ b/slam/tests/testSimulated3D.cpp @@ -14,38 +14,35 @@ using namespace gtsam; using namespace simulated3D; +/* ************************************************************************* */ +TEST( simulated3D, Config ) +{ + Config actual; + actual.insert(PointKey(1),Point3(1,1,1)); + actual.insert(PoseKey(2),Point3(2,2,2)); + EXPECT(assert_equal(actual,actual,1e-9)); +} + /* ************************************************************************* */ TEST( simulated3D, Dprior ) { - Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0)); - Vector v = logmap(x1); - Matrix numerical = numericalDerivative11(prior,v); - Matrix computed = Dprior(v); - CHECK(assert_equal(numerical,computed,1e-9)); + Point3 x(1,-9, 7); + Matrix numerical = numericalDerivative11(boost::bind(prior, _1, boost::none),x); + Matrix computed; + prior(x,computed); + EXPECT(assert_equal(numerical,computed,1e-9)); } /* ************************************************************************* */ -TEST( simulated3D, DOdo1 ) +TEST( simulated3D, DOdo ) { - Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0)); - Vector v1 = logmap(x1); - Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0)); - Vector v2 = logmap(x2); - Matrix numerical = numericalDerivative21(odo,v1,v2); - Matrix computed = Dodo1(v1,v2); - CHECK(assert_equal(numerical,computed,1e-9)); -} - -/* ************************************************************************* */ -TEST( simulated3D, DOdo2 ) -{ - Pose3 x1(Rot3::rodriguez(0, 0, 1.57), Point3(1, 5, 0)); - Vector v1 = logmap(x1); - Pose3 x2(Rot3::rodriguez(0, 0, 0), Point3(2, 3, 0)); - Vector v2 = logmap(x2); - Matrix numerical = numericalDerivative22(odo,v1,v2); - Matrix computed = Dodo2(v1,v2); - CHECK(assert_equal(numerical,computed,1e-9)); + Point3 x1(1,-9,7),x2(-5,6,7); + Matrix H1,H2; + odo(x1,x2,H1,H2); + Matrix A1 = numericalDerivative21(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2); + EXPECT(assert_equal(A1,H1,1e-9)); + Matrix A2 = numericalDerivative22(boost::bind(odo, _1, _2, boost::none, boost::none),x1,x2); + EXPECT(assert_equal(A2,H2,1e-9)); } diff --git a/tests/Makefile.am b/tests/Makefile.am index 17070bcb4..393db81e5 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -18,7 +18,7 @@ check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint check_PROGRAMS += testTransformConstraint testLinearApproxFactor # experimental -#check_PROGRAMS += testFusionTupleConfig +#check_PROGRAMS += testFusionTupleConfig # Doesn't work on Macs if USE_LDL check_PROGRAMS += testConstraintOptimizer diff --git a/tests/testTransformConstraint.cpp b/tests/testTransformConstraint.cpp index 03d5bc4e4..621c4f6ca 100644 --- a/tests/testTransformConstraint.cpp +++ b/tests/testTransformConstraint.cpp @@ -62,6 +62,10 @@ TEST( TransformConstraint, equals ) { } /* ************************************************************************* */ +LieVector evaluateError_(const PointTransformConstraint& c, + const Point2& global, const Pose2& trans, const Point2& local) { + return LieVector(c.evaluateError(global, trans, local)); +} TEST( TransformConstraint, jacobians ) { // from examples below @@ -73,14 +77,14 @@ TEST( TransformConstraint, jacobians ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( - boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + numericalDF = numericalDerivative31( + boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( - boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + numericalDT = numericalDerivative32( + boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( - boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + numericalDL = numericalDerivative33( + boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); CHECK(assert_equal(numericalDF, actualDF)); @@ -105,14 +109,14 @@ TEST( TransformConstraint, jacobians_zero ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( - boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + numericalDF = numericalDerivative31( + boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( - boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + numericalDT = numericalDerivative32( + boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( - boost::bind(&PointTransformConstraint::evaluateError, ref(tc), _1, _2, _3, boost::none, boost::none, boost::none), + numericalDL = numericalDerivative33( + boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); CHECK(assert_equal(numericalDF, actualDF));