diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index a119096d4..09b128d91 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -133,6 +133,26 @@ template<> struct traits : public internal::Manifold { }; - /// Recover camera from 3*4 camera matrix - GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); +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); }