From 6ead19648fa06c5e702577003af9ca69ef2ba37f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 8 Dec 2009 14:02:56 +0000 Subject: [PATCH] Modernized Pose2 constraint and added numerical derivative check --- cpp/Pose2.cpp | 15 +++++++++++ cpp/Pose2.h | 30 ++++++++++++++++++--- cpp/testNonlinearFactor.cpp | 53 +++++++++++++++++++++---------------- cpp/testPose2.cpp | 14 ++++++++++ 4 files changed, 85 insertions(+), 27 deletions(-) diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index be74751d5..e1393f5b1 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -25,6 +25,21 @@ namespace gtsam { return Pose2(x_ + v(0), y_ + v(1), theta_ + v(2)); } + /* ************************************************************************* */ + Vector Pose2::vector() const { + Vector v(3); + v(0) = x_; + v(1) = y_; + v(2) = theta_; + return v; + } + + /* ************************************************************************* */ + Pose2 Pose2::rotate(double theta) const { + double c = cos(theta), s = sin(theta); + return Pose2(c * x_ - s * y_, s * x_ + c * y_, theta + theta_); + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/cpp/Pose2.h b/cpp/Pose2.h index ccc8c7709..7a3e028ea 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -9,6 +9,7 @@ #pragma once #include "Point2.h" +#include "Vector.h" #include "Testable.h" namespace gtsam { @@ -55,11 +56,32 @@ namespace gtsam { bool equals(const Pose2& pose, double tol = 1e-9) const; /** get functions for x, y, theta */ - double x() const { return x_;} - double y() const { return y_;} - double theta() const { return theta_;} + inline double x() const { return x_;} + inline double y() const { return y_;} + inline double theta() const { return theta_;} + /** return DOF, dimensionality of tangent space = 3 */ + size_t dim() const { + return 3; + } + + /* exponential map */ Pose2 exmap(const Vector& v) const; - }; + + /** return vectorized form (column-wise)*/ + Vector vector() const; + + /** rotate pose by theta */ + Pose2 rotate(double theta) const; + + // operators + Pose2 operator+(const Pose2& p2) const { + return Pose2(x_ + p2.x_, y_ + p2.y_, theta_ + p2.theta_); + } + + Pose2 operator-(const Pose2& p2) const { + return Pose2(x_ - p2.x_, y_ - p2.y_, theta_ - p2.theta_); + } + }; // Pose2 } // namespace gtsam diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 6414507d5..04c0aa3be 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -173,53 +173,60 @@ TEST( NonlinearFactor, size ) } /* ************************************************************************* */ -Vector RotatePoseDisplacement(Vector d, double theta) { - double co=cos(theta); - double si=sin(theta); - return Matrix_(3,3, co, -si, 0.0, si, co, 0.0, 0.0, 0.0, 1.0)*d; +#include "numericalDerivative.h" -} -Vector h(const Pose2& p1, const Pose2& p2) { - double dx= p2.x()-p1.x(); - double dy= p2.y()-p1.y(); - double dtheta= p2.theta()-p1.theta(); - return RotatePoseDisplacement(Vector_(3,dx,dy,dtheta),-p1.theta()); +Pose2 h(const Pose2& p1, const Pose2& p2) { + Pose2 dpose = p2 - p1; + return dpose.rotate(-p1.theta()); } Matrix H1(const Pose2& p1, const Pose2& p2) { - double dx= p2.x()-p1.x(); double dy= p2.y()-p1.y(); - double co=cos(p1.theta()); - double si=sin(p1.theta()); - return Matrix_(3,3, -co, -si, -si*dx+co*dy, si, -co, -co*dx-si*dy, 0.0, 0.0, -1.0); - + double ct=cos(p1.theta()); + double st=sin(p1.theta()); + return Matrix_(3,3, + -ct, -st, -st*dx+ct*dy, + st, -ct, -ct*dx-st*dy, + 0.0, 0.0, -1.0 + ); } Matrix H2(const Pose2& p1) { - double co=cos(p1.theta()); - double si=sin(p1.theta()); - return Matrix_(3,3, co, si, 0.0, -si, co, 0.0, 0.0, 0.0, 1.0); + double ct=cos(p1.theta()); + double st=sin(p1.theta()); + return Matrix_(3,3, + ct, st, 0.0, + -st, ct, 0.0, + 0.0, 0.0, 1.0 + ); } TEST( PoseConstraintFactor2, testFunctions ) { Pose2 p1(0.0, 6.0, 0.0); Pose2 p2(0.101826, 6.111236, 0.011499); + //expected - Vector expectedh = Vector_(3, 0.101826, 0.111236, 0.011499); + Pose2 expectedh(0.101826, 0.111236, 0.011499); Matrix expectedH1 = Matrix_(3,3,-1.0, 0.0, 0.111236, 0.0, -1.0, -0.101826, 0.0, 0.0, -1.0); Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); // actual - Vector actualh = h(p1,p2); + Pose2 actualh = h(p1,p2); Matrix actualH1 = H1(p1,p2); Matrix actualH2 = H2(p1); - CHECK(assert_equal(actualh,expectedh)); - CHECK(assert_equal(actualH1,expectedH1)); - CHECK(assert_equal(actualH2,expectedH2)); + CHECK(assert_equal(expectedh,actualh)); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(expectedH2,actualH2)); + + Matrix numericalH1 = numericalDerivative21(h, p1, p2, 1e-5); + //CHECK(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5); + CHECK(assert_equal(numericalH2,actualH2)); } /* ************************************************************************* */ diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index e0bfc4a13..345f762f0 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -16,6 +16,20 @@ TEST(Pose2, constructors) { assert_equal(pose,origin); } +/* ************************************************************************* */ +TEST(Pose2, rotate) { + double theta = 0.1, c=cos(theta),s=sin(theta); + Pose2 p1(1,0,0.2), p2(0,1,0.4); + CHECK(assert_equal(Pose2( c,s,0.3),p1.rotate(theta))); + CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta))); +} + +/* ************************************************************************* */ +TEST(Pose2, operators) { + CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1))); + CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1))); +} + /* ************************************************************************* */ int main() { TestResult tr;