diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 4276fe9b1..97dac111d 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -93,4 +93,28 @@ struct traits > : Testable >, internal::ManifoldTraits > {}; +// Helper class for to implement Range traits for classes with a bearing method +template +struct HasBearing { + typedef T result_type; + T operator()( + const A1& a1, const A2& a2, + OptionalJacobian::dimension, traits::dimension> H1, + OptionalJacobian::dimension, traits::dimension> H2) { + return a1.bearing(a2, H1, H2); + } +}; + +// Helper class for to implement Range traits for classes with a range method +template +struct HasRange { + typedef T result_type; + T operator()( + const A1& a1, const A2& a2, + OptionalJacobian::dimension, traits::dimension> H1, + OptionalJacobian::dimension, traits::dimension> H2) { + return a1.range(a2, H1, H2); + } +}; + } // namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index c289f8fb6..741f7a1e4 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -384,47 +385,22 @@ private: /// @} }; -template<> -struct traits : public internal::Manifold { -}; - -template<> -struct traits : public internal::Manifold< - CalibratedCamera> { -}; - -// Define Range functor specializations that are used in RangeFactor -template struct Range; +// manifold traits +template <> +struct traits : public internal::Manifold {}; template <> -struct Range { - typedef double result_type; - double operator()(const CalibratedCamera& camera, const Point3& point, - OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { - return camera.range(point, H1, H2); - } -}; +struct traits : public internal::Manifold {}; + +// range traits, used in RangeFactor +template <> +struct Range : HasRange {}; template <> -struct Range { - typedef double result_type; - double operator()(const CalibratedCamera& camera, const Pose3& pose, - OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) { - return camera.range(pose, H1, H2); - } -}; +struct Range : HasRange {}; template <> -struct Range { - typedef double result_type; - double operator()(const CalibratedCamera& camera1, - const CalibratedCamera& camera2, - OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) { - return camera1.range(camera2, H1, H2); - } -}; -} +struct Range : HasRange {}; + +} // namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 93c017290..31052bfa6 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -19,6 +19,7 @@ #pragma once #include +#include namespace gtsam { @@ -302,62 +303,31 @@ private: }; -template -struct traits > : public internal::Manifold< - PinholeCamera > { -}; - -template -struct traits > : public internal::Manifold< - PinholeCamera > { -}; - -// Define Range functor specializations that are used in RangeFactor -template struct Range; +// manifold traits template -struct Range, Point3> { - typedef double result_type; - typedef PinholeCamera Camera; - double operator()(const Camera& camera, const Point3& point, - OptionalJacobian<1, Camera::dimension> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { - return camera.range(point, H1, H2); - } -}; +struct traits > + : public internal::Manifold > {}; template -struct Range, Pose3> { - typedef double result_type; - typedef PinholeCamera Camera; - double operator()(const Camera& camera, const Pose3& pose, - OptionalJacobian<1, Camera::dimension> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) { - return camera.range(pose, H1, H2); - } -}; +struct traits > + : public internal::Manifold > {}; + +// range traits, used in RangeFactor +template +struct Range, Point3> + : HasRange, Point3> {}; + +template +struct Range, Pose3> + : HasRange, Pose3> {}; template -struct Range, PinholeCamera > { - typedef double result_type; - typedef PinholeCamera CameraA; - typedef PinholeCamera CameraB; - double operator()(const CameraA& camera1, const CameraB& camera2, - OptionalJacobian<1, CameraA::dimension> H1 = boost::none, - OptionalJacobian<1, CameraB::dimension> H2 = boost::none) { - return camera1.range(camera2, H1, H2); - } -}; +struct Range, PinholeCamera > + : HasRange, PinholeCamera > {}; template -struct Range, CalibratedCamera> { - typedef double result_type; - typedef PinholeCamera Camera; - double operator()(const Camera& camera, const CalibratedCamera& cc, - OptionalJacobian<1, Camera::dimension> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) { - return camera.range(cc, H1, H2); - } -}; +struct Range, CalibratedCamera> + : HasRange, CalibratedCamera> {}; } // \ gtsam diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index ab488c65f..0358bcf42 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -290,57 +291,24 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -template<> +template <> struct traits : public internal::LieGroup {}; -template<> +template <> struct traits : public internal::LieGroup {}; -// Define Bearing functor specializations that are used in BearingFactor -template struct Bearing; +// bearing and range traits, used in RangeFactor +template <> +struct Bearing : HasBearing {}; template <> -struct Bearing { - typedef Rot2 result_type; - Rot2 operator()(const Pose2& pose, const Point2& point, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none) { - return pose.bearing(point, H1, H2); - } -}; +struct Bearing : HasBearing {}; template <> -struct Bearing { - typedef Rot2 result_type; - Rot2 operator()(const Pose2& pose1, const Pose2& pose2, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { - return pose1.bearing(pose2, H1, H2); - } -}; - -// Define Range functor specializations that are used in RangeFactor -template struct Range; +struct Range : HasRange {}; template <> -struct Range { - typedef double result_type; - double operator()(const Pose2& pose, const Point2& point, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none) { - return pose.range(point, H1, H2); - } -}; - -template <> -struct Range { - typedef double result_type; - double operator()(const Pose2& pose1, const Pose2& pose2, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { - return pose1.range(pose2, H1, H2); - } -}; +struct Range : HasRange {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index afe953d1d..732a511c2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -19,6 +19,7 @@ #include +#include #include #include #include @@ -337,40 +338,14 @@ struct traits : public internal::LieGroup {}; template <> struct traits : public internal::LieGroup {}; -// Define Bearing functor specializations that are used in BearingFactor -template struct Bearing; +// bearing and range traits, used in RangeFactor +template <> +struct Bearing : HasBearing {}; template <> -struct Bearing { - typedef Unit3 result_type; - Unit3 operator()(const Pose3& pose, const Point3& point, - OptionalJacobian<2, 6> H1 = boost::none, - OptionalJacobian<2, 3> H2 = boost::none) { - return pose.bearing(point, H1, H2); - } -}; - -// Define Range functor specializations that are used in RangeFactor -template struct Range; +struct Range : HasRange {}; template <> -struct Range { - typedef double result_type; - double operator()(const Pose3& pose, const Point3& point, - OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { - return pose.range(point, H1, H2); - } -}; - -template <> -struct Range { - typedef double result_type; - double operator()(const Pose3& pose1, const Pose3& pose2, - OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) { - return pose1.range(pose2, H1, H2); - } -}; +struct Range : HasRange {}; } // namespace gtsam diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 09b128d91..5d8d09ab8 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -125,34 +126,28 @@ public: }; -template<> -struct traits : public internal::Manifold { -}; - -template<> -struct traits : public internal::Manifold { -}; - -template <> -struct Range { - typedef double result_type; - double operator()(const SimpleCamera& camera, const Point3& point, - OptionalJacobian<1, 11> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { - return camera.range(point, H1, H2); - } -}; - -template <> -struct Range { - typedef double result_type; - double operator()(const SimpleCamera& camera, const SimpleCamera& sc, - OptionalJacobian<1, 11> H1 = boost::none, - OptionalJacobian<1, 11> H2 = boost::none) { - return camera.range(sc, H1, H2); - } -}; - /// Recover camera from 3*4 camera matrix GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); -} + +// manifold traits +template <> +struct traits : public internal::Manifold {}; + +template <> +struct traits : public internal::Manifold {}; + +// range traits, used in RangeFactor +template <> +struct Range : HasRange {}; + +template <> +struct Range : HasRange {}; + +template <> +struct Range : HasRange {}; + +template +struct Range > + : HasRange > {}; + +} // namespace gtsam diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5e56d518a..4575f4bd1 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -357,6 +357,7 @@ struct Range { double operator()(const Vector3& v1, const Vector3& v2, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { return (v2 - v1).norm(); + // derivatives not implemented } }; }