diff --git a/gtsam/geometry/SphericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp index 3124d10ff..e3b3245a3 100644 --- a/gtsam/geometry/SphericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -60,6 +60,21 @@ Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose, return pu; } +/* ************************************************************************* */ +Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint) const { + + Matrix23 Dtf_rot; + Matrix2 Dtf_point; // calculated by transformTo if needed + const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0, Dpoint ? &Dtf_point : 0); + + if (Dpose) + *Dpose << Dtf_rot, Matrix::Zero(2,3); //2x6 (translation part is zero) + if (Dpoint) + *Dpoint = Dtf_point; //2x2 + return pu; +} + /* ************************************************************************* */ Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const { return pose().transformFrom(depth * pu); diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index df433e807..2cac50c56 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -31,15 +31,20 @@ namespace gtsam { class GTSAM_EXPORT EmptyCal { protected: - double d_ = 0; + Matrix3 K_; public: + ///< shared pointer to calibration object EmptyCal() - : d_(0) { + : K_(Matrix3::Identity()) { } /// Default destructor virtual ~EmptyCal() = default; using shared_ptr = boost::shared_ptr; + void print(const std::string& s) const { + std::cout << "empty calibration: " << s << std::endl; + } + Matrix3 K() const {return K_;} }; /** @@ -159,6 +164,14 @@ class GTSAM_EXPORT SphericalCamera { Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + /** Project point into the image + * (note: there is no CheiralityException for a spherical camera) + * @param point 3D direction in world coordinates + * @return the intrinsic coordinates of the projected point + */ + Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; + /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Unit3& p, const double depth) const;