From c20eaecf8211ee70c9326b890da3a4959b406a0a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:28:17 +0100 Subject: [PATCH] PinholeBase class --- gtsam/geometry/PinholePose.h | 484 ++++++++++++++++++++++++++--------- 1 file changed, 358 insertions(+), 126 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index f36aec571..d7c099ab3 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -32,92 +32,32 @@ namespace gtsam { * \nosubgrouping */ template -class PinholePose { +class PinholeBase { private: + Pose3 pose_; - boost::shared_ptr K_; public: - enum { - dimension = 6 - }; - /// @name Standard Constructors /// @{ /** default constructor */ - PinholePose() { + PinholeBase() { } /** constructor with pose */ - explicit PinholePose(const Pose3& pose) : - pose_(pose), K_(new Calibration()) { - } - - /** constructor with pose and calibration */ - PinholePose(const Pose3& pose, const boost::shared_ptr& K) : - pose_(pose), K_(K) { - } - - /// @} - /// @name Named Constructors - /// @{ - - /** - * Create a level camera at the given 2D pose and height - * @param K the calibration - * @param pose2 specifies the location and viewing direction - * (theta 0 = looking in direction of positive X axis) - * @param height camera height - */ - static PinholePose Level(const boost::shared_ptr& K, - const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - const Pose3 pose3(wRc, t); - return PinholePose(pose3, K); - } - - /// PinholePose::level with default calibration - static PinholePose Level(const Pose2& pose2, double height) { - return PinholePose::Level(boost::make_shared(), pose2, height); - } - - /** - * Create a camera at the given eye position looking at a target point in the scene - * with the specified up direction vector. - * @param eye specifies the camera position - * @param target the point to look at - * @param upVector specifies the camera up direction vector, - * doesn't need to be on the image plane nor orthogonal to the viewing axis - * @param K optional calibration parameter - */ - static PinholePose Lookat(const Point3& eye, const Point3& target, - const Point3& upVector, const boost::shared_ptr& K = - boost::make_shared()) { - Point3 zc = target - eye; - zc = zc / zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc / xc.norm(); - Point3 yc = zc.cross(xc); - Pose3 pose3(Rot3(xc, yc, zc), eye); - return PinholePose(pose3, K); + explicit PinholeBase(const Pose3& pose) : + pose_(pose) { } /// @} /// @name Advanced Constructors /// @{ - explicit PinholePose(const Vector &v) { - pose_ = Pose3::Expmap(v); - } - - PinholePose(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + explicit PinholeBase(const Vector &v) : + pose_(Pose3::Expmap(v)) { } /// @} @@ -125,35 +65,29 @@ public: /// @{ /// assert equality up to a tolerance - bool equals(const PinholePose &camera, double tol = 1e-9) const { + bool equals(const PinholeBase &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol); } /// print - void print(const std::string& s = "PinholePose") const { + void print(const std::string& s = "PinholeBase") const { pose_.print(s + ".pose"); - K_->print(s + ".calibration"); } /// @} /// @name Standard Interface /// @{ - virtual ~PinholePose() { - } - - /// return pose - inline Pose3& pose() { - return pose_; + virtual ~PinholeBase() { } /// return pose, constant version - inline const Pose3& pose() const { + const Pose3& pose() const { return pose_; } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, 6> H) const { + const Pose3& pose(OptionalJacobian<6, 6> H) const { if (H) { H->setZero(); H->block(0, 0, 6, 6) = I_6x6; @@ -162,45 +96,7 @@ public: } /// return calibration - inline boost::shared_ptr calibration() { - return K_; - } - - /// return calibration - inline const boost::shared_ptr calibration() const { - return K_; - } - - /// @} - /// @name Manifold - /// @{ - - /// Manifold 6 - inline size_t dim() const { - return 6; - } - - /// Manifold 6 - inline static size_t Dim() { - return 6; - } - - typedef Eigen::Matrix VectorK6; - - /// move a cameras according to d - PinholePose retract(const Vector6& d) const { - return PinholePose(pose().retract(d), calibration()); - } - - /// return canonical coordinate - VectorK6 localCoordinates(const PinholePose& p) const { - return pose().localCoordinates(p.pose()); - } - - /// for Canonical - static PinholePose identity() { - return PinholePose(); // assumes that the default constructor is valid - } + virtual const Calibration& calibration() const = 0; /// @} /// @name Transformations and measurement functions @@ -226,10 +122,10 @@ public: } /// Project a point into the image and check depth - inline std::pair projectSafe(const Point3& pw) const { + std::pair projectSafe(const Point3& pw) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); - return std::make_pair(K_->uncalibrate(pn), pc.z() > 0); + return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); } /** project a point from world coordinate to the image, fixed Jacobians @@ -245,13 +141,13 @@ public: const Point2 pn = project_to_camera(pc); if (!Dcamera && !Dpoint) { - return K_->uncalibrate(pn); + return calibration().uncalibrate(pn); } else { const double z = pc.z(), d = 1.0 / z; // uncalibration Matrix2 Dpi_pn; - const Point2 pi = K_->uncalibrate(pn, boost::none, Dpi_pn); + const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); if (Dcamera) calculateDpose(pn, d, Dpi_pn, *Dcamera); @@ -263,15 +159,15 @@ public: } /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = K_->calibrate(p); + Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = calibration().calibrate(p); const Point3 pc(pn.x() * depth, pn.y() * depth, depth); return pose_.transform_from(pc); } /// backproject a 2-dimensional point to a 3-dimensional point at infinity - inline Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = K_->calibrate(p); + Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 return pose_.rotation().rotate(pc); } @@ -313,7 +209,7 @@ public: */ template double range( - const PinholePose& camera, // + const PinholeBase& camera, // OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return pose_.range(camera.pose(), Dcamera, Dother); @@ -384,10 +280,346 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(pose_); + } + +}; +// end of class PinholeBase + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * Instead of using this class, one might consider calibrating the measurements + * and using CalibratedCamera, which would then be faster. + * @addtogroup geometry + * \nosubgrouping + */ +template +class PinholePose: public PinholeBase { + +private: + + typedef PinholeBase Base; + boost::shared_ptr K_; + +public: + + enum { + dimension = 6 + }; + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholePose() { + } + + /** constructor with pose, uses default calibration */ + explicit PinholePose(const Pose3& pose) : + Base(pose), K_(new Calibration()) { + } + + /** constructor with pose and calibration */ + PinholePose(const Pose3& pose, const boost::shared_ptr& K) : + Base(pose), K_(K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static PinholePose Level(const boost::shared_ptr& K, + const Pose2& pose2, double height) { + return PinholePose(CalibratedCamera::LevelPose(pose2, height), K); + } + + /// PinholePose::level with default calibration + static PinholePose Level(const Pose2& pose2, double height) { + return PinholePose::Level(boost::make_shared(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static PinholePose Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const boost::shared_ptr& K = + boost::make_shared()) { + return PinholePose(CalibratedCamera::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholePose(const Vector &v) : + Base(v), K_(new Calibration()) { + } + + PinholePose(const Vector &v, const Vector &K) : + Base(v), K_(new Calibration(K)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholePose* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_->equals(e->calibration(), tol); + } + + /// print + void print(const std::string& s = "PinholePose") const { + Base::print(s); + K_->print(s + ".calibration"); + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholePose() { + } + + /// return calibration + const Calibration& calibration() const { + return *K_; + } + + /// @} + /// @name Manifold + /// @{ + + /// Manifold 6 + size_t dim() const { + return 6; + } + + /// Manifold 6 + static size_t Dim() { + return 6; + } + + typedef Eigen::Matrix VectorK6; + + /// move a cameras according to d + PinholePose retract(const Vector6& d) const { + return PinholePose(Base::pose().retract(d), K_); + } + + /// return canonical coordinate + VectorK6 localCoordinates(const PinholePose& p) const { + return Base::pose().localCoordinates(p.Base::pose()); + } + + /// for Canonical + static PinholePose identity() { + return PinholePose(); // assumes that the default constructor is valid + } + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * projects a 3-dimensional point in camera coordinates into the + * camera and returns a 2-dimensional point, no calibration applied + * @param P A point in camera coordinates + * @param Dpoint is the 2*3 Jacobian w.r.t. P + */ + static Point2 project_to_camera(const Point3& P, // + OptionalJacobian<2, 3> Dpoint = boost::none) { +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (P.z() <= 0) + throw CheiralityException(); +#endif + double d = 1.0 / P.z(); + const double u = P.x() * d, v = P.y() * d; + if (Dpoint) + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; + return Point2(u, v); + } + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const { + const Point3 pc = Base::pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); + } + + /** project a point from world coordinate to the image, fixed Jacobians + * @param pw is a point in the world coordinate + * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] + * @param Dpoint is the Jacobian w.r.t. point3 + */ + Point2 project2(const Point3& pw, + OptionalJacobian<2, 6> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + + const Point3 pc = Base::pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + + if (!Dcamera && !Dpoint) { + return calibration().uncalibrate(pn); + } else { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); + + if (Dcamera) + calculateDpose(pn, d, Dpi_pn, *Dcamera); + if (Dpoint) + calculateDpoint(pn, d, Base::pose().rotation().matrix(), Dpi_pn, + *Dpoint); + + return pi; + } + } + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = calibration().calibrate(p); + const Point3 pc(pn.x() * depth, pn.y() * depth, depth); + return Base::pose().transform_from(pc); + } + + /// backproject a 2-dimensional point to a 3-dimensional point at infinity + Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); + const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + return Base::pose().rotation().rotate(pc); + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpoint the optionally computed Jacobian with respect to the landmark + * @return range (double) + */ + double range( + const Point3& point, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return Base::pose().range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpose2 the optionally computed Jacobian with respect to the other pose + * @return range (double) + */ + double range( + const Pose3& pose, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return Base::pose().range(pose, Dcamera, Dpose); + } + + /** + * Calculate range to another camera + * @param camera Other camera + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + template + double range( + const PinholePose& camera, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return Base::pose().range(camera.pose(), Dcamera, Dother); + } + + /** + * Calculate range to another camera + * @param camera Other camera + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + double range( + const CalibratedCamera& camera, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return range(camera.pose(), Dcamera, Dother); + } + +private: + + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpose Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, + Eigen::MatrixBase const & Dpose) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + double uv = u * v, uu = u * u, vv = v * v; + Matrix26 Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + assert(Dpose.rows()==2 && Dpose.cols()==6); + const_cast&>(Dpose) = // + Dpi_pn * Dpn_pose; + } + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpoint Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, + const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 Dpn_point; + Dpn_point << // + R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // + /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + Dpn_point *= d; + assert(Dpoint.rows()==2 && Dpoint.cols()==3); + const_cast&>(Dpoint) = // + Dpi_pn * Dpn_point; + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } }; +// end of class PinholePose template struct traits > : public internal::Manifold<