From 120b6878cfc7e075a41806d6df8e59ace396fd10 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 13 Jan 2011 17:09:29 +0000 Subject: [PATCH] Fixed range function for Pose3 --- gtsam/geometry/Pose3.cpp | 5 +++-- gtsam/geometry/tests/testPose3.cpp | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8d4099933..6fb1c567c 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -253,8 +253,9 @@ namespace gtsam { boost::optional H2) const { if (!H1 && !H2) return transform_to(point).norm(); Point3 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 = Matrix_(1, 2, x / n, y / n); + double x = d.x(), y = d.y(), z = d.z(), + d2 = x * x + y * y + z * z, n = sqrt(d2); + Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n); if (H1) *H1 = D_result_d * (*H1); if (H2) *H2 = D_result_d * (*H2); return n; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9d8456e01..7cd523744 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -466,7 +466,7 @@ TEST( Pose3, between ) /* ************************************************************************* */ // some shared test values - pulled from equivalent test in Pose2 -Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 3, 0); +Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4); Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI_4, 0.0, 0.0), l2); /* ************************************************************************* */ @@ -495,7 +495,7 @@ TEST( Pose3, range ) // Another test double actual34 = x3.range(l4, actualH1, actualH2); - EXPECT_DOUBLES_EQUAL(2,actual34,1e-9); + EXPECT_DOUBLES_EQUAL(5,actual34,1e-9); // Check numerical derivatives expectedH1 = numericalDerivative21(range_proxy, x3, l4, 1e-5);