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 {};