From 131482298da522f054dceaceb15bd4a473ed93ba Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 6 Oct 2011 18:10:45 +0000 Subject: [PATCH] Added range to pose for Pose2 --- .cproject | 169 +++++++++++++++-------------- gtsam/geometry/Pose2.cpp | 17 ++- gtsam/geometry/Pose2.h | 9 ++ gtsam/geometry/tests/testPose2.cpp | 37 +++++++ 4 files changed, 152 insertions(+), 80 deletions(-) diff --git a/.cproject b/.cproject index 6ae853633..9848b8ba2 100644 --- a/.cproject +++ b/.cproject @@ -375,6 +375,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -401,7 +409,6 @@ make - tests/testBayesTree.run true false @@ -409,7 +416,6 @@ make - testBinaryBayesNet.run true false @@ -457,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -465,7 +470,6 @@ make - tests/testSymbolicFactor.run true false @@ -473,7 +477,6 @@ make - testSymbolicFactorGraph.run true false @@ -489,20 +492,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -529,6 +523,7 @@ make + testGraph.run true false @@ -600,6 +595,7 @@ make + testInference.run true false @@ -607,6 +603,7 @@ make + testGaussianFactor.run true false @@ -614,6 +611,7 @@ make + testJunctionTree.run true false @@ -621,6 +619,7 @@ make + testSymbolicBayesNet.run true false @@ -628,6 +627,7 @@ make + testSymbolicFactorGraph.run true false @@ -721,6 +721,14 @@ true true + + make + -j2 + check + true + true + true + make -j2 @@ -761,14 +769,6 @@ true true - - make - -j2 - check - true - true - true - make -j2 @@ -825,14 +825,6 @@ true true - - make - -j2 - vSFMexample.run - true - true - true - make -j2 @@ -841,6 +833,14 @@ true true + + make + -j2 + vSFMexample.run + true + true + true + make -j2 @@ -1083,7 +1083,6 @@ make - testErrors.run true false @@ -1385,6 +1384,14 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + make -j2 @@ -1467,6 +1474,7 @@ make + testSimulated2DOriented.run true false @@ -1506,6 +1514,7 @@ make + testSimulated2D.run true false @@ -1513,6 +1522,7 @@ make + testSimulated3D.run true false @@ -1704,6 +1714,7 @@ make + tests/testGaussianISAM2 true false @@ -1725,46 +1736,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -1861,6 +1832,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + make -j2 @@ -1901,14 +1920,6 @@ true true - - make - -j2 - check - true - true - true - diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 9d09c7947..0fc02ea95 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -210,7 +210,7 @@ namespace gtsam { /* ************************************************************************* */ double Pose2::range(const Point2& point, boost::optional H1, boost::optional H2) const { - if (!H1 && !H2) return transform_to(point).norm(); + 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, n = sqrt(d2); Matrix D_result_d; @@ -224,6 +224,21 @@ namespace gtsam { return n; } + /* ************************************************************************* */ + double Pose2::range(const Pose2& point, + boost::optional H1, + boost::optional H2) const { + double r = range(point.t(), H1, H2); + if (H2) { + // NOTE: expmap changes the orientation of expmap direction, + // so we must rotate the jacobian + Matrix H2_ = *H2 * point.r().matrix(); + *H2 = zeros(1, 3); + insertSub(*H2, H2_, 0, 0); + } + return r; + } + /* ************************************************************************* * New explanation, from scan.ml * It finds the angle using a linear method: diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 6e53ac4f3..549963856 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -166,6 +166,15 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /** + * Calculate range to another pose + * @param point 2D location of other pose + * @return range (double) + */ + double range(const Pose2& point, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 200a2e2ff..d498bef1f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -539,6 +539,43 @@ TEST( Pose2, range ) EXPECT(assert_equal(expectedH2,actualH2)); } +/* ************************************************************************* */ +LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { + return LieVector(pose.range(point)); +} +TEST( Pose2, range_pose ) +{ + Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0); + + Matrix expectedH1, actualH1, expectedH2, actualH2; + + // establish range is indeed zero + EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9); + + // establish range is indeed 45 degrees + EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9); + + // Another pair + double actual23 = x2.range(xl3, actualH1, actualH2); + EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3); + expectedH2 = numericalDerivative22(range_pose_proxy, x2, xl3); + EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); + + // Another test + double actual34 = x3.range(xl4, actualH1, actualH2); + EXPECT_DOUBLES_EQUAL(2,actual34,1e-9); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(range_pose_proxy, x3, xl4); + expectedH2 = numericalDerivative22(range_pose_proxy, x3, xl4); + EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); +} + /* ************************************************************************* */ TEST(Pose2, align_1) {