From 376683b80886b2723ce567314216ada93b69e8f3 Mon Sep 17 00:00:00 2001 From: BrettRD Date: Mon, 9 Jan 2023 15:33:48 +1100 Subject: [PATCH] adds tests for OrientedPlane3 derivatives --- gtsam/geometry/tests/testOrientedPlane3.cpp | 23 +++++++++++++++++---- gtsam/slam/expressions.h | 3 +-- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 533041a2c..e2525bd43 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -34,19 +34,34 @@ GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* TEST(OrientedPlane3, getMethods) { Vector4 c; + Matrix23 H_normal, expected_H_normal; + Matrix13 H_distance, expected_H_distance; + c << -1, 0, 0, 5; + expected_H_normal << 1,0,0, + 0,1,0; + expected_H_distance << 0,0,1; + OrientedPlane3 plane1(c); OrientedPlane3 plane2(c[0], c[1], c[2], c[3]); + Vector4 coefficient1 = plane1.planeCoefficients(); - double distance1 = plane1.distance(); + double distance1 = plane1.distance(H_distance); + Unit3 normal1 = plane1.normal(H_normal); + EXPECT(assert_equal(expected_H_normal, H_normal, 1e-5)); + EXPECT(assert_equal(expected_H_distance, H_distance, 1e-5)); EXPECT(assert_equal(coefficient1, c, 1e-8)); - EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector())); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), normal1.unitVector())); EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); + Vector4 coefficient2 = plane2.planeCoefficients(); - double distance2 = plane2.distance(); + double distance2 = plane2.distance(H_distance); + Unit3 normal2 = plane2.normal(H_normal); EXPECT(assert_equal(coefficient2, c, 1e-8)); + EXPECT(assert_equal(expected_H_normal, H_normal, 1e-5)); + EXPECT(assert_equal(expected_H_distance, H_distance, 1e-5)); EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8); - EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector())); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), normal2.unitVector())); } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 3e36835af..d1bfab7f2 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -27,8 +27,7 @@ inline Point2_ transformTo(const Pose2_& x, const Point2_& p) { return Point2_(x, &Pose2::transformTo, p); } -inline Double_ range(const Point2_& p, const Point2_& q) -{ +inline Double_ range(const Point2_& p, const Point2_& q) { return Double_(Range(), p, q); }