From 68d26ff279d2a2ca275929bac2b1205397134181 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Thu, 9 Mar 2017 16:57:29 -0800 Subject: [PATCH 1/5] Support bearing factors between Pose3 values I am modelling a system in which there are two bodies, and one can observe the other but cannot estimate the other's pose. This is perfectly modeled by a BearingRangeFactor, but without this patch, you cannot make a BearingRangeFactor between two Pose3 values. This adds support for that by extending the Pose3 class to support calling bearing() on another Pose3. --- gtsam/geometry/Pose3.cpp | 9 +++++++++ gtsam/geometry/Pose3.h | 12 ++++++++++++ 2 files changed, 21 insertions(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 25cd661e8..529453bc9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -378,6 +378,15 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } } +/* ************************************************************************* */ +Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1, + OptionalJacobian<2, 6> H2) const { + if (H2) { + return bearing(point.translation(), H1, H2.cols<3>(3)); + } + return bearing(point.translation(), H1, boost::none); +} + /* ************************************************************************* */ boost::optional Pose3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ee1206119..111f226bb 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -282,6 +282,15 @@ public: Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, OptionalJacobian<2, 3> H2 = boost::none) const; + /** + * Calculate bearing to another pose + * @param other 3D location and orientation of other body. The orientation + * information is ignored. + * @return bearing (Unit3) + */ + Unit3 bearing(const Pose3& point, OptionalJacobian<2, 6> H1 = boost::none, + OptionalJacobian<2, 6> H2 = boost::none) const; + /// @} /// @name Advanced Interface /// @{ @@ -353,6 +362,9 @@ struct traits : public internal::LieGroup {}; template <> struct Bearing : HasBearing {}; +template<> +struct Bearing : HasBearing {}; + template struct Range : HasRange {}; From 42e7e313402eb5ed5d65db870ab20ba55567529b Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Mon, 13 Mar 2017 10:31:37 -0700 Subject: [PATCH 2/5] Add new Pose3::bearing overload to Python wrapper --- python/handwritten/geometry/Pose3.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index f778ea4e0..551f2f60c 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -53,6 +53,9 @@ void exportPose3(){ // function pointers to desambiguate range() calls double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + // function pointers to desambiguate bearing() calls + Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const = &Pose3::bearing; + Unit3 (Pose3::*bearing2)(const Pose3 &, OptionalJacobian<2,6>, OptionalJacobian<2,6>) const = &Pose3::bearing; class_("Pose3") .def(init<>()) @@ -65,7 +68,6 @@ void exportPose3(){ .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol"))) .def("identity", &Pose3::identity) .staticmethod("identity") - .def("bearing", &Pose3::bearing) .def("matrix", &Pose3::matrix) .def("transform_from", &Pose3::transform_from, transform_from_overloads(args("point", "H1", "H2"))) @@ -88,5 +90,6 @@ void exportPose3(){ .def("between", between2, between_overloads()) .def("range", range1, range_overloads()) .def("range", range2, range_overloads()) - .def("bearing", &Pose3::bearing, bearing_overloads()); + .def("bearing", bearing1, bearing_overloads()) + .def("bearing", bearing2, bearing_overloads()); } From 53898778962ee4579f77b5f2875ecdee80db7625 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Wed, 15 Mar 2017 10:36:10 -0700 Subject: [PATCH 3/5] set attitude jacobian to zero in Pose3-to-Pose3 bearing --- gtsam/geometry/Pose3.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 529453bc9..8554b7f46 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -382,6 +382,7 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1, OptionalJacobian<2, 6> H2) const { if (H2) { + H2->setZero(); return bearing(point.translation(), H1, H2.cols<3>(3)); } return bearing(point.translation(), H1, boost::none); From 5ae331ad041235e80b6a645d6dbe6062c9783435 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Thu, 30 Mar 2017 12:52:48 -0700 Subject: [PATCH 4/5] add test for pose-to-pose bearing --- gtsam/geometry/tests/testPose3.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 97ccbcb34..98d3e11ee 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -708,6 +708,27 @@ TEST(Pose3, Bearing2) { EXPECT(assert_equal(expectedH2, actualH2)); } +TEST(Pose3, PoseToPoseBearing) { + Matrix expectedH1, actualH1, expectedH2, actualH2, H2block; + EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2); + + // Since the second pose is treated as a point, the value calculated by + // numericalDerivative22 only depends on the position of the pose. Here, we + // calculate the Jacobian w.r.t. the second pose's position, and then augment + // that with zeroes in the block that is w.r.t. the second pose's + // orientation. + H2block = numericalDerivative22(bearing_proxy, xl1, l2); + expectedH2 = Matrix(2, 6); + expectedH2.setZero(); + expectedH2.block<2, 3>(0, 3) = H2block; + + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ TEST( Pose3, unicycle ) { From 2058b928826abbe121ebffa85fa8f989c67d5f46 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Thu, 30 Mar 2017 12:53:01 -0700 Subject: [PATCH 5/5] rename Pose3 parameter from point to pose --- gtsam/geometry/Pose3.cpp | 6 +++--- gtsam/geometry/Pose3.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8554b7f46..50e487e75 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -379,13 +379,13 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } /* ************************************************************************* */ -Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1, +Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1, OptionalJacobian<2, 6> H2) const { if (H2) { H2->setZero(); - return bearing(point.translation(), H1, H2.cols<3>(3)); + return bearing(pose.translation(), H1, H2.cols<3>(3)); } - return bearing(point.translation(), H1, boost::none); + return bearing(pose.translation(), H1, boost::none); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 111f226bb..6df62485b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -288,7 +288,7 @@ public: * information is ignored. * @return bearing (Unit3) */ - Unit3 bearing(const Pose3& point, OptionalJacobian<2, 6> H1 = boost::none, + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none, OptionalJacobian<2, 6> H2 = boost::none) const; /// @}