From da8e88d5a153236b50bec155d2e5d140d4e67932 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 13:33:19 +0100 Subject: [PATCH] New class copied from PinholeCamera --- .cproject | 26 +- gtsam/geometry/PinholePose.h | 515 +++++++++++++++++++++++ gtsam/geometry/tests/testPinholePose.cpp | 313 ++++++++++++++ 3 files changed, 850 insertions(+), 4 deletions(-) create mode 100644 gtsam/geometry/PinholePose.h create mode 100644 gtsam/geometry/tests/testPinholePose.cpp 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/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h new file mode 100644 index 000000000..761949d96 --- /dev/null +++ b/gtsam/geometry/PinholePose.h @@ -0,0 +1,515 @@ +/* ---------------------------------------------------------------------------- + + * 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 Calibration. + * @addtogroup geometry + * \nosubgrouping + */ +template +class PinholePose { + +private: + Pose3 pose_; + Calibration K_; + + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + + // Get dimensions of calibration type at compile time + static const int DimK = FixedDimension::value; + +public: + + enum { dimension = 6 + DimK }; + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholePose() { + } + + /** constructor with pose */ + explicit PinholePose(const Pose3& pose) : + pose_(pose) { + } + + /** constructor with pose and calibration */ + PinholePose(const Pose3& pose, const Calibration& 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 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 PinholePose(pose3, K); + } + + /// PinholePose::level with default calibration + static PinholePose Level(const Pose2& pose2, double height) { + return PinholePose::Level(Calibration(), 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 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 PinholePose(pose3, K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholePose(const Vector &v) { + pose_ = Pose3::Expmap(v.head(6)); + if (v.size() > 6) { + K_ = Calibration(v.tail(DimK)); + } + } + + PinholePose(const Vector &v, const Vector &K) : + pose_(Pose3::Expmap(v)), K_(K) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const PinholePose &camera, double tol = 1e-9) const { + return pose_.equals(camera.pose(), tol) + && K_.equals(camera.calibration(), tol); + } + + /// print + void print(const std::string& s = "PinholePose") const { + pose_.print(s + ".pose"); + K_.print(s + ".calibration"); + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholePose() { + } + + /// return pose + inline Pose3& pose() { + return pose_; + } + + /// return pose, constant version + inline const Pose3& pose() const { + return pose_; + } + + /// return pose, with derivative + inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; + } + return pose_; + } + + /// return calibration + inline Calibration& calibration() { + return K_; + } + + /// return calibration + inline const Calibration& calibration() const { + return K_; + } + + /// @} + /// @name Manifold + /// @{ + + /// Manifold dimension + inline size_t dim() const { + return dimension; + } + + /// Manifold dimension + inline static size_t Dim() { + return dimension; + } + + typedef Eigen::Matrix VectorK6; + + /// move a cameras according to d + PinholePose retract(const Vector& d) const { + if ((size_t) d.size() == 6) + return PinholePose(pose().retract(d), calibration()); + else + return PinholePose(pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); + } + + /// return canonical coordinate + VectorK6 localCoordinates(const PinholePose& T2) const { + VectorK6 d; + // TODO: why does d.head<6>() not compile?? + d.head(6) = pose().localCoordinates(T2.pose()); + d.tail(DimK) = calibration().localCoordinates(T2.calibration()); + return d; + } + + /// 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 + 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 + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @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, + OptionalJacobian<2, DimK> Dcal = boost::none) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + if (Dpose || Dpoint) { + const double z = pc.z(), d = 1.0 / z; + + // 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); + } + + /** project a point at infinity from world coordinate to the image + * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) + * @param Dpose is the Jacobian w.r.t. pose3 + * @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, + 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 + return K_.uncalibrate(pn); + } + + // world to camera coordinate + Matrix3 Dpc_rot, Dpc_point; + const Point3 pc = 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); + + // uncalibration + 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; + if (Dpoint) + *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); + + if (!Dcamera && !Dpoint) { + return K_.uncalibrate(pn); + } else { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2K Dcal; + Matrix2 Dpi_pn; + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + 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); + } + + /** + * 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 { + Matrix16 Dpose_; + double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + if (Dcamera) + *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 { + Matrix16 Dpose_; + double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + if (Dcamera) + *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 PinholePose& camera, // + OptionalJacobian<1, dimension> Dcamera = boost::none, +// OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::optional Dother = + boost::none) const { + Matrix16 Dcamera_, Dother_; + double result = 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->setZero(); + Dother->block(0, 0, 1, 6) = Dother_; + } + 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) + */ + double range( + const CalibratedCamera& camera, // + OptionalJacobian<1, dimension> 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_NVP(pose_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + +}; + + +template +struct traits< PinholePose > : public internal::Manifold > {}; + +template +struct traits< const PinholePose > : public internal::Manifold > {}; + +} // \ gtsam diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp new file mode 100644 index 000000000..271dcf5f9 --- /dev/null +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -0,0 +1,313 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +using namespace std; +using namespace gtsam; + +typedef PinholePose Camera; + +static const Cal3_S2 K(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( K, camera.calibration())); + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholePose, 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, level2) +{ + // Create a level camera, looking in Y-direction + Pose2 pose2(0.4,0.3,M_PI/2.0); + Camera camera = Camera::Level(K, pose2, 0.1); + + // expected + Point3 x(1,0,0),y(0,0,-1),z(0,1,0); + Rot3 wRc(x,y,z); + Pose3 expected(wRc,Point3(0.4,0.3,0.1)); + EXPECT(assert_equal( camera.pose(), expected)); +} + +/* ************************************************************************* */ +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.project(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project(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); +} + +/* ************************************************************************* */ +TEST( PinholePose, backprojectInfinity2) +{ + 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.backprojectPointAtInfinity(Point2()); + Point3 expected(0., 1., 0.); + Point2 x = camera.projectPointAtInfinity(expected); + + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); +} + +/* ************************************************************************* */ +TEST( PinholePose, backprojectInfinity3) +{ + Point3 origin; + Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity + Camera camera(Pose3(rot, origin), K); + + Point3 actual = camera.backprojectPointAtInfinity(Point2()); + Point3 expected(0., 0., 1.); + Point2 x = camera.projectPointAtInfinity(expected); + + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); +} + +/* ************************************************************************* */ +static Point2 project3(const Pose3& pose, const Point3& point, const Cal3_S2& cal) { + return Camera(pose,cal).project(point); +} + +/* ************************************************************************* */ +TEST( PinholePose, Dproject) +{ + Matrix Dpose, Dpoint, Dcal; + Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix numerical_cal = numericalDerivative33(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)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); +} + +/* ************************************************************************* */ +static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).projectPointAtInfinity(point3D); +} + +TEST( PinholePose, Dproject_Infinity) +{ + Matrix Dpose, Dpoint, Dcal; + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 + + // test Projection + Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Point2 expected(-5.0, 5.0); + EXPECT(assert_equal(actual, expected, 1e-7)); + + // test Jacobians + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K); + Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K); + Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 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)); +} + +/* ************************************************************************* */ +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 Cal3Bundler K2(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); } +/* ************************************************************************* */ + +