From a8288746b4beea8204daa91ec7d291572722517d Mon Sep 17 00:00:00 2001 From: BrettRD Date: Mon, 9 Jan 2023 13:40:41 +1100 Subject: [PATCH 1/4] wraps more getters in Expressions --- gtsam/geometry/OrientedPlane3.h | 6 ++++-- gtsam/slam/expressions.h | 36 +++++++++++++++++++++++++++++++++ 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 92d1673793..d0d912ee9a 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -125,12 +125,14 @@ class GTSAM_EXPORT OrientedPlane3 { } /// Return the normal - inline Unit3 normal() const { + inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const { + if (H) *H << I_2x2, Z_2x1; return n_; } /// Return the perpendicular distance to the origin - inline double distance() const { + inline double distance(OptionalJacobian<1, 3> H = boost::none) const { + if (H) *H << 0,0,1; return d_; } }; diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 24476cb5e2..3e36835afa 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -12,6 +12,7 @@ #include #include #include +#include #include namespace gtsam { @@ -26,6 +27,11 @@ inline Point2_ transformTo(const Pose2_& x, const Point2_& p) { return Point2_(x, &Pose2::transformTo, p); } +inline Double_ range(const Point2_& p, const Point2_& q) +{ + return Double_(Range(), p, q); +} + // 3D Geometry typedef Expression Point3_; @@ -33,6 +39,7 @@ typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; typedef Expression Line3_; +typedef Expression OrientedPlane3_; inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformTo, p); @@ -48,6 +55,10 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) { return Line3_(f, wTc, wL); } +inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) { + return Pose3_(p, &Pose3::transformPoseTo, q); +} + inline Point3_ normalize(const Point3_& a){ Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize; return Point3_(f, a); @@ -70,16 +81,28 @@ namespace internal { inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { return pose.rotation(H); } + +inline Point3 translation(const Pose3& pose, OptionalJacobian<3, 6> H) { + return pose.translation(H); +} } // namespace internal inline Rot3_ rotation(const Pose3_& pose) { return Rot3_(internal::rotation, pose); } +inline Point3_ translation(const Pose3_& pose) { + return Point3_(internal::translation, pose); +} + inline Point3_ rotate(const Rot3_& x, const Point3_& p) { return Point3_(x, &Rot3::rotate, p); } +inline Point3_ point3(const Unit3_& v) { + return Point3_(&Unit3::point3, v); +} + inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) { return Unit3_(x, &Rot3::rotate, p); } @@ -92,6 +115,14 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) { return Unit3_(x, &Rot3::unrotate, p); } +inline Double_ distance(const OrientedPlane3_& p) { + return Double_(&OrientedPlane3::distance, p); +} + +inline Unit3_ normal(const OrientedPlane3_& p) { + return Unit3_(&OrientedPlane3::normal, p); +} + // Projection typedef Expression Cal3_S2_; @@ -143,6 +174,11 @@ Point2_ uncalibrate(const Expression& K, const Point2_& xy_hat) { return Point2_(K, &CALIBRATION::uncalibrate, xy_hat); } +template +inline Pose3_ getPose(const Expression > & cam) { + return Pose3_(&PinholeCamera::getPose, cam); +} + /// logmap // TODO(dellaert): Should work but fails because of a type deduction conflict. From 376683b80886b2723ce567314216ada93b69e8f3 Mon Sep 17 00:00:00 2001 From: BrettRD Date: Mon, 9 Jan 2023 15:33:48 +1100 Subject: [PATCH 2/4] 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 533041a2cf..e2525bd43c 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 3e36835afa..d1bfab7f2f 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); } From 65a21edc89908b853f368d1e852925b9dd3820bc Mon Sep 17 00:00:00 2001 From: BrettRD Date: Mon, 9 Jan 2023 16:25:43 +1100 Subject: [PATCH 3/4] refers to tangent space in test --- gtsam/geometry/tests/testOrientedPlane3.cpp | 40 +++++++++++---------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index e2525bd43c..88eb0c251f 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -34,34 +34,19 @@ 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(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)); + double distance1 = plane1.distance(); EXPECT(assert_equal(coefficient1, c, 1e-8)); - EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), normal1.unitVector())); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector())); EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8); - Vector4 coefficient2 = plane2.planeCoefficients(); - double distance2 = plane2.distance(H_distance); - Unit3 normal2 = plane2.normal(H_normal); + double distance2 = plane2.distance(); 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(), normal2.unitVector())); + EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector())); } @@ -181,6 +166,23 @@ TEST(OrientedPlane3, jacobian_retract) { } } +//******************************************************************************* +TEST(OrientedPlane3, getMethodJacobians) { + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + Matrix33 H_retract, H_getters; + Matrix23 H_normal; + Matrix13 H_distance; + + // The getter's jacobians lie exactly on the tangent space + // so they should exactly equal the retract jacobian for the zero vector. + Vector3 v(0, 0, 0); + plane.retract(v, H_retract); + plane.normal(H_normal); + plane.distance(H_distance); + H_getters << H_normal, H_distance; + EXPECT(assert_equal(H_retract, H_getters, 1e-5)); +} + /* ************************************************************************* */ int main() { srand(time(nullptr)); From 80aa47539cea6a05c85273d7e3eb0cae5b3797f9 Mon Sep 17 00:00:00 2001 From: BrettRD Date: Mon, 9 Jan 2023 16:58:59 +1100 Subject: [PATCH 4/4] numerically checks getter jacobians --- gtsam/geometry/tests/testOrientedPlane3.cpp | 29 +++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 88eb0c251f..9eb5e2f29c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -166,6 +166,32 @@ TEST(OrientedPlane3, jacobian_retract) { } } +//******************************************************************************* +TEST(OrientedPlane3, jacobian_normal) { + Matrix23 H_actual, H_expected; + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + + std::function f = std::bind( + &OrientedPlane3::normal, std::placeholders::_1, boost::none); + + H_expected = numericalDerivative11(f, plane); + plane.normal(H_actual); + EXPECT(assert_equal(H_actual, H_expected, 1e-5)); +} + +//******************************************************************************* +TEST(OrientedPlane3, jacobian_distance) { + Matrix13 H_actual, H_expected; + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + + std::function f = std::bind( + &OrientedPlane3::distance, std::placeholders::_1, boost::none); + + H_expected = numericalDerivative11(f, plane); + plane.distance(H_actual); + EXPECT(assert_equal(H_actual, H_expected, 1e-5)); +} + //******************************************************************************* TEST(OrientedPlane3, getMethodJacobians) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); @@ -173,8 +199,7 @@ TEST(OrientedPlane3, getMethodJacobians) { Matrix23 H_normal; Matrix13 H_distance; - // The getter's jacobians lie exactly on the tangent space - // so they should exactly equal the retract jacobian for the zero vector. + // confirm the getters are exactly on the tangent space Vector3 v(0, 0, 0); plane.retract(v, H_retract); plane.normal(H_normal);