From e35c73388fdc0d765fa14a8cd3662bf1bae362d4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Jun 2013 05:06:15 +0000 Subject: [PATCH] Simplified Pose2::range --- gtsam/geometry/Point2.cpp | 32 +++++++++++------ gtsam/geometry/Point2.h | 6 ++-- gtsam/geometry/Pose2.cpp | 13 +++---- gtsam/geometry/tests/testPoint2.cpp | 54 ++++++++++++++++++++--------- gtsam/geometry/tests/testPose2.cpp | 23 ++++++------ 5 files changed, 76 insertions(+), 52 deletions(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index e0c82f060..051d33279 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -25,6 +25,8 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ INSTANTIATE_LIE(Point2); +static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0); + /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; @@ -36,8 +38,17 @@ bool Point2::equals(const Point2& q, double tol) const { } /* ************************************************************************* */ -double Point2::norm() const { - return sqrt(x_ * x_ + y_ * y_); +double Point2::norm(boost::optional H) const { + double r = sqrt(x_ * x_ + y_ * y_); + if (H) { + Matrix D_result_d; + if (std::abs(r) > 1e-10) + D_result_d = Matrix_(1, 2, x_ / r, y_ / r); + else + D_result_d = oneOne; // TODO: really infinity, why 1 here?? + *H = D_result_d; + } + return r; } /* ************************************************************************* */ @@ -45,15 +56,14 @@ static const Matrix I2 = eye(2); double Point2::distance(const Point2& point, boost::optional H1, boost::optional H2) const { Point2 d = point - *this; - double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2); - Matrix D_result_d; - if (std::abs(r) > 1e-10) - D_result_d = Matrix_(1, 2, x / r, y / r); - else - D_result_d = Matrix_(1, 2, 1.0, 1.0); - if (H1) *H1 = -D_result_d; - if (H2) *H2 = D_result_d; - return r; + if (H1 || H2) { + Matrix H; + double r = d.norm(H); + if (H1) *H1 = -H; + if (H2) *H2 = H; + return r; + } else + return d.norm(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 1e5e507b4..f2f0a0fef 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -144,12 +144,12 @@ public: /// @name Vector Space /// @{ - /** norm of point */ - double norm() const; - /** creates a unit vector */ Point2 unit() const { return *this/norm(); } + /** norm of point */ + double norm(boost::optional H = boost::none) const; + /** distance between two points */ double distance(const Point2& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 3282f3392..6229e4077 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -223,15 +223,10 @@ double Pose2::range(const Point2& point, boost::optional H1, boost::optional H2) const { if (!H1 && !H2) return transform_to(point).norm(); Point2 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2); - Matrix D_result_d; - if(std::abs(r) > 1e-10) - D_result_d = Matrix_(1, 2, x / r, y / r); - else { - D_result_d = Matrix_(1,2, 1.0, 1.0); - } - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); + Matrix H; + double r = d.norm(H); + if (H1) *H1 = H * (*H1); + if (H2) *H2 = H * (*H2); return r; } diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 9d516ebb1..cb06598c6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -72,16 +72,6 @@ TEST( Point2, arithmetic) EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); } -/* ************************************************************************* */ -TEST( Point2, norm) -{ - Point2 p0(cos(5.0), sin(5.0)); - DOUBLES_EQUAL(1,p0.norm(),1e-6); - Point2 p1(4, 5), p2(1, 1); - DOUBLES_EQUAL( 5,p1.distance(p2),1e-6); - DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); -} - /* ************************************************************************* */ TEST( Point2, unit) { @@ -92,12 +82,35 @@ TEST( Point2, unit) } /* ************************************************************************* */ -TEST( Point2, stream) +// some shared test values +Point2 x1, x2(1, 1), x3(1, 1); +Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); + +/* ************************************************************************* */ +LieVector norm_proxy(const Point2& point) { + return LieVector(point.norm()); +} +TEST( Point2, norm ) { - Point2 p(1,2); - std::ostringstream os; - os << p; - EXPECT(os.str() == "(1, 2)"); + Point2 p0(cos(5.0), sin(5.0)); + DOUBLES_EQUAL(1,p0.norm(),1e-6); + Point2 p1(4, 5), p2(1, 1); + DOUBLES_EQUAL( 5,p1.distance(p2),1e-6); + DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); + + Matrix expectedH, actualH; + double actual ; + + // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] + actual = x1.norm(actualH); + EXPECT_DOUBLES_EQUAL(0,actual,1e-9); + expectedH = Matrix_(1, 2, 1.0, 1.0); + EXPECT(assert_equal(expectedH,actualH)); + + actual = x2.norm(actualH); + EXPECT_DOUBLES_EQUAL(sqrt(2),actual,1e-9); + expectedH = numericalDerivative11(norm_proxy, x2); + EXPECT(assert_equal(expectedH,actualH)); } /* ************************************************************************* */ @@ -106,8 +119,6 @@ LieVector distance_proxy(const Point2& location, const Point2& point) { } TEST( Point2, distance ) { - Point2 x1, x2(1, 1), x3(1, 1); - Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Matrix expectedH1, actualH1, expectedH2, actualH2; // establish distance is indeed zero @@ -137,6 +148,15 @@ TEST( Point2, distance ) EXPECT(assert_equal(expectedH2,actualH2)); } +/* ************************************************************************* */ +TEST( Point2, stream) +{ + Point2 p(1,2); + std::ostringstream os; + os << p; + EXPECT(os.str() == "(1, 2)"); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index e155807ae..0a3b3d3e3 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -14,22 +14,21 @@ * @brief Unit tests for Pose2 class */ -#include -#include - -#include -#include -#include // for operator += -using namespace boost::assign; - -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include +#include +#include +#include // for operator += +#include +#include + +using namespace boost::assign; using namespace gtsam; using namespace std;