diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index a298dafa4..996313c5c 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,32 +18,32 @@ #pragma once -#include -#include -#include +#include namespace gtsam { /** * A pinhole camera class that has a Pose3 and a Calibration. + * Use PinholePose if you will not be optimizing for Calibration * @addtogroup geometry * \nosubgrouping */ template -class PinholeCamera { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { private: - Pose3 pose_; - Calibration K_; - GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + Calibration K_; ///< Calibration, part of class now // Get dimensions of calibration type at compile time -static const int DimK = FixedDimension::value; + static const int DimK = FixedDimension::value; public: - enum {dimension = 6 + DimK}; + enum { + dimension = 6 + DimK + }; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ @@ -54,12 +54,12 @@ public: /** constructor with pose */ explicit PinholeCamera(const Pose3& pose) : - pose_(pose) { + Base(pose) { } /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& K) : - pose_(pose), K_(K) { + Base(pose), K_(K) { } /// @} @@ -75,12 +75,7 @@ public: */ static PinholeCamera Level(const Calibration &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 PinholeCamera(pose3, K); + return PinholeCamera(CalibratedCamera::LevelPose(pose2, height), K); } /// PinholeCamera::level with default calibration @@ -99,28 +94,23 @@ public: */ static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { - 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 PinholeCamera(pose3, K); + return PinholeCamera(CalibratedCamera::LookatPose(eye, target, upVector), K); } /// @} /// @name Advanced Constructors /// @{ - explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(6)); - if (v.size() > 6) { - K_ = Calibration(v.tail(DimK)); - } + /// Init from vector, can be 6D (default calibration) or dim + explicit PinholeCamera(const Vector &v) : + Base(v.head<6>()) { + if (v.size() > 6) + K_ = Calibration(v.tail()); } + /// Init from Vector and calibration PinholeCamera(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + Base(v), K_(K) { } /// @} @@ -128,14 +118,14 @@ public: /// @{ /// assert equality up to a tolerance - bool equals(const PinholeCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) - && K_.equals(camera.calibration(), tol); + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholeCamera* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_.equals(e->calibration(), tol); } /// print void print(const std::string& s = "PinholeCamera") const { - pose_.print(s + ".pose"); + Base::print(s); K_.print(s + ".calibration"); } @@ -147,13 +137,8 @@ public: } /// return pose - Pose3& pose() { - return pose_; - } - - /// return pose, constant version const Pose3& pose() const { - return pose_; + return Base::pose(); } /// return pose, with derivative @@ -162,12 +147,7 @@ public: H->setZero(); H->block(0, 0, 6, 6) = I_6x6; } - return pose_; - } - - /// return calibration - Calibration& calibration() { - return K_; + return Base::pose(); } /// return calibration @@ -179,12 +159,12 @@ public: /// @name Manifold /// @{ - /// Manifold dimension + /// @deprecated size_t dim() const { return dimension; } - /// Manifold dimension + /// @deprecated static size_t Dim() { return dimension; } @@ -194,17 +174,17 @@ public: /// move a cameras according to d PinholeCamera retract(const Vector& d) const { if ((size_t) d.size() == 6) - return PinholeCamera(pose().retract(d), calibration()); + return PinholeCamera(this->pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(6)), - calibration().retract(d.tail(calibration().dim()))); + return PinholeCamera(this->pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); } /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 d; // TODO: why does d.head<6>() not compile?? - d.head(6) = pose().localCoordinates(T2.pose()); + d.head(6) = this->pose().localCoordinates(T2.pose()); d.tail(DimK) = calibration().localCoordinates(T2.calibration()); return d; } @@ -218,32 +198,6 @@ public: /// @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 = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); - } - typedef Eigen::Matrix Matrix2K; /** project a point from world coordinate to the image @@ -252,31 +206,25 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcal, + Dpose || Dpoint ? &Dpi_pn : 0); - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; + // If needed, apply chain rule + if (Dpose) + *Dpose = Dpi_pn * *Dpose; + if (Dpoint) + *Dpoint = Dpi_pn * *Dpoint; - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) - calculateDpose(pn, d, Dpi_pn, *Dpose); - if (Dpoint) - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - return pi; - } else - return K_.uncalibrate(pn, Dcal); + return pi; } /** project a point at infinity from world coordinate to the image @@ -285,149 +233,114 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - Point2 projectPointAtInfinity(const Point3& pw, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none, + Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = project_to_camera(pc);// project the point to the camera + const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) + const Point2 pn = Base::project_to_camera(pc); // project the point to the camera return K_.uncalibrate(pn); } // world to camera coordinate Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); + const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); Matrix36 Dpc_pose; Dpc_pose.setZero(); Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate - Matrix23 Dpn_pc;// 2*3 - const Point2 pn = project_to_camera(pc, Dpn_pc); + Matrix23 Dpn_pc; // 2*3 + const Point2 pn = Base::project_to_camera(pc, Dpn_pc); // uncalibration - Matrix2 Dpi_pn;// 2*2 + Matrix2 Dpi_pn; // 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; + *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) return pi; } /** 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, dimension> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); + // project to normalized coordinates + Matrix26 Dpose; + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; + // uncalibrate to pixel coordinates + Matrix2K Dcal; + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0, + Dcamera || Dpoint ? &Dpi_pn : 0); - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + // If needed, calculate derivatives + if (Dcamera) + *Dcamera << Dpi_pn * Dpose, Dcal; + if (Dpoint) + *Dpoint = Dpi_pn * (*Dpoint); - if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); - (*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib - } - if (Dpoint) { - calculateDpoint(pn, d, 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 = K_.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 - Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 - return pose_.rotation().rotate(pc); + return pi; } /** * 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, dimension> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * 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, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * 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 PinholeCamera& camera, // + double range(const PinholeCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, -// OptionalJacobian<1, 6 + traits::dimension::value> Dother = - boost::optional Dother = - boost::none) const { + boost::optional Dother = boost::none) const { Matrix16 Dcamera_, Dother_; - double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, Dother ? &Dother_ : 0); if (Dcamera) { Dcamera->resize(1, 6 + DimK); *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - Dother->resize(1, 6+CalibrationB::dimension); + Dother->resize(1, 6 + CalibrationB::dimension); Dother->setZero(); Dother->block(0, 0, 1, 6) = Dother_; } @@ -437,12 +350,9 @@ public: /** * 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, // + double range(const CalibratedCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); @@ -450,55 +360,13 @@ public: 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 <&>(Dpoint) =// - Dpi_pn * Dpn_point; - } - /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 72e816852..f1e698fc1 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include