diff --git a/.cproject b/.cproject index ab1d0cdfc..8eb74b58b 100644 --- a/.cproject +++ b/.cproject @@ -1019,6 +1019,14 @@ true true + + make + -j4 + testPinholePose.run + true + true + true + make -j2 @@ -1285,7 +1293,6 @@ make - testSimulated2DOriented.run true false @@ -1325,7 +1332,6 @@ make - testSimulated2D.run true false @@ -1333,7 +1339,6 @@ make - testSimulated3D.run true false @@ -1437,6 +1442,7 @@ make + testErrors.run true false @@ -1739,6 +1745,7 @@ cpack + -G DEB true false @@ -1746,6 +1753,7 @@ cpack + -G RPM true false @@ -1753,6 +1761,7 @@ cpack + -G TGZ true false @@ -1760,6 +1769,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1951,7 +1961,6 @@ make - tests/testGaussianISAM2 true false @@ -2103,6 +2112,7 @@ make + tests/testBayesTree.run true false @@ -2110,6 +2120,7 @@ make + testBinaryBayesNet.run true false @@ -2157,6 +2168,7 @@ make + testSymbolicBayesNet.run true false @@ -2164,6 +2176,7 @@ make + tests/testSymbolicFactor.run true false @@ -2171,6 +2184,7 @@ make + testSymbolicFactorGraph.run true false @@ -2186,6 +2200,7 @@ make + tests/testBayesTree true false @@ -3305,6 +3320,7 @@ make + testGraph.run true false @@ -3312,6 +3328,7 @@ make + testJunctionTree.run true false @@ -3319,6 +3336,7 @@ make + testSymbolicBayesNetB.run true false diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 564930d74..35f9884e1 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 9507797eb..b83dfa1db 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace std; diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9319a1541..cd56780e5 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -462,15 +462,17 @@ namespace gtsam { // cannot just create a root Choice node on the label: if the label is not the // highest label, we need to do a complicated and expensive recursive call. template template - typename DecisionTree::NodePtr DecisionTree::compose( - Iterator begin, Iterator end, const L& label) const { + typename DecisionTree::NodePtr DecisionTree::compose(Iterator begin, + Iterator end, const L& label) const { // find highest label among branches boost::optional highestLabel; boost::optional nrChoices; for (Iterator it = begin; it != end; it++) { - if (it->root_->isLeaf()) continue; - boost::shared_ptr c = boost::dynamic_pointer_cast (it->root_); + if (it->root_->isLeaf()) + continue; + boost::shared_ptr c = + boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel.reset(c->label()); nrChoices.reset(c->nrChoices()); @@ -478,30 +480,31 @@ namespace gtsam { } // if label is already in correct order, just put together a choice on label - if (!highestLabel || label > *highestLabel) { + if (!highestLabel || !nrChoices || label > *highestLabel) { boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); - } - - // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices)); - // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { - // make a new set of functions for composing by iterating over the given - // functions, and selecting the appropriate branch. - std::vector functions; - for (Iterator it = begin; it != end; it++) { - // by restricting the input functions to value i for labelBelow - DecisionTree chosen = it->choose(*highestLabel, index); - functions.push_back(chosen); + } else { + // Set up a new choice on the highest label + boost::shared_ptr choiceOnHighestLabel( + new Choice(*highestLabel, *nrChoices)); + // now, for all possible values of highestLabel + for (size_t index = 0; index < *nrChoices; index++) { + // make a new set of functions for composing by iterating over the given + // functions, and selecting the appropriate branch. + std::vector functions; + for (Iterator it = begin; it != end; it++) { + // by restricting the input functions to value i for labelBelow + DecisionTree chosen = it->choose(*highestLabel, index); + functions.push_back(chosen); + } + // We then recurse, for all values of the highest label + NodePtr fi = compose(functions.begin(), functions.end(), label); + choiceOnHighestLabel->push_back(fi); } - // We then recurse, for all values of the highest label - NodePtr fi = compose(functions.begin(), functions.end(), label); - choiceOnHighestLabel->push_back(fi); + return Choice::Unique(choiceOnHighestLabel); } - return Choice::Unique(choiceOnHighestLabel); } /*********************************************************************************/ @@ -667,9 +670,10 @@ namespace gtsam { void DecisionTree::dot(const std::string& name, bool showZero) const { std::ofstream os((name + ".dot").c_str()); dot(os, showZero); - system( + int result = system( ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str()); - } + if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed"); +} /*********************************************************************************/ diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 1f5f1f8a5..33f2c84eb 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -19,88 +19,131 @@ #include #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { +Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { + // 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; + return Dpn_pose; } /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Vector &v) : - pose_(Pose3::Expmap(v)) { +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 Dpn_point; + Dpn_point << // + Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), // + /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2); + Dpn_point *= d; + return Dpn_point; } /* ************************************************************************* */ -Point2 CalibratedCamera::project_to_camera(const Point3& P, - OptionalJacobian<2,3> H1) { - if (H1) { - double d = 1.0 / P.z(), d2 = d * d; - *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; +Pose3 PinholeBase::LevelPose(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); + return Pose3(wRc, t); +} + +/* ************************************************************************* */ +Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector) { + 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); + return Pose3(Rot3(xc, yc, zc), eye); +} + +/* ************************************************************************* */ +bool PinholeBase::equals(const PinholeBase &camera, double tol) const { + return pose_.equals(camera.pose(), tol); +} + +/* ************************************************************************* */ +void PinholeBase::print(const string& s) const { + pose_.print(s + ".pose"); +} + +/* ************************************************************************* */ +const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; } - return Point2(P.x() / P.z(), P.y() / P.z()); + return pose_; } /* ************************************************************************* */ -Point3 CalibratedCamera::backproject_from_camera(const Point2& p, - const double scale) { - return Point3(p.x() * scale, p.y() * scale, scale); +Point2 PinholeBase::project_to_camera(const Point3& pc, + OptionalJacobian<2, 3> Dpoint) { + double d = 1.0 / pc.z(); + const double u = pc.x() * d, v = pc.y() * d; + if (Dpoint) + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; + return Point2(u, v); +} + +/* ************************************************************************* */ +pair PinholeBase::projectSafe(const Point3& pw) const { + const Point3 pc = pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + return make_pair(pn, pc.z() > 0); +} + +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint) const { + + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw CheiralityException(); +#endif + const Point2 pn = project_to_camera(q); + + if (Dpose || Dpoint) { + const double d = 1.0 / q.z(); + if (Dpose) + *Dpose = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt); + } + return pn; +} + +/* ************************************************************************* */ +Point3 PinholeBase::backproject_from_camera(const Point2& p, + const double depth) { + return Point3(p.x() * depth, p.y() * depth, depth); } /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { - double st = sin(pose2.theta()), ct = cos(pose2.theta()); - Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - Rot3 wRc(x, y, z); - Point3 t(pose2.x(), pose2.y(), height); - Pose3 pose3(wRc, t); - return CalibratedCamera(pose3); + return CalibratedCamera(LevelPose(pose2, height)); +} + +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, + const Point3& target, const Point3& upVector) { + return CalibratedCamera(LookatPose(eye, target, upVector)); } /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const { - -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - Matrix36 Dpose_; - Matrix3 Dpoint_; - Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); -#else - Point3 q = pose_.transform_to(point); -#endif - Point2 intrinsic = project_to_camera(q); - - // Check if point is in front of camera - if (q.z() <= 0) - throw CheiralityException(); - - if (Dpose || Dpoint) { -#ifdef CALIBRATEDCAMERA_CHAIN_RULE - // just implement chain rule - if(Dpose && Dpoint) { - Matrix23 H; - project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose_); - if (Dpoint) *Dpoint = H * (*Dpoint_); - } -#else - // optimized version, see CalibratedCamera.nb - const double z = q.z(), d = 1.0 / z; - const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; - if (Dpose) - *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v; - if (Dpoint) { - const Matrix3 R(pose_.rotation().matrix()); - Matrix23 Dpoint_; - Dpoint_ << 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); - *Dpoint = d * Dpoint_; - } -#endif - } - return intrinsic; + OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { + return project2(point, Dcamera, Dpoint); } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9e907f1d5..ed0c55208 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,10 +19,11 @@ #pragma once #include -#include namespace gtsam { +class Point2; + class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -31,6 +32,146 @@ public: } }; +/** + * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT PinholeBase { + +private: + + Pose3 pose_; ///< 3D pose of camera + +protected: + + /// @name Derivatives + /// @{ + + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + */ + static Matrix26 Dpose(const Point2& pn, double d); + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Rt transposed rotation matrix + */ + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); + + /// @} + +public: + + /// @name Static functions + /// @{ + + /** + * Create a level pose 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 Pose3 LevelPose(const Pose2& pose2, double height); + + /** + * Create a camera pose 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 + */ + static Pose3 LookatPose(const Point3& eye, const Point3& target, + const Point3& upVector); + + /// @} + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeBase() { + } + + /** constructor with pose */ + explicit PinholeBase(const Pose3& pose) : + pose_(pose) { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeBase(const Vector &v) : + pose_(Pose3::Expmap(v)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const PinholeBase &camera, double tol = 1e-9) const; + + /// print + void print(const std::string& s = "PinholeBase") const; + + /// @} + /// @name Standard Interface + /// @{ + + /// return pose, constant version + const Pose3& pose() const { + return pose_; + } + + /// return pose, with derivative + const Pose3& getPose(OptionalJacobian<6, 6> H) const; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * Project from 3D point in camera coordinates into image + * Does *not* throw a CheiralityException, even if pc behind image plane + * @param pc point in camera coordinates + */ + static Point2 project_to_camera(const Point3& pc, // + OptionalJacobian<2, 3> Dpoint = boost::none); + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const; + + /** + * Project point into the image + * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION + * @param point 3D point in world coordinates + * @return the intrinsic coordinates of the projected point + */ + Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + static Point3 backproject_from_camera(const Point2& p, const double depth); + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(pose_); + } + +}; +// end of class PinholeBase + /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient @@ -38,13 +179,13 @@ public: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera { -private: - Pose3 pose_; // 6DOF pose +class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ @@ -54,26 +195,40 @@ public: } /// construct with pose - explicit CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose) : + PinholeBase(pose) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param pose2 specifies the location and viewing direction + * @param height specifies the height of the camera (along the positive Z-axis) + * (theta 0 = looking in direction of positive X axis) + */ + static CalibratedCamera Level(const Pose2& pose2, double 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 + */ + static CalibratedCamera Lookat(const Point3& eye, const Point3& target, + const Point3& upVector); /// @} /// @name Advanced Constructors /// @{ /// construct from vector - explicit CalibratedCamera(const Vector &v); - - /// @} - /// @name Testable - /// @{ - - virtual void print(const std::string& s = "") const { - pose_.print(s); - } - - /// check equality to another camera - bool equals(const CalibratedCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol); + explicit CalibratedCamera(const Vector &v) : + PinholeBase(v) { } /// @} @@ -84,19 +239,6 @@ public: virtual ~CalibratedCamera() { } - /// return pose - inline const Pose3& pose() const { - return pose_; - } - - /** - * Create a level camera at the given 2D pose and height - * @param pose2 specifies the location and viewing direction - * @param height specifies the height of the camera (along the positive Z-axis) - * (theta 0 = looking in direction of positive X axis) - */ - static CalibratedCamera Level(const Pose2& pose2, double height); - /// @} /// @name Manifold /// @{ @@ -107,87 +249,68 @@ public: /// Return canonical coordinate Vector localCoordinates(const CalibratedCamera& T2) const; - /// Lie group dimensionality + /// @deprecated inline size_t dim() const { return 6; } - /// Lie group dimensionality + /// @deprecated inline static size_t Dim() { return 6; } - /* ************************************************************************* */ - // measurement functions and derivatives - /* ************************************************************************* */ - /// @} /// @name Transformations and mesaurement functions /// @{ - /** - * This function receives the camera pose and the landmark location and - * returns the location the point is supposed to appear in the image - * @param point a 3D point to be projected - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the 3D point - * @return the intrinsic coordinates of the projected point - */ - Point2 project(const Point3& point, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * With optional 2by3 derivative + * @deprecated + * Use project2, which is more consistently named across Pinhole cameras */ - static Point2 project_to_camera(const Point3& cameraPoint, - OptionalJacobian<2, 3> H1 = boost::none); + Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; - /** - * backproject a 2-dimensional point to a 3-dimension point - */ - static Point3 backproject_from_camera(const Point2& p, const double scale); + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& pn, double depth) const { + return pose().transform_from(backproject_from_camera(pn, depth)); + } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const { - return pose_.range(point, H1, H2); + double range(const Point3& point, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) const { - return pose_.range(pose, H1, H2); + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); } /** * Calculate range to another camera * @param camera Other camera - * @param H1 optionally computed Jacobian with respect to pose - * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = - boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { - return pose_.range(camera.pose_, H1, H2); + double range(const CalibratedCamera& camera, // + OptionalJacobian<1, 6> H1 = boost::none, // + OptionalJacobian<1, 6> H2 = boost::none) const { + return pose().range(camera.pose(), H1, H2); } + /// @} + private: - /// @} /// @name Advanced Interface /// @{ @@ -195,17 +318,22 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); } /// @} }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold< + CalibratedCamera> { +}; } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 46df47ecb..1edb1a496 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; 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(Base::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(Base::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,31 +137,21 @@ public: } /// return pose - inline Pose3& pose() { - return pose_; - } - - /// return pose, constant version - inline const Pose3& pose() const { - return pose_; + const Pose3& pose() const { + return Base::pose(); } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + const Pose3& getPose(OptionalJacobian<6, dimension> H) const { if (H) { H->setZero(); H->block(0, 0, 6, 6) = I_6x6; } - return pose_; + return Base::pose(); } /// return calibration - inline Calibration& calibration() { - return K_; - } - - /// return calibration - inline const Calibration& calibration() const { + const Calibration& calibration() const { return K_; } @@ -179,13 +159,13 @@ public: /// @name Manifold /// @{ - /// Manifold dimension - inline size_t dim() const { + /// @deprecated + size_t dim() const { return dimension; } - /// Manifold dimension - inline static size_t Dim() { + /// @deprecated + static size_t Dim() { return dimension; } @@ -194,9 +174,9 @@ 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)), + return PinholeCamera(this->pose().retract(d.head(6)), calibration().retract(d.tail(calibration().dim()))); } @@ -204,7 +184,7 @@ public: 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 - inline 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 */ - inline 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,20 +233,19 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline 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(); @@ -306,7 +253,7 @@ public: // camera to normalized image coordinate Matrix23 Dpn_pc; // 2*3 - const Point2 pn = project_to_camera(pc, Dpn_pc); + const Point2 pn = Base::project_to_camera(pc, Dpn_pc); // uncalibration Matrix2 Dpi_pn; // 2*2 @@ -323,65 +270,40 @@ public: /** 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 - inline 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 - inline 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(); return result; @@ -390,16 +312,12 @@ public: /** * 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(); return result; @@ -408,26 +326,21 @@ 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) */ 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->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,65 +360,26 @@ 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 << // - 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_NVP(pose_); + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } }; +template +struct traits > : public internal::Manifold< + PinholeCamera > { +}; template -struct traits< PinholeCamera > : public internal::Manifold > {}; - -template -struct traits< const PinholeCamera > : public internal::Manifold > {}; +struct traits > : public internal::Manifold< + PinholeCamera > { +}; } // \ gtsam diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h new file mode 100644 index 000000000..7588f517e --- /dev/null +++ b/gtsam/geometry/PinholePose.h @@ -0,0 +1,344 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PinholePose.h + * @brief Pinhole camera with known calibration + * @author Yong-Dian Jian + * @author Frank Dellaert + * @date Feb 20, 2015 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * @addtogroup geometry + * \nosubgrouping + */ +template +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { + + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + +public : + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholeBaseK() { + } + + /** constructor with pose */ + explicit PinholeBaseK(const Pose3& pose) : + PinholeBase(pose) { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholeBaseK(const Vector &v) : + PinholeBase(v) { + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholeBaseK() { + } + + /// return calibration + virtual const Calibration& calibration() const = 0; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const { + std::pair pn = PinholeBase::projectSafe(pw); + pn.first = calibration().uncalibrate(pn.first); + return pn; + } + + /** project a point from world coordinate to the image, fixed Jacobians + * @param pw is a point in the world coordinates + */ + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); + + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, boost::none, + Dpose || Dpoint ? &Dpi_pn : 0); + + // If needed, apply chain rule + if (Dpose) *Dpose = Dpi_pn * (*Dpose); + if (Dpoint) *Dpoint = 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); + return pose().transform_from(backproject_from_camera(pn, depth)); + } + + /// 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 pose().rotation().rotate(pc); + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); + } + + /** + * Calculate range to a CalibratedCamera + * @param camera Other camera + * @return range (double) + */ + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera = + boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + + /** + * Calculate range to a PinholePoseK derived class + * @param camera Other camera + * @return range (double) + */ + template + double range(const PinholeBaseK& camera, + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + +private: + + /** 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)); + } + +}; +// end of class PinholeBaseK + +/** + * 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 GTSAM_EXPORT PinholePose: public PinholeBaseK { + +private: + + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + boost::shared_ptr K_; ///< shared pointer to fixed calibration + +public: + + enum { + dimension = 6 + }; ///< There are 6 DOF to optimize for + + /// @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(Base::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(Base::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + /// Init from 6D vector + explicit PinholePose(const Vector &v) : + Base(v), K_(new Calibration()) { + } + + /// Init from Vector and 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 + virtual const Calibration& calibration() const { + return *K_; + } + + /// @} + /// @name Manifold + /// @{ + + /// @deprecated + size_t dim() const { + return 6; + } + + /// @deprecated + static size_t Dim() { + return 6; + } + + /// move a cameras according to d + PinholePose retract(const Vector6& d) const { + return PinholePose(Base::pose().retract(d), K_); + } + + /// return canonical coordinate + Vector6 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 + } + + /// @} + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(K_); + } + +}; +// end of class PinholePose + +template +struct traits > : public internal::Manifold< + PinholePose > { +}; + +template +struct traits > : public internal::Manifold< + PinholePose > { +}; + +} // \ gtsam diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 6a990e08e..b1e265266 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -29,6 +29,7 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) +// Camera situated at 0.5 meters high, looking down static const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., @@ -97,24 +98,37 @@ TEST( CalibratedCamera, Dproject_to_camera1) { } /* ************************************************************************* */ -static Point2 project2(const Pose3& pose, const Point3& point) { - return CalibratedCamera(pose).project(point); +static Point2 project2(const CalibratedCamera& camera, const Point3& point) { + return camera.project(point); } TEST( CalibratedCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; Point2 result = camera.project(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); - Matrix numerical_point = numericalDerivative22(project2, pose1, point1); + Matrix numerical_pose = numericalDerivative21(project2, camera, point1); + Matrix numerical_point = numericalDerivative22(project2, camera, point1); CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1))); CHECK(assert_equal(Point2(-.16, .16), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject_point_pose2) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose1); + Matrix Dpose, Dpoint; + camera.project(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project2, camera, point1); + Matrix numerical_point = numericalDerivative22(project2, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 20f7a3231..0e610d8d6 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,12 +15,14 @@ * @brief test PinholeCamera class */ -#include -#include -#include #include #include #include +#include +#include +#include + +#include #include #include @@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2) EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); } +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( PinholeCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp new file mode 100644 index 000000000..411273c1f --- /dev/null +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -0,0 +1,259 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testPinholePose.cpp + * @author Frank Dellaert + * @brief test PinholePose class + * @date Feb 20, 2015 + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PinholePose Camera; + +static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); + +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); + +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); + +static const Point3 point1(-0.08,-0.08, 0.0); +static const Point3 point2(-0.08, 0.08, 0.0); +static const Point3 point3( 0.08, 0.08, 0.0); +static const Point3 point4( 0.08,-0.08, 0.0); + +static const Point3 point1_inf(-0.16,-0.16, -1.0); +static const Point3 point2_inf(-0.16, 0.16, -1.0); +static const Point3 point3_inf( 0.16, 0.16, -1.0); +static const Point3 point4_inf( 0.16,-0.16, -1.0); + +/* ************************************************************************* */ +TEST( PinholePose, constructor) +{ + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholeCamera, Pose) { + + Matrix actualH; + EXPECT(assert_equal(pose, camera.getPose(actualH))); + + // Check derivative + boost::function f = // + boost::bind(&Camera::getPose,_1,boost::none); + Matrix numericalH = numericalDerivative11(f,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +/* ************************************************************************* */ +TEST( PinholePose, lookat) +{ + // Create a level camera, looking in Y-direction + Point3 C(10.0,0.0,0.0); + Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + + // expected + Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); + Pose3 expected(Rot3(xc,yc,zc),C); + EXPECT(assert_equal( camera.pose(), expected)); + + Point3 C2(30.0,0.0,10.0); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + + Matrix R = camera2.pose().rotation().matrix(); + Matrix I = trans(R)*R; + EXPECT(assert_equal(I, eye(3))); +} + +/* ************************************************************************* */ +TEST( PinholePose, project) +{ + EXPECT(assert_equal( camera.project2(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project2(point4), Point2( 100, 100) )); +} + +/* ************************************************************************* */ +TEST( PinholePose, backproject) +{ + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backprojectInfinity) +{ + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backproject2) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backproject(Point2(), 1.); + Point3 expected(0., 1., 0.); + pair x = camera.projectSafe(expected); + + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); +} + +/* ************************************************************************* */ +static Point2 project3(const Pose3& pose, const Point3& point, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project2(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject) +{ + Matrix Dpose, Dpoint; + Point2 result = camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 project4(const Camera& camera, const Point3& point) { + return camera.project2(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject2) +{ + Matrix Dcamera, Dpoint; + Point2 result = camera.project2(point1, Dcamera, Dpoint); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +// Add a test with more arbitrary rotation +TEST( CalibratedCamera, Dproject3) +{ + static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Camera camera(pose1); + Matrix Dpose, Dpoint; + camera.project2(point1, Dpose, Dpoint); + Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix numerical_point = numericalDerivative22(project4, camera, point1); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, range0) { + Matrix D1; Matrix D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholePose, range1) { + Matrix D1; Matrix D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholePose Camera2; +static const boost::shared_ptr K2 = + boost::make_shared(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholePose, range2) { + Matrix D1; Matrix D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholePose, range3) { + Matrix D1; Matrix D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index 84fe5980e..dfef0a9c5 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -16,8 +16,6 @@ * @date Feb 7, 2012 */ -#include -#include #include #include #include diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 002cbe51b..71979481c 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -15,13 +15,13 @@ * @brief test SimpleCamera class */ -#include -#include - -#include +#include +#include #include #include -#include +#include +#include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 2bdc04d5b..6dc3e3d2f 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index f7314c73f..b819993ef 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include #include #include diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index d6a5ffa8c..0406c3d27 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -15,16 +15,16 @@ * @author Richard Roberts, Luca Carlone */ -#include - -#include - -#include -#include -#include #include #include +#include +#include +#include + +#include + +#include using namespace gtsam::symbol_shorthand; using namespace std; diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 2d1793417..7db71d8ce 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index f6180fb73..ef0456146 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -17,9 +17,8 @@ * @brief unit tests for Block Automatic Differentiation */ -#include #include -#include +#include #include #include #include