From da8e88d5a153236b50bec155d2e5d140d4e67932 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 13:33:19 +0100 Subject: [PATCH 01/36] 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); } +/* ************************************************************************* */ + + From 1a6102a7a5b49c4a3515701d59919c62bf089c0d Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 17:39:33 +0100 Subject: [PATCH 02/36] Calibration became fixed --- gtsam/geometry/PinholePose.h | 205 +++++------------------ gtsam/geometry/tests/testPinholePose.cpp | 80 ++------- 2 files changed, 56 insertions(+), 229 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 761949d96..7eaed2d45 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -21,12 +21,13 @@ #include #include +#include #include namespace gtsam { /** - * A pinhole camera class that has a Pose3 and a Calibration. + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. * @addtogroup geometry * \nosubgrouping */ @@ -35,16 +36,11 @@ 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; + boost::shared_ptr K_; public: - enum { dimension = 6 + DimK }; + enum { dimension = 6 }; /// @name Standard Constructors /// @{ @@ -55,11 +51,11 @@ public: /** constructor with pose */ explicit PinholePose(const Pose3& pose) : - pose_(pose) { + pose_(pose), K_(new Calibration()) { } /** constructor with pose and calibration */ - PinholePose(const Pose3& pose, const Calibration& K) : + PinholePose(const Pose3& pose, const boost::shared_ptr& K) : pose_(pose), K_(K) { } @@ -74,7 +70,7 @@ public: * (theta 0 = looking in direction of positive X axis) * @param height camera height */ - static PinholePose Level(const Calibration &K, const Pose2& pose2, + static PinholePose Level(const boost::shared_ptr& K, const Pose2& pose2, double height) { const double st = sin(pose2.theta()), ct = cos(pose2.theta()); const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); @@ -86,7 +82,7 @@ public: /// PinholePose::level with default calibration static PinholePose Level(const Pose2& pose2, double height) { - return PinholePose::Level(Calibration(), pose2, height); + return PinholePose::Level(boost::make_shared(), pose2, height); } /** @@ -99,7 +95,8 @@ public: * @param K optional calibration parameter */ static PinholePose Lookat(const Point3& eye, const Point3& target, - const Point3& upVector, const Calibration& K = Calibration()) { + const Point3& upVector, const boost::shared_ptr& K = + boost::make_shared()) { Point3 zc = target - eye; zc = zc / zc.norm(); Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down @@ -114,10 +111,7 @@ public: /// @{ explicit PinholePose(const Vector &v) { - pose_ = Pose3::Expmap(v.head(6)); - if (v.size() > 6) { - K_ = Calibration(v.tail(DimK)); - } + pose_ = Pose3::Expmap(v); } PinholePose(const Vector &v, const Vector &K) : @@ -130,14 +124,13 @@ public: /// 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); + return pose_.equals(camera.pose(), tol); } /// print void print(const std::string& s = "PinholePose") const { pose_.print(s + ".pose"); - K_.print(s + ".calibration"); + K_->print(s + ".calibration"); } /// @} @@ -158,7 +151,7 @@ public: } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + inline const Pose3& getPose(gtsam::OptionalJacobian<6, 6> H) const { if (H) { H->setZero(); H->block(0, 0, 6, 6) = I_6x6; @@ -167,12 +160,12 @@ public: } /// return calibration - inline Calibration& calibration() { + inline boost::shared_ptr calibration() { return K_; } /// return calibration - inline const Calibration& calibration() const { + inline const boost::shared_ptr calibration() const { return K_; } @@ -180,34 +173,26 @@ public: /// @name Manifold /// @{ - /// Manifold dimension + /// Manifold 6 inline size_t dim() const { - return dimension; + return 6; } - /// Manifold dimension + /// Manifold 6 inline static size_t Dim() { - return dimension; + return 6; } - typedef Eigen::Matrix VectorK6; + 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()))); + PinholePose retract(const Vector6& d) const { + return PinholePose(pose().retract(d), calibration()); } /// 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; + VectorK6 localCoordinates(const PinholePose& p) const { + return pose().localCoordinates(p.pose()); } /// for Canonical @@ -242,84 +227,7 @@ public: 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; + return std::make_pair(K_->uncalibrate(pn), pc.z() > 0); } /** project a point from world coordinate to the image, fixed Jacobians @@ -328,44 +236,41 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 */ Point2 project2( - const Point3& pw, // - OptionalJacobian<2, dimension> Dcamera = boost::none, + const Point3& pw, + OptionalJacobian<2, 6> 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); + 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); + const Point2 pi = K_->uncalibrate(pn, boost::none, 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) { + if (Dcamera) + calculateDpose(pn, d, Dpi_pn, *Dcamera); + 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 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 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); } @@ -379,13 +284,9 @@ public: */ double range( const Point3& point, // - OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 6> 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; + return pose_.range(point, Dcamera, Dpoint); } /** @@ -397,13 +298,9 @@ public: */ double range( const Pose3& pose, // - OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 6> 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; + return pose_.range(pose, Dcamera, Dpose); } /** @@ -416,23 +313,9 @@ public: 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; + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return pose_.range(camera.pose(), Dcamera, Dother); } /** @@ -444,7 +327,7 @@ public: */ double range( const CalibratedCamera& camera, // - OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 271dcf5f9..682085b4e 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -31,7 +31,7 @@ using namespace gtsam; typedef PinholePose Camera; -static const Cal3_S2 K(625, 625, 0, 0, 0); +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); @@ -52,7 +52,6 @@ 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())); } @@ -106,10 +105,10 @@ TEST( PinholePose, lookat) /* ************************************************************************* */ 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) )); + 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) )); } /* ************************************************************************* */ @@ -147,77 +146,21 @@ TEST( PinholePose, backproject2) } /* ************************************************************************* */ -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); +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, Dcal; - Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); + 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); - 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)); } /* ************************************************************************* */ @@ -272,7 +215,8 @@ TEST( PinholePose, range1) { /* ************************************************************************* */ typedef PinholePose Camera2; -static const Cal3Bundler K2(625, 1e-3, 1e-3); +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); From f097ceef380c808af0e419de6f38a50a1e90a195 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 07:26:48 +0100 Subject: [PATCH 03/36] Header order --- gtsam/geometry/tests/testPinholeCamera.cpp | 7 ++++--- gtsam/geometry/tests/testPinholePose.cpp | 7 ++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 20f7a3231..a6825b41e 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,12 +15,13 @@ * @brief test PinholeCamera class */ -#include -#include -#include #include #include #include +#include +#include + +#include #include #include diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 682085b4e..8b733ab04 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -16,12 +16,13 @@ * @date Feb 20, 2015 */ -#include -#include -#include #include #include #include +#include +#include + +#include #include #include From 0498a4550bc2b094c7edec2d1ac9197b5fe4e71c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 07:29:08 +0100 Subject: [PATCH 04/36] Standard formatting --- gtsam/geometry/PinholeCamera.h | 73 ++++++++++++++++++---------------- gtsam/geometry/PinholePose.h | 24 ++++++----- 2 files changed, 52 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 46df47ecb..4d2c35dd0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -39,11 +39,11 @@ private: GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) // Get dimensions of calibration type at compile time - static const int DimK = FixedDimension::value; +static const int DimK = FixedDimension::value; public: - enum { dimension = 6 + DimK }; + enum {dimension = 6 + DimK}; /// @name Standard Constructors /// @{ @@ -54,12 +54,12 @@ public: /** constructor with pose */ explicit PinholeCamera(const Pose3& pose) : - pose_(pose) { + pose_(pose) { } /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& K) : - pose_(pose), K_(K) { + pose_(pose), K_(K) { } /// @} @@ -120,7 +120,7 @@ public: } PinholeCamera(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + pose_(Pose3::Expmap(v)), K_(K) { } /// @} @@ -130,7 +130,7 @@ 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); + && K_.equals(camera.calibration(), tol); } /// print @@ -194,10 +194,10 @@ 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(pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(6)), - calibration().retract(d.tail(calibration().dim()))); + return PinholeCamera(pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); } /// return canonical coordinate @@ -228,12 +228,12 @@ public: OptionalJacobian<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) - throw CheiralityException(); + 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; + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; return Point2(u, v); } @@ -271,12 +271,12 @@ public: // chain the Jacobian matrices if (Dpose) - calculateDpose(pn, d, Dpi_pn, *Dpose); + calculateDpose(pn, d, Dpi_pn, *Dpose); if (Dpoint) - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); return pi; } else - return K_.uncalibrate(pn, Dcal); + return K_.uncalibrate(pn, Dcal); } /** project a point at infinity from world coordinate to the image @@ -292,7 +292,7 @@ public: 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 Point2 pn = project_to_camera(pc);// project the point to the camera return K_.uncalibrate(pn); } @@ -305,19 +305,19 @@ public: Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate - Matrix23 Dpn_pc; // 2*3 + Matrix23 Dpn_pc;// 2*3 const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration - Matrix2 Dpi_pn; // 2*2 + Matrix2 Dpi_pn;// 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; + *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only) return pi; } @@ -346,7 +346,7 @@ public: if (Dcamera) { // TODO why does leftCols<6>() not compile ?? calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); - (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib + (*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib } if (Dpoint) { calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); @@ -383,7 +383,7 @@ public: Matrix16 Dpose_; double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } @@ -401,7 +401,7 @@ public: Matrix16 Dpose_; double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } @@ -417,13 +417,13 @@ public: 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, Dother ? &Dother_ : 0); if (Dcamera) { - Dcamera->resize(1, 6 + DimK); + Dcamera->resize(1, 6 + DimK); *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { @@ -467,8 +467,8 @@ private: 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; + const_cast&>(Dpose) =// + Dpi_pn * Dpn_pose; } /** @@ -485,13 +485,13 @@ private: // 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), // + Dpn_point <&>(Dpoint) = // - Dpi_pn * Dpn_point; + const_cast&>(Dpoint) =// + Dpi_pn * Dpn_point; } /** Serialization function */ @@ -504,11 +504,14 @@ private: }; +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 index 7eaed2d45..f36aec571 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -40,7 +40,9 @@ private: public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ @@ -70,8 +72,8 @@ public: * (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) { + static PinholePose Level(const boost::shared_ptr& K, + const Pose2& pose2, double height) { const double st = sin(pose2.theta()), ct = cos(pose2.theta()); const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); const Rot3 wRc(x, y, z); @@ -214,7 +216,7 @@ public: OptionalJacobian<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) - throw CheiralityException(); + throw CheiralityException(); #endif double d = 1.0 / P.z(); const double u = P.x() * d, v = P.y() * d; @@ -235,8 +237,7 @@ public: * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dpoint is the Jacobian w.r.t. point3 */ - Point2 project2( - const Point3& pw, + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { @@ -388,11 +389,14 @@ private: }; +template +struct traits > : public internal::Manifold< + PinholePose > { +}; template -struct traits< PinholePose > : public internal::Manifold > {}; - -template -struct traits< const PinholePose > : public internal::Manifold > {}; +struct traits > : public internal::Manifold< + PinholePose > { +}; } // \ gtsam From 109e538ce68d3ba4d8b34a6ef6a8d3baf67c2066 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:24:09 +0100 Subject: [PATCH 05/36] Added two static functions and Lookat named constructor. Will be called in Pinhole* classes to avoid copy/paste --- gtsam/geometry/CalibratedCamera.cpp | 49 ++++++++++++++++------- gtsam/geometry/CalibratedCamera.h | 62 ++++++++++++++++++++++++----- 2 files changed, 86 insertions(+), 25 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 1f5f1f8a5..f48312e47 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) : /* ************************************************************************* */ Point2 CalibratedCamera::project_to_camera(const Point3& P, - OptionalJacobian<2,3> H1) { + 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; + *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; } return Point2(P.x() / P.z(), P.y() / P.z()); } @@ -47,19 +47,40 @@ Point3 CalibratedCamera::backproject_from_camera(const Point2& p, return Point3(p.x() * scale, p.y() * scale, scale); } +/* ************************************************************************* */ +Pose3 CalibratedCamera::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); +} + /* ************************************************************************* */ 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)); +} + +/* ************************************************************************* */ +Pose3 CalibratedCamera::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); +} + +/* ************************************************************************* */ +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 { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { #ifdef CALIBRATEDCAMERA_CHAIN_RULE Matrix36 Dpose_; @@ -88,14 +109,14 @@ Point2 CalibratedCamera::project(const Point3& point, 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; + *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_ << 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 diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9e907f1d5..d0ed16d96 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -44,7 +44,9 @@ private: public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ @@ -56,6 +58,49 @@ public: /// construct with pose explicit CalibratedCamera(const Pose3& pose); + /// @} + /// @name Named Constructors + /// @{ + + /** + * 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 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 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); + + /** + * 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 /// @{ @@ -89,14 +134,6 @@ public: 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 /// @{ @@ -202,10 +239,13 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold< + CalibratedCamera> { +}; } From eccb0663f3d44f5eace4c6efaed1df069a136e1d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:25:59 +0100 Subject: [PATCH 06/36] Forward declare of Point2 --- gtsam/geometry/CalibratedCamera.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index d0ed16d96..d17c61ba7 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: From 14ea858e3f6bd7090114b126a2c27ecab153ca41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:28:00 +0100 Subject: [PATCH 07/36] getPose -> pose --- gtsam/geometry/PinholeCamera.h | 24 +++++++++++----------- gtsam/geometry/tests/testPinholeCamera.cpp | 4 ++-- gtsam/geometry/tests/testPinholePose.cpp | 4 ++-- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 4d2c35dd0..a298dafa4 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -147,17 +147,17 @@ public: } /// return pose - inline Pose3& pose() { + Pose3& pose() { return pose_; } /// return pose, constant version - inline const Pose3& pose() const { + const Pose3& pose() const { return pose_; } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + const Pose3& pose(OptionalJacobian<6, dimension> H) const { if (H) { H->setZero(); H->block(0, 0, 6, 6) = I_6x6; @@ -166,12 +166,12 @@ public: } /// return calibration - inline Calibration& calibration() { + Calibration& calibration() { return K_; } /// return calibration - inline const Calibration& calibration() const { + const Calibration& calibration() const { return K_; } @@ -180,12 +180,12 @@ public: /// @{ /// Manifold dimension - inline size_t dim() const { + size_t dim() const { return dimension; } /// Manifold dimension - inline static size_t Dim() { + static size_t Dim() { return dimension; } @@ -238,7 +238,7 @@ public: } /// Project a point into the image and check depth - inline std::pair projectSafe(const Point3& pw) const { + std::pair projectSafe(const Point3& pw) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); @@ -252,7 +252,7 @@ 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 = + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { @@ -285,7 +285,7 @@ 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, + Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { @@ -356,14 +356,14 @@ public: } /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject(const Point2& p, double depth) const { + 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 { + 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); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index a6825b41e..72e816852 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -60,11 +60,11 @@ TEST( PinholeCamera, constructor) TEST(PinholeCamera, Pose) { Matrix actualH; - EXPECT(assert_equal(pose, camera.getPose(actualH))); + EXPECT(assert_equal(pose, camera.pose(actualH))); // Check derivative boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + boost::bind(&Camera::pose,_1,boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 8b733ab04..0d2f33890 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -60,11 +60,11 @@ TEST( PinholePose, constructor) TEST(PinholePose, Pose) { Matrix actualH; - EXPECT(assert_equal(pose, camera.getPose(actualH))); + EXPECT(assert_equal(pose, camera.pose(actualH))); // Check derivative boost::function f = // - boost::bind(&Camera::getPose,_1,boost::none); + boost::bind(&Camera::pose,_1,boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } From c20eaecf8211ee70c9326b890da3a4959b406a0a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:28:17 +0100 Subject: [PATCH 08/36] PinholeBase class --- gtsam/geometry/PinholePose.h | 484 ++++++++++++++++++++++++++--------- 1 file changed, 358 insertions(+), 126 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index f36aec571..d7c099ab3 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -32,92 +32,32 @@ namespace gtsam { * \nosubgrouping */ template -class PinholePose { +class PinholeBase { private: + Pose3 pose_; - boost::shared_ptr K_; public: - enum { - dimension = 6 - }; - /// @name Standard Constructors /// @{ /** default constructor */ - PinholePose() { + PinholeBase() { } /** constructor with pose */ - explicit PinholePose(const Pose3& pose) : - pose_(pose), K_(new Calibration()) { - } - - /** constructor with pose and calibration */ - PinholePose(const Pose3& pose, const boost::shared_ptr& K) : - pose_(pose), K_(K) { - } - - /// @} - /// @name Named Constructors - /// @{ - - /** - * Create a level camera at the given 2D pose and height - * @param K the calibration - * @param pose2 specifies the location and viewing direction - * (theta 0 = looking in direction of positive X axis) - * @param height camera height - */ - static PinholePose Level(const boost::shared_ptr& K, - const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - const Pose3 pose3(wRc, t); - return PinholePose(pose3, K); - } - - /// PinholePose::level with default calibration - static PinholePose Level(const Pose2& pose2, double height) { - return PinholePose::Level(boost::make_shared(), pose2, height); - } - - /** - * Create a camera at the given eye position looking at a target point in the scene - * with the specified up direction vector. - * @param eye specifies the camera position - * @param target the point to look at - * @param upVector specifies the camera up direction vector, - * doesn't need to be on the image plane nor orthogonal to the viewing axis - * @param K optional calibration parameter - */ - static PinholePose Lookat(const Point3& eye, const Point3& target, - const Point3& upVector, const boost::shared_ptr& K = - boost::make_shared()) { - Point3 zc = target - eye; - zc = zc / zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc / xc.norm(); - Point3 yc = zc.cross(xc); - Pose3 pose3(Rot3(xc, yc, zc), eye); - return PinholePose(pose3, K); + explicit PinholeBase(const Pose3& pose) : + pose_(pose) { } /// @} /// @name Advanced Constructors /// @{ - explicit PinholePose(const Vector &v) { - pose_ = Pose3::Expmap(v); - } - - PinholePose(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + explicit PinholeBase(const Vector &v) : + pose_(Pose3::Expmap(v)) { } /// @} @@ -125,35 +65,29 @@ public: /// @{ /// assert equality up to a tolerance - bool equals(const PinholePose &camera, double tol = 1e-9) const { + bool equals(const PinholeBase &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol); } /// print - void print(const std::string& s = "PinholePose") const { + void print(const std::string& s = "PinholeBase") const { pose_.print(s + ".pose"); - K_->print(s + ".calibration"); } /// @} /// @name Standard Interface /// @{ - virtual ~PinholePose() { - } - - /// return pose - inline Pose3& pose() { - return pose_; + virtual ~PinholeBase() { } /// return pose, constant version - inline const Pose3& pose() const { + const Pose3& pose() const { return pose_; } /// return pose, with derivative - inline const Pose3& getPose(gtsam::OptionalJacobian<6, 6> H) const { + const Pose3& pose(OptionalJacobian<6, 6> H) const { if (H) { H->setZero(); H->block(0, 0, 6, 6) = I_6x6; @@ -162,45 +96,7 @@ public: } /// return calibration - inline boost::shared_ptr calibration() { - return K_; - } - - /// return calibration - inline const boost::shared_ptr calibration() const { - return K_; - } - - /// @} - /// @name Manifold - /// @{ - - /// Manifold 6 - inline size_t dim() const { - return 6; - } - - /// Manifold 6 - inline static size_t Dim() { - return 6; - } - - typedef Eigen::Matrix VectorK6; - - /// move a cameras according to d - PinholePose retract(const Vector6& d) const { - return PinholePose(pose().retract(d), calibration()); - } - - /// return canonical coordinate - VectorK6 localCoordinates(const PinholePose& p) const { - return pose().localCoordinates(p.pose()); - } - - /// for Canonical - static PinholePose identity() { - return PinholePose(); // assumes that the default constructor is valid - } + virtual const Calibration& calibration() const = 0; /// @} /// @name Transformations and measurement functions @@ -226,10 +122,10 @@ public: } /// Project a point into the image and check depth - inline std::pair projectSafe(const Point3& pw) const { + std::pair projectSafe(const Point3& pw) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); - return std::make_pair(K_->uncalibrate(pn), pc.z() > 0); + return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); } /** project a point from world coordinate to the image, fixed Jacobians @@ -245,13 +141,13 @@ public: const Point2 pn = project_to_camera(pc); if (!Dcamera && !Dpoint) { - return K_->uncalibrate(pn); + return calibration().uncalibrate(pn); } else { const double z = pc.z(), d = 1.0 / z; // uncalibration Matrix2 Dpi_pn; - const Point2 pi = K_->uncalibrate(pn, boost::none, Dpi_pn); + const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); if (Dcamera) calculateDpose(pn, d, Dpi_pn, *Dcamera); @@ -263,15 +159,15 @@ public: } /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = K_->calibrate(p); + Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = calibration().calibrate(p); const Point3 pc(pn.x() * depth, pn.y() * depth, depth); return pose_.transform_from(pc); } /// backproject a 2-dimensional point to a 3-dimensional point at infinity - inline Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = K_->calibrate(p); + Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 return pose_.rotation().rotate(pc); } @@ -313,7 +209,7 @@ public: */ template double range( - const PinholePose& camera, // + const PinholeBase& camera, // OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return pose_.range(camera.pose(), Dcamera, Dother); @@ -384,10 +280,346 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(pose_); + } + +}; +// end of class PinholeBase + +/** + * A pinhole camera class that has a Pose3 and a *fixed* Calibration. + * Instead of using this class, one might consider calibrating the measurements + * and using CalibratedCamera, which would then be faster. + * @addtogroup geometry + * \nosubgrouping + */ +template +class PinholePose: public PinholeBase { + +private: + + typedef PinholeBase Base; + boost::shared_ptr K_; + +public: + + enum { + dimension = 6 + }; + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + PinholePose() { + } + + /** constructor with pose, uses default calibration */ + explicit PinholePose(const Pose3& pose) : + Base(pose), K_(new Calibration()) { + } + + /** constructor with pose and calibration */ + PinholePose(const Pose3& pose, const boost::shared_ptr& K) : + Base(pose), K_(K) { + } + + /// @} + /// @name Named Constructors + /// @{ + + /** + * Create a level camera at the given 2D pose and height + * @param K the calibration + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + * @param height camera height + */ + static PinholePose Level(const boost::shared_ptr& K, + const Pose2& pose2, double height) { + return PinholePose(CalibratedCamera::LevelPose(pose2, height), K); + } + + /// PinholePose::level with default calibration + static PinholePose Level(const Pose2& pose2, double height) { + return PinholePose::Level(boost::make_shared(), pose2, height); + } + + /** + * Create a camera at the given eye position looking at a target point in the scene + * with the specified up direction vector. + * @param eye specifies the camera position + * @param target the point to look at + * @param upVector specifies the camera up direction vector, + * doesn't need to be on the image plane nor orthogonal to the viewing axis + * @param K optional calibration parameter + */ + static PinholePose Lookat(const Point3& eye, const Point3& target, + const Point3& upVector, const boost::shared_ptr& K = + boost::make_shared()) { + return PinholePose(CalibratedCamera::LookatPose(eye, target, upVector), K); + } + + /// @} + /// @name Advanced Constructors + /// @{ + + explicit PinholePose(const Vector &v) : + Base(v), K_(new Calibration()) { + } + + PinholePose(const Vector &v, const Vector &K) : + Base(v), K_(new Calibration(K)) { + } + + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholePose* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_->equals(e->calibration(), tol); + } + + /// print + void print(const std::string& s = "PinholePose") const { + Base::print(s); + K_->print(s + ".calibration"); + } + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~PinholePose() { + } + + /// return calibration + const Calibration& calibration() const { + return *K_; + } + + /// @} + /// @name Manifold + /// @{ + + /// Manifold 6 + size_t dim() const { + return 6; + } + + /// Manifold 6 + static size_t Dim() { + return 6; + } + + typedef Eigen::Matrix VectorK6; + + /// move a cameras according to d + PinholePose retract(const Vector6& d) const { + return PinholePose(Base::pose().retract(d), K_); + } + + /// return canonical coordinate + VectorK6 localCoordinates(const PinholePose& p) const { + return Base::pose().localCoordinates(p.Base::pose()); + } + + /// for Canonical + static PinholePose identity() { + return PinholePose(); // assumes that the default constructor is valid + } + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * projects a 3-dimensional point in camera coordinates into the + * camera and returns a 2-dimensional point, no calibration applied + * @param P A point in camera coordinates + * @param Dpoint is the 2*3 Jacobian w.r.t. P + */ + static Point2 project_to_camera(const Point3& P, // + OptionalJacobian<2, 3> Dpoint = boost::none) { +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (P.z() <= 0) + throw CheiralityException(); +#endif + double d = 1.0 / P.z(); + const double u = P.x() * d, v = P.y() * d; + if (Dpoint) + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; + return Point2(u, v); + } + + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) const { + const Point3 pc = Base::pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); + } + + /** project a point from world coordinate to the image, fixed Jacobians + * @param pw is a point in the world coordinate + * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] + * @param Dpoint is the Jacobian w.r.t. point3 + */ + Point2 project2(const Point3& pw, + OptionalJacobian<2, 6> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { + + const Point3 pc = Base::pose().transform_to(pw); + const Point2 pn = project_to_camera(pc); + + if (!Dcamera && !Dpoint) { + return calibration().uncalibrate(pn); + } else { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); + + if (Dcamera) + calculateDpose(pn, d, Dpi_pn, *Dcamera); + if (Dpoint) + calculateDpoint(pn, d, Base::pose().rotation().matrix(), Dpi_pn, + *Dpoint); + + return pi; + } + } + + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& p, double depth) const { + const Point2 pn = calibration().calibrate(p); + const Point3 pc(pn.x() * depth, pn.y() * depth, depth); + return Base::pose().transform_from(pc); + } + + /// backproject a 2-dimensional point to a 3-dimensional point at infinity + Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = calibration().calibrate(p); + const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + return Base::pose().rotation().rotate(pc); + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpoint the optionally computed Jacobian with respect to the landmark + * @return range (double) + */ + double range( + const Point3& point, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return Base::pose().range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpose2 the optionally computed Jacobian with respect to the other pose + * @return range (double) + */ + double range( + const Pose3& pose, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return Base::pose().range(pose, Dcamera, Dpose); + } + + /** + * Calculate range to another camera + * @param camera Other camera + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + template + double range( + const PinholePose& camera, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return Base::pose().range(camera.pose(), Dcamera, Dother); + } + + /** + * Calculate range to another camera + * @param camera Other camera + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + double range( + const CalibratedCamera& camera, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return range(camera.pose(), Dcamera, Dother); + } + +private: + + /** + * Calculate Jacobian with respect to pose + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpose Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, + Eigen::MatrixBase const & Dpose) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + double uv = u * v, uu = u * u, vv = v * v; + Matrix26 Dpn_pose; + Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; + assert(Dpose.rows()==2 && Dpose.cols()==6); + const_cast&>(Dpose) = // + Dpi_pn * Dpn_pose; + } + + /** + * Calculate Jacobian with respect to point + * @param pn projection in normalized coordinates + * @param d disparity (inverse depth) + * @param Dpi_pn derivative of uncalibrate with respect to pn + * @param Dpoint Output argument, can be matrix or block, assumed right size ! + * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html + */ + template + static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, + const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 Dpn_point; + Dpn_point << // + R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // + /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + Dpn_point *= d; + assert(Dpoint.rows()==2 && Dpoint.cols()==3); + const_cast&>(Dpoint) = // + Dpi_pn * Dpn_point; + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("PinholeBase", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } }; +// end of class PinholePose template struct traits > : public internal::Manifold< From 35d6b9dc0e636fbb4f1d85b3c8f0cb2bf585cdf1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 08:44:57 +0100 Subject: [PATCH 09/36] Got rid of code duplication --- gtsam/geometry/PinholePose.h | 188 ++--------------------------------- 1 file changed, 8 insertions(+), 180 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index d7c099ab3..0cb189471 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -36,7 +36,7 @@ class PinholeBase { private: - Pose3 pose_; + Pose3 pose_; ///< 3D pose of camera public: @@ -229,7 +229,7 @@ public: return range(camera.pose(), Dcamera, Dother); } -private: +protected: /** * Calculate Jacobian with respect to pose @@ -275,6 +275,8 @@ private: Dpi_pn * Dpn_point; } +private: + /** Serialization function */ friend class boost::serialization::access; template @@ -297,14 +299,14 @@ class PinholePose: public PinholeBase { private: - typedef PinholeBase Base; - boost::shared_ptr K_; + typedef PinholeBase 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 /// @{ @@ -395,7 +397,7 @@ public: } /// return calibration - const Calibration& calibration() const { + virtual const Calibration& calibration() const { return *K_; } @@ -431,183 +433,9 @@ public: } /// @} - /// @name Transformations and measurement functions - /// @{ - - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P - */ - static Point2 project_to_camera(const Point3& P, // - OptionalJacobian<2, 3> Dpoint = boost::none) { -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (P.z() <= 0) - throw CheiralityException(); -#endif - double d = 1.0 / P.z(); - const double u = P.x() * d, v = P.y() * d; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - - /// Project a point into the image and check depth - std::pair projectSafe(const Point3& pw) const { - const Point3 pc = Base::pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); - } - - /** project a point from world coordinate to the image, fixed Jacobians - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2(const Point3& pw, - OptionalJacobian<2, 6> Dcamera = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { - - const Point3 pc = Base::pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return calibration().uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); - - if (Dcamera) - calculateDpose(pn, d, Dpi_pn, *Dcamera); - if (Dpoint) - calculateDpoint(pn, d, Base::pose().rotation().matrix(), Dpi_pn, - *Dpoint); - - return pi; - } - } - - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x() * depth, pn.y() * depth, depth); - return Base::pose().transform_from(pc); - } - - /// backproject a 2-dimensional point to a 3-dimensional point at infinity - Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 - return Base::pose().rotation().rotate(pc); - } - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - double range( - const Point3& point, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { - return Base::pose().range(point, Dcamera, Dpoint); - } - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpose2 the optionally computed Jacobian with respect to the other pose - * @return range (double) - */ - double range( - const Pose3& pose, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { - return Base::pose().range(pose, Dcamera, Dpose); - } - - /** - * Calculate range to another camera - * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera - * @return range (double) - */ - template - double range( - const PinholePose& camera, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { - return Base::pose().range(camera.pose(), Dcamera, Dother); - } - - /** - * Calculate range to another camera - * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera - * @return range (double) - */ - double range( - const CalibratedCamera& camera, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { - return range(camera.pose(), Dcamera, Dother); - } private: - /** - * Calculate Jacobian with respect to pose - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpose Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, - Eigen::MatrixBase const & Dpose) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - double uv = u * v, uu = u * u, vv = v * v; - Matrix26 Dpn_pose; - Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; - assert(Dpose.rows()==2 && Dpose.cols()==6); - const_cast&>(Dpose) = // - Dpi_pn * Dpn_pose; - } - - /** - * Calculate Jacobian with respect to point - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpoint Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, - const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - Matrix23 Dpn_point; - Dpn_point << // - R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // - /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); - Dpn_point *= d; - assert(Dpoint.rows()==2 && Dpoint.cols()==3); - const_cast&>(Dpoint) = // - Dpi_pn * Dpn_point; - } - /** Serialization function */ friend class boost::serialization::access; template From 7d37aa4512ec65611acb5c358885f659987055b0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:00:58 +0100 Subject: [PATCH 10/36] Put PinholeBase in CalibratedCamera --- gtsam/geometry/CalibratedCamera.cpp | 33 ++++++ gtsam/geometry/CalibratedCamera.h | 160 +++++++++++++++++++++++++++- 2 files changed, 191 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index f48312e47..1bba07d06 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -21,6 +21,39 @@ namespace gtsam { +/* ************************************************************************* */ +bool PinholeBase::equals(const PinholeBase &camera, double tol) const { + return pose_.equals(camera.pose(), tol); +} + +/* ************************************************************************* */ +void PinholeBase::print(const std::string& s) const { + pose_.print(s + ".pose"); +} + +/* ************************************************************************* */ +const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; + } + return pose_; +} + +/* ************************************************************************* */ +Point2 PinholeBase::project_to_camera(const Point3& P, + OptionalJacobian<2, 3> Dpoint) { +#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); +} + /* ************************************************************************* */ CalibratedCamera::CalibratedCamera(const Pose3& pose) : pose_(pose) { diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index d17c61ba7..a57c6f852 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,11 +19,10 @@ #pragma once #include +#include namespace gtsam { -class Point2; - class GTSAM_EXPORT CheiralityException: public ThreadsafeException< CheiralityException> { public: @@ -32,6 +31,163 @@ 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 + +public: + + /// @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 + /// @{ + + virtual ~PinholeBase() { + } + + /// return pose, constant version + const Pose3& pose() const { + return pose_; + } + + /// return pose, with derivative + const Pose3& pose(OptionalJacobian<6, 6> H) const; + + /// @} + /// @name Transformations and measurement functions + /// @{ + + /** + * projects a 3-dimensional point in camera coordinates into the + * camera and returns a 2-dimensional point + * @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); + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpoint the optionally computed Jacobian with respect to the landmark + * @return range (double) + */ + double range( + const Point3& point, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose_.range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpose2 the optionally computed Jacobian with respect to the other pose + * @return range (double) + */ + double range( + const Pose3& pose, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return pose_.range(pose, Dcamera, Dpose); + } + +protected: + + /** + * 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; + } + +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 From f5581ec652567d7d196372296ebe94e6eed64157 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:13:12 +0100 Subject: [PATCH 11/36] CalibratedCamera now derived from PinholeBase --- gtsam/geometry/CalibratedCamera.cpp | 40 +++------ gtsam/geometry/CalibratedCamera.h | 127 +++++++++------------------- 2 files changed, 51 insertions(+), 116 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 1bba07d06..327948f3e 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -55,33 +55,13 @@ Point2 PinholeBase::project_to_camera(const Point3& P, } /* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Pose3& pose) : - pose_(pose) { -} - -/* ************************************************************************* */ -CalibratedCamera::CalibratedCamera(const Vector &v) : - pose_(Pose3::Expmap(v)) { -} - -/* ************************************************************************* */ -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; - } - return Point2(P.x() / P.z(), P.y() / P.z()); -} - -/* ************************************************************************* */ -Point3 CalibratedCamera::backproject_from_camera(const Point2& p, +Point3 PinholeBase::backproject_from_camera(const Point2& p, const double scale) { return Point3(p.x() * scale, p.y() * scale, scale); } /* ************************************************************************* */ -Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) { +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); @@ -90,12 +70,7 @@ Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) { } /* ************************************************************************* */ -CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { - return CalibratedCamera(LevelPose(pose2, height)); -} - -/* ************************************************************************* */ -Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target, +Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, const Point3& upVector) { Point3 zc = target - eye; zc = zc / zc.norm(); @@ -105,6 +80,11 @@ Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target, return Pose3(Rot3(xc, yc, zc), eye); } +/* ************************************************************************* */ +CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { + return CalibratedCamera(LevelPose(pose2, height)); +} + /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, const Point3& target, const Point3& upVector) { @@ -120,7 +100,7 @@ Point2 CalibratedCamera::project(const Point3& point, Matrix3 Dpoint_; Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); #else - Point3 q = pose_.transform_to(point); + Point3 q = pose().transform_to(point); #endif Point2 intrinsic = project_to_camera(q); @@ -145,7 +125,7 @@ Point2 CalibratedCamera::project(const Point3& point, *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()); + 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) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index a57c6f852..6185cb4f9 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -44,6 +44,30 @@ private: 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 /// @{ @@ -102,6 +126,11 @@ public: static Point2 project_to_camera(const Point3& P, // OptionalJacobian<2, 3> Dpoint = boost::none); + /** + * backproject a 2-dimensional point to a 3-dimension point + */ + static Point3 backproject_from_camera(const Point2& p, const double scale); + /** * Calculate range to a landmark * @param point 3D location of landmark @@ -195,9 +224,7 @@ private: * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT CalibratedCamera { -private: - Pose3 pose_; // 6DOF pose +class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: @@ -213,21 +240,14 @@ public: } /// construct with pose - explicit CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose) : + PinholeBase(pose) { + } /// @} /// @name Named Constructors /// @{ - /** - * 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 level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -236,17 +256,6 @@ public: */ static CalibratedCamera Level(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); - /** * Create a camera at the given eye position looking at a target point in the scene * with the specified up direction vector. @@ -263,19 +272,8 @@ public: /// @{ /// 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) { } /// @} @@ -286,11 +284,6 @@ public: virtual ~CalibratedCamera() { } - /// return pose - inline const Pose3& pose() const { - return pose_; - } - /// @} /// @name Manifold /// @{ @@ -311,10 +304,6 @@ public: return 6; } - /* ************************************************************************* */ - // measurement functions and derivatives - /* ************************************************************************* */ - /// @} /// @name Transformations and mesaurement functions /// @{ @@ -330,43 +319,6 @@ public: 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 - */ - static Point2 project_to_camera(const Point3& cameraPoint, - OptionalJacobian<2, 3> H1 = boost::none); - - /** - * backproject a 2-dimensional point to a 3-dimension point - */ - static Point3 backproject_from_camera(const Point2& p, const double scale); - - /** - * 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); - } - - /** - * 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); - } - /** * Calculate range to another camera * @param camera Other camera @@ -376,12 +328,13 @@ public: */ 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); + return pose().range(camera.pose(), H1, H2); } + /// @} + private: - /// @} /// @name Advanced Interface /// @{ @@ -389,7 +342,9 @@ 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)); } /// @} From ead834982735442f2d7fcc0f260cab5125463950 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:15:08 +0100 Subject: [PATCH 12/36] Got rid of legacy code --- gtsam/geometry/CalibratedCamera.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 327948f3e..59d7e6abf 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -95,13 +95,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, 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 @@ -109,15 +103,6 @@ Point2 CalibratedCamera::project(const Point3& point, 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; @@ -132,7 +117,6 @@ Point2 CalibratedCamera::project(const Point3& point, - v * R(2, 2); *Dpoint = d * Dpoint_; } -#endif } return intrinsic; } From 90d2146f62c5373a00c10e71cb91059fc9eed3bd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:35:27 +0100 Subject: [PATCH 13/36] scale -> depth --- gtsam/geometry/CalibratedCamera.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 59d7e6abf..41ed3e331 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -56,8 +56,8 @@ Point2 PinholeBase::project_to_camera(const Point3& P, /* ************************************************************************* */ Point3 PinholeBase::backproject_from_camera(const Point2& p, - const double scale) { - return Point3(p.x() * scale, p.y() * scale, scale); + const double depth) { + return Point3(p.x() * depth, p.y() * depth, depth); } /* ************************************************************************* */ From e6828439c1c69b0aaa6fd5a699ce2467f3951078 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:35:49 +0100 Subject: [PATCH 14/36] Moved range back into derived as overloaded --- gtsam/geometry/CalibratedCamera.h | 61 +++++++++++++++---------------- 1 file changed, 29 insertions(+), 32 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 6185cb4f9..13d607c80 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -102,9 +102,6 @@ public: /// @name Standard Interface /// @{ - virtual ~PinholeBase() { - } - /// return pose, constant version const Pose3& pose() const { return pose_; @@ -129,35 +126,7 @@ public: /** * backproject a 2-dimensional point to a 3-dimension point */ - static Point3 backproject_from_camera(const Point2& p, const double scale); - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the landmark - * @return range (double) - */ - double range( - const Point3& point, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { - return pose_.range(point, Dcamera, Dpoint); - } - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpose2 the optionally computed Jacobian with respect to the other pose - * @return range (double) - */ - double range( - const Pose3& pose, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { - return pose_.range(pose, Dcamera, Dpose); - } + static Point3 backproject_from_camera(const Point2& p, const double depth); protected: @@ -319,6 +288,34 @@ public: OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpoint the optionally computed Jacobian with respect to the landmark + * @return range (double) + */ + double range( + const Point3& point, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + return pose().range(point, Dcamera, Dpoint); + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dpose2 the optionally computed Jacobian with respect to the other pose + * @return range (double) + */ + double range( + const Pose3& pose, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + return this->pose().range(pose, Dcamera, Dpose); + } + /** * Calculate range to another camera * @param camera Other camera From fd62c6f0e62886c35690e39238b74be9326c96a6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 09:36:12 +0100 Subject: [PATCH 15/36] PinholeBaseK now derives from PinholeBase --- gtsam/geometry/PinholePose.h | 143 ++++------------------- gtsam/geometry/tests/testPinholePose.cpp | 1 + 2 files changed, 23 insertions(+), 121 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 0cb189471..40373c9fb 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -20,7 +20,6 @@ #pragma once #include -#include #include #include @@ -32,11 +31,7 @@ namespace gtsam { * \nosubgrouping */ template -class PinholeBase { - -private: - - Pose3 pose_; ///< 3D pose of camera +class GTSAM_EXPORT PinholeBaseK : public PinholeBase { public: @@ -44,55 +39,27 @@ public: /// @{ /** default constructor */ - PinholeBase() { + PinholeBaseK() { } /** constructor with pose */ - explicit PinholeBase(const Pose3& pose) : - pose_(pose) { + explicit PinholeBaseK(const Pose3& pose) : + PinholeBase(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 { - return pose_.equals(camera.pose(), tol); - } - - /// print - void print(const std::string& s = "PinholeBase") const { - pose_.print(s + ".pose"); + explicit PinholeBaseK(const Vector &v) : + PinholeBase(v) { } /// @} /// @name Standard Interface /// @{ - virtual ~PinholeBase() { - } - - /// return pose, constant version - const Pose3& pose() const { - return pose_; - } - - /// return pose, with derivative - const Pose3& pose(OptionalJacobian<6, 6> H) const { - if (H) { - H->setZero(); - H->block(0, 0, 6, 6) = I_6x6; - } - return pose_; + virtual ~PinholeBaseK() { } /// return calibration @@ -102,28 +69,9 @@ public: /// @name Transformations and measurement functions /// @{ - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P - */ - static Point2 project_to_camera(const Point3& P, // - OptionalJacobian<2, 3> Dpoint = boost::none) { -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (P.z() <= 0) - throw CheiralityException(); -#endif - double d = 1.0 / P.z(); - const double u = P.x() * d, v = P.y() * d; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); + const Point3 pc = pose().transform_to(pw); const Point2 pn = project_to_camera(pc); return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); } @@ -137,7 +85,7 @@ public: OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { - const Point3 pc = pose_.transform_to(pw); + const Point3 pc = pose().transform_to(pw); const Point2 pn = project_to_camera(pc); if (!Dcamera && !Dpoint) { @@ -152,7 +100,7 @@ public: if (Dcamera) calculateDpose(pn, d, Dpi_pn, *Dcamera); if (Dpoint) - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + calculateDpoint(pn, d, pose().rotation().matrix(), Dpi_pn, *Dpoint); return pi; } @@ -161,15 +109,14 @@ public: /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Point2& p, double depth) const { const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x() * depth, pn.y() * depth, depth); - return pose_.transform_from(pc); + 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); + return pose().rotation().rotate(pc); } /** @@ -183,7 +130,7 @@ public: const Point3& point, // OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { - return pose_.range(point, Dcamera, Dpoint); + return pose().range(point, Dcamera, Dpoint); } /** @@ -197,7 +144,7 @@ public: const Pose3& pose, // OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { - return pose_.range(pose, Dcamera, Dpose); + return this->pose().range(pose, Dcamera, Dpose); } /** @@ -209,10 +156,10 @@ public: */ template double range( - const PinholeBase& camera, // + const PinholeBaseK& camera, // OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { - return pose_.range(camera.pose(), Dcamera, Dother); + return pose().range(camera.pose(), Dcamera, Dother); } /** @@ -226,53 +173,7 @@ public: const CalibratedCamera& camera, // OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { - return range(camera.pose(), Dcamera, Dother); - } - -protected: - - /** - * 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; + return pose().range(camera.pose(), Dcamera, Dother); } private: @@ -281,11 +182,11 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + ar & BOOST_SERIALIZATION_NVP(pose()); } }; -// end of class PinholeBase +// end of class PinholeBaseK /** * A pinhole camera class that has a Pose3 and a *fixed* Calibration. @@ -295,11 +196,11 @@ private: * \nosubgrouping */ template -class PinholePose: public PinholeBase { +class GTSAM_EXPORT PinholePose: public PinholeBaseK { private: - typedef PinholeBase Base; ///< base class has 3D pose as private member + typedef PinholeBaseK Base; ///< base class has 3D pose as private member boost::shared_ptr K_; ///< shared pointer to fixed calibration public: @@ -441,7 +342,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("PinholeBase", + & boost::serialization::make_nvp("PinholeBaseK", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 0d2f33890..29765273c 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include From 29e5faeef05a1157640f7d201abd8e19fd3a6223 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 10:00:56 +0100 Subject: [PATCH 16/36] Refactored derivatives --- gtsam/geometry/CalibratedCamera.cpp | 54 +++++++++++++++++------------ gtsam/geometry/CalibratedCamera.h | 38 +++----------------- gtsam/geometry/PinholePose.h | 8 +++-- 3 files changed, 42 insertions(+), 58 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 41ed3e331..620ba1cc8 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -80,6 +80,28 @@ Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target, return Pose3(Rot3(xc, yc, zc), eye); } +/* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) { + // 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; + return Dpn_point; +} + /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { return CalibratedCamera(LevelPose(pose2, height)); @@ -93,32 +115,20 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { + OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { - Point3 q = pose().transform_to(point); - Point2 intrinsic = project_to_camera(q); + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); + Point2 pn = project_to_camera(q); - // Check if point is in front of camera - if (q.z() <= 0) - throw CheiralityException(); - - if (Dpose || Dpoint) { - // optimized version, see CalibratedCamera.nb + if (Dcamera || Dpoint) { 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_; - } + if (Dcamera) + *Dcamera = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose } - return intrinsic; + return pn; } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 13d607c80..fa02f706d 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: @@ -134,45 +135,16 @@ protected: * 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; - } + 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 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 + * @param R rotation matrix */ - 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; - } + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R); private: diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 40373c9fb..488e860bc 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -85,7 +86,8 @@ public: OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { - const Point3 pc = pose().transform_to(pw); + Matrix3 Rt; // calculated by transform_to if needed + const Point3 pc = pose().transform_to(pw, boost::none, Dpoint ? &Rt : 0); const Point2 pn = project_to_camera(pc); if (!Dcamera && !Dpoint) { @@ -98,9 +100,9 @@ public: const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); if (Dcamera) - calculateDpose(pn, d, Dpi_pn, *Dcamera); + *Dcamera = Dpi_pn * PinholeBase::Dpose(pn, d); if (Dpoint) - calculateDpoint(pn, d, pose().rotation().matrix(), Dpi_pn, *Dpoint); + *Dpoint = Dpi_pn * PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose return pi; } From 3a755cc4fba1fc81bbcae775930e54bfba869af1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 10:02:30 +0100 Subject: [PATCH 17/36] Moved static methods up --- gtsam/geometry/CalibratedCamera.cpp | 84 ++++++++++++++--------------- gtsam/geometry/CalibratedCamera.h | 39 ++++++++------ 2 files changed, 64 insertions(+), 59 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 620ba1cc8..89ca6ba8c 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -21,6 +21,48 @@ namespace gtsam { +/* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) { + // 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; + return Dpn_point; +} + +/* ************************************************************************* */ +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); @@ -60,48 +102,6 @@ Point3 PinholeBase::backproject_from_camera(const Point2& p, return Point3(p.x() * depth, p.y() * depth, depth); } -/* ************************************************************************* */ -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); -} - -/* ************************************************************************* */ -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; -} - -/* ************************************************************************* */ -Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) { - // 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; - return Dpn_point; -} - /* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { return CalibratedCamera(LevelPose(pose2, height)); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index fa02f706d..457ae2819 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -43,6 +43,28 @@ 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 R rotation matrix + */ + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R); + + /// @} + public: /// @name Static functions @@ -129,23 +151,6 @@ public: */ static Point3 backproject_from_camera(const Point2& p, const double depth); -protected: - - /** - * 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 R rotation matrix - */ - static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R); - private: /** Serialization function */ From 286a3ff412c55a6e7b1b7d3ea3491730f636305b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 10:19:10 +0100 Subject: [PATCH 18/36] Moved project2 to PinholeBase --- gtsam/geometry/CalibratedCamera.cpp | 32 +++++++++++++++++------------ gtsam/geometry/CalibratedCamera.h | 23 +++++++++++++-------- 2 files changed, 34 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 89ca6ba8c..2876da579 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -96,6 +96,24 @@ Point2 PinholeBase::project_to_camera(const Point3& P, return Point2(u, v); } +/* ************************************************************************* */ +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); + Point2 pn = project_to_camera(q); + + if (Dpose || Dpoint) { + const double z = q.z(), d = 1.0 / z; + if (Dpose) + *Dpose = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose + } + return pn; +} + /* ************************************************************************* */ Point3 PinholeBase::backproject_from_camera(const Point2& p, const double depth) { @@ -116,19 +134,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, OptionalJacobian<2, 6> Dcamera, 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); - Point2 pn = project_to_camera(q); - - if (Dcamera || Dpoint) { - const double z = q.z(), d = 1.0 / z; - if (Dcamera) - *Dcamera = PinholeBase::Dpose(pn, d); - if (Dpoint) - *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose - } - return pn; + return project2(point, Dcamera, Dpoint); } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 457ae2819..9f0ba5ea0 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -147,10 +147,20 @@ public: OptionalJacobian<2, 3> Dpoint = boost::none); /** - * backproject a 2-dimensional point to a 3-dimension point + * backproject a 2-dimensional point to a 3-dimensional point at given depth */ static Point3 backproject_from_camera(const Point2& p, const double depth); + /** + * Project point into the image + * @param point 3D point in world coordinates + * @param Dpose the optionally computed Jacobian with respect to camera + * @param Dpoint the optionally computed Jacobian with respect to the 3D point + * @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; + private: /** Serialization function */ @@ -253,16 +263,13 @@ public: /// @} /// @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 + * @deprecated + * Use project2, which is more consistently named across Pinhole cameras */ Point2 project(const Point3& point, - OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /** From f08e22817372615d450bf391c0d011bd11c00e99 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 10:27:07 +0100 Subject: [PATCH 19/36] Now just calls PinholeBase::project2 --- gtsam/geometry/PinholePose.h | 46 ++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 488e860bc..8785ec3fc 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -32,7 +32,7 @@ namespace gtsam { * \nosubgrouping */ template -class GTSAM_EXPORT PinholeBaseK : public PinholeBase { +class GTSAM_EXPORT PinholeBaseK: public PinholeBase { public: @@ -45,7 +45,7 @@ public: /** constructor with pose */ explicit PinholeBaseK(const Pose3& pose) : - PinholeBase(pose) { + PinholeBase(pose) { } /// @} @@ -53,7 +53,7 @@ public: /// @{ explicit PinholeBaseK(const Vector &v) : - PinholeBase(v) { + PinholeBase(v) { } /// @} @@ -78,40 +78,34 @@ 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 + * @param pw is a point in the world coordinates + * @param Dpose is the Jacobian w.r.t. pose + * @param Dpoint is the Jacobian w.r.t. pw */ - Point2 project2(const Point3& pw, - OptionalJacobian<2, 6> Dcamera = boost::none, + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { - Matrix3 Rt; // calculated by transform_to if needed - const Point3 pc = pose().transform_to(pw, boost::none, Dpoint ? &Rt : 0); - const Point2 pn = project_to_camera(pc); + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - if (!Dcamera && !Dpoint) { - return calibration().uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, boost::none, + Dpose || Dpoint ? &Dpi_pn : 0); - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn); + // If needed, apply chain rule + if (Dpose) + *Dpose = Dpi_pn * *Dpose; + if (Dpoint) + *Dpoint = Dpi_pn * *Dpoint; - if (Dcamera) - *Dcamera = Dpi_pn * PinholeBase::Dpose(pn, d); - if (Dpoint) - *Dpoint = Dpi_pn * PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose - - return pi; - } + 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)); + return pose().transform_from(backproject_from_camera(pn, depth)); } /// backproject a 2-dimensional point to a 3-dimensional point at infinity From c7a41d30cc91a0516456b24f0fc84cde6504e5b4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 10:41:53 +0100 Subject: [PATCH 20/36] Cleaned up projectSafe and cheirality exception --- gtsam/geometry/CalibratedCamera.cpp | 29 +++++++++++++++++++---------- gtsam/geometry/CalibratedCamera.h | 18 ++++++++++-------- gtsam/geometry/PinholePose.h | 6 +++--- 3 files changed, 32 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 2876da579..80f63f3fc 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -19,6 +19,8 @@ #include #include +using namespace std; + namespace gtsam { /* ************************************************************************* */ @@ -69,7 +71,7 @@ bool PinholeBase::equals(const PinholeBase &camera, double tol) const { } /* ************************************************************************* */ -void PinholeBase::print(const std::string& s) const { +void PinholeBase::print(const string& s) const { pose_.print(s + ".pose"); } @@ -83,29 +85,36 @@ const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const { } /* ************************************************************************* */ -Point2 PinholeBase::project_to_camera(const Point3& P, +Point2 PinholeBase::project_to_camera(const Point3& pc, OptionalJacobian<2, 3> Dpoint) { -#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; + 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); - Point2 pn = project_to_camera(q); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw CheiralityException(); +#endif + const Point2 pn = project_to_camera(q); if (Dpose || Dpoint) { - const double z = q.z(), d = 1.0 / z; + const double d = 1.0 / q.z(); if (Dpose) *Dpose = PinholeBase::Dpose(pn, d); if (Dpoint) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9f0ba5ea0..35e8a5086 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -138,21 +138,23 @@ public: /// @{ /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P + * 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 + * @param Dpoint is the 2*3 Jacobian w.r.t. pc */ - static Point2 project_to_camera(const Point3& P, // + static Point2 project_to_camera(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); - /** - * backproject a 2-dimensional point to a 3-dimensional point at given depth - */ + /// Project a point into the image and check depth + std::pair projectSafe(const Point3& pw) 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); /** * 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 * @param Dpose the optionally computed Jacobian with respect to camera * @param Dpoint the optionally computed Jacobian with respect to the 3D point diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 8785ec3fc..2fc463d2b 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -72,9 +72,9 @@ public: /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose().transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); + 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 From 2ee090ece58b6bd2402a8bfaa50a9ff94c6c24d8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:00:13 +0100 Subject: [PATCH 21/36] Saved a transpose --- gtsam/geometry/CalibratedCamera.cpp | 8 ++++---- gtsam/geometry/CalibratedCamera.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 80f63f3fc..0e5a6a3ea 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -34,13 +34,13 @@ Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { } /* ************************************************************************* */ -Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) { +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 << // - 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); + 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; } @@ -118,7 +118,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, if (Dpose) *Dpose = PinholeBase::Dpose(pn, d); if (Dpoint) - *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose + *Dpoint = PinholeBase::Dpoint(pn, d, Rt); } return pn; } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 35e8a5086..18df33f40 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -59,9 +59,9 @@ protected: * Calculate Jacobian with respect to point * @param pn projection in normalized coordinates * @param d disparity (inverse depth) - * @param R rotation matrix + * @param Rt transposed rotation matrix */ - static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R); + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt); /// @} From 059ff82beb0c3b5f207dd76abb11e27d6629fe0b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:00:37 +0100 Subject: [PATCH 22/36] Added more diagnostic test for derivatives --- gtsam/geometry/tests/testCalibratedCamera.cpp | 24 +++++++++++++++---- gtsam/geometry/tests/testPinholePose.cpp | 14 +++++++++++ 2 files changed, 33 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 6a990e08e..0f3fac53e 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, 0.5)); + 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/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 29765273c..c2ba412a4 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -182,6 +182,20 @@ TEST( PinholePose, Dproject2) 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, 0.5)); + 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); From 05d5bad1a703e08d7b90cd644b328315e45f5d61 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:05:25 +0100 Subject: [PATCH 23/36] backproject --- gtsam/geometry/CalibratedCamera.h | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 18df33f40..9cc910f1d 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -149,9 +149,6 @@ public: /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) 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); - /** * Project point into the image * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -163,6 +160,9 @@ public: 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 */ @@ -274,6 +274,11 @@ public: OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + /// 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 From 1114509e9807a62751ef841e9427e0b48b456608 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:05:43 +0100 Subject: [PATCH 24/36] Switch order of two range functions --- gtsam/geometry/PinholePose.h | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 2fc463d2b..5a3b002a1 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -144,7 +144,21 @@ public: } /** - * Calculate range to another camera + * Calculate range to a CalibratedCamera + * @param camera Other camera + * @param Dcamera the optionally computed Jacobian with respect to pose + * @param Dother the optionally computed Jacobian with respect to the other camera + * @return range (double) + */ + double range( + const CalibratedCamera& camera, // + OptionalJacobian<1, 6> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return pose().range(camera.pose(), Dcamera, Dother); + } + + /** + * Calculate range to a PinholePoseK derived class * @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 @@ -158,20 +172,6 @@ public: return pose().range(camera.pose(), Dcamera, Dother); } - /** - * Calculate range to another camera - * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera - * @return range (double) - */ - double range( - const CalibratedCamera& camera, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { - return pose().range(camera.pose(), Dcamera, Dother); - } - private: /** Serialization function */ From 81b3b896be64bd7f9829b86a9c9723cf0924fd41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:09:56 +0100 Subject: [PATCH 25/36] OptionalJacobian is self-documenting. Removing redundant doc makes header shorter/easier to read. --- gtsam/geometry/CalibratedCamera.h | 26 +++++++------------------- gtsam/geometry/PinholePose.h | 26 +++++--------------------- 2 files changed, 12 insertions(+), 40 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 9cc910f1d..1afbd1417 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -141,7 +141,6 @@ public: * 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 - * @param Dpoint is the 2*3 Jacobian w.r.t. pc */ static Point2 project_to_camera(const Point3& pc, // OptionalJacobian<2, 3> Dpoint = boost::none); @@ -153,8 +152,6 @@ public: * 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 - * @param Dpose the optionally computed Jacobian with respect to camera - * @param Dpoint the optionally computed Jacobian with respect to the 3D point * @return the intrinsic coordinates of the projected point */ Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = @@ -270,9 +267,8 @@ public: * @deprecated * Use project2, which is more consistently named across Pinhole cameras */ - Point2 project(const Point3& point, - OptionalJacobian<2, 6> Dcamera = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + 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-dimensional point at given depth Point3 backproject(const Point2& pn, double depth) const { @@ -282,12 +278,9 @@ public: /** * 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, // + double range(const Point3& point, OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { return pose().range(point, Dcamera, Dpoint); @@ -296,13 +289,9 @@ 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, 6> Dcamera = boost::none, + 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); } @@ -310,12 +299,11 @@ public: /** * 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 { + 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); } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 5a3b002a1..e3dd98be0 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -79,8 +79,6 @@ public: /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinates - * @param Dpose is the Jacobian w.r.t. pose - * @param Dpoint is the Jacobian w.r.t. pw */ Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { @@ -118,12 +116,9 @@ public: /** * 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, // + double range(const Point3& point, OptionalJacobian<1, 6> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { return pose().range(point, Dcamera, Dpoint); @@ -132,13 +127,9 @@ 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, 6> Dcamera = boost::none, + 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); } @@ -146,27 +137,20 @@ public: /** * Calculate range to a CalibratedCamera * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ - double range( - const CalibratedCamera& camera, // - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { + 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 - * @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 PinholeBaseK& camera, // + 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); From 5ed2abd292a90b3d4b6d21d87ea5535d6148334a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:47:59 +0100 Subject: [PATCH 26/36] Little things --- gtsam/geometry/CalibratedCamera.h | 4 ++-- gtsam/geometry/PinholePose.h | 25 ++++++++++++------------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 1afbd1417..34edae2e1 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -249,12 +249,12 @@ 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; } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index e3dd98be0..7bcc22172 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -22,7 +22,6 @@ #include #include #include -#include namespace gtsam { @@ -34,7 +33,9 @@ namespace gtsam { template class GTSAM_EXPORT PinholeBaseK: public PinholeBase { -public: + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + +public : /// @name Standard Constructors /// @{ @@ -45,7 +46,7 @@ public: /** constructor with pose */ explicit PinholeBaseK(const Pose3& pose) : - PinholeBase(pose) { + PinholeBase(pose) { } /// @} @@ -53,7 +54,7 @@ public: /// @{ explicit PinholeBaseK(const Vector &v) : - PinholeBase(v) { + PinholeBase(v) { } /// @} @@ -92,10 +93,8 @@ public: Dpose || Dpoint ? &Dpi_pn : 0); // If needed, apply chain rule - if (Dpose) - *Dpose = Dpi_pn * *Dpose; - if (Dpoint) - *Dpoint = Dpi_pn * *Dpoint; + if (Dpose) *Dpose = Dpi_pn * (*Dpose); + if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint); return pi; } @@ -246,10 +245,12 @@ public: /// @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)) { } @@ -286,25 +287,23 @@ public: /// @name Manifold /// @{ - /// Manifold 6 + /// @deprecated size_t dim() const { return 6; } - /// Manifold 6 + /// @deprecated static size_t Dim() { return 6; } - typedef Eigen::Matrix VectorK6; - /// move a cameras according to d PinholePose retract(const Vector6& d) const { return PinholePose(Base::pose().retract(d), K_); } /// return canonical coordinate - VectorK6 localCoordinates(const PinholePose& p) const { + Vector6 localCoordinates(const PinholePose& p) const { return Base::pose().localCoordinates(p.Base::pose()); } From 57c921c6cfb611ef2b02e103feed3b8c3e18876c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 11:48:46 +0100 Subject: [PATCH 27/36] Big refactor of PinholeCamera: now derives from PinholeBaseK --- gtsam/geometry/PinholeCamera.h | 304 ++++++--------------- gtsam/geometry/tests/testPinholeCamera.cpp | 1 + 2 files changed, 87 insertions(+), 218 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index a298dafa4..996313c5c 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,32 +18,32 @@ #pragma once -#include -#include -#include +#include namespace gtsam { /** * A pinhole camera class that has a Pose3 and a Calibration. + * Use PinholePose if you will not be optimizing for Calibration * @addtogroup geometry * \nosubgrouping */ template -class PinholeCamera { +class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { private: - Pose3 pose_; - Calibration K_; - GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + typedef PinholeBaseK Base; ///< base class has 3D pose as private member + Calibration K_; ///< Calibration, part of class now // Get dimensions of calibration type at compile time -static const int DimK = FixedDimension::value; + static const int DimK = FixedDimension::value; public: - enum {dimension = 6 + DimK}; + enum { + dimension = 6 + DimK + }; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ @@ -54,12 +54,12 @@ public: /** constructor with pose */ explicit PinholeCamera(const Pose3& pose) : - pose_(pose) { + Base(pose) { } /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& K) : - pose_(pose), K_(K) { + Base(pose), K_(K) { } /// @} @@ -75,12 +75,7 @@ public: */ static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) { - const double st = sin(pose2.theta()), ct = cos(pose2.theta()); - const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); - const Rot3 wRc(x, y, z); - const Point3 t(pose2.x(), pose2.y(), height); - const Pose3 pose3(wRc, t); - return PinholeCamera(pose3, K); + return PinholeCamera(CalibratedCamera::LevelPose(pose2, height), K); } /// PinholeCamera::level with default calibration @@ -99,28 +94,23 @@ public: */ static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { - Point3 zc = target - eye; - zc = zc / zc.norm(); - Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down - xc = xc / xc.norm(); - Point3 yc = zc.cross(xc); - Pose3 pose3(Rot3(xc, yc, zc), eye); - return PinholeCamera(pose3, K); + return PinholeCamera(CalibratedCamera::LookatPose(eye, target, upVector), K); } /// @} /// @name Advanced Constructors /// @{ - explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(6)); - if (v.size() > 6) { - K_ = Calibration(v.tail(DimK)); - } + /// Init from vector, can be 6D (default calibration) or dim + explicit PinholeCamera(const Vector &v) : + Base(v.head<6>()) { + if (v.size() > 6) + K_ = Calibration(v.tail()); } + /// Init from Vector and calibration PinholeCamera(const Vector &v, const Vector &K) : - pose_(Pose3::Expmap(v)), K_(K) { + Base(v), K_(K) { } /// @} @@ -128,14 +118,14 @@ public: /// @{ /// assert equality up to a tolerance - bool equals(const PinholeCamera &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) - && K_.equals(camera.calibration(), tol); + bool equals(const Base &camera, double tol = 1e-9) const { + const PinholeCamera* e = dynamic_cast(&camera); + return Base::equals(camera, tol) && K_.equals(e->calibration(), tol); } /// print void print(const std::string& s = "PinholeCamera") const { - pose_.print(s + ".pose"); + Base::print(s); K_.print(s + ".calibration"); } @@ -147,13 +137,8 @@ public: } /// return pose - Pose3& pose() { - return pose_; - } - - /// return pose, constant version const Pose3& pose() const { - return pose_; + return Base::pose(); } /// return pose, with derivative @@ -162,12 +147,7 @@ public: H->setZero(); H->block(0, 0, 6, 6) = I_6x6; } - return pose_; - } - - /// return calibration - Calibration& calibration() { - return K_; + return Base::pose(); } /// return calibration @@ -179,12 +159,12 @@ public: /// @name Manifold /// @{ - /// Manifold dimension + /// @deprecated size_t dim() const { return dimension; } - /// Manifold dimension + /// @deprecated static size_t Dim() { return dimension; } @@ -194,17 +174,17 @@ public: /// move a cameras according to d PinholeCamera retract(const Vector& d) const { if ((size_t) d.size() == 6) - return PinholeCamera(pose().retract(d), calibration()); + return PinholeCamera(this->pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(6)), - calibration().retract(d.tail(calibration().dim()))); + return PinholeCamera(this->pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); } /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 d; // TODO: why does d.head<6>() not compile?? - d.head(6) = pose().localCoordinates(T2.pose()); + d.head(6) = this->pose().localCoordinates(T2.pose()); d.tail(DimK) = calibration().localCoordinates(T2.calibration()); return d; } @@ -218,32 +198,6 @@ public: /// @name Transformations and measurement functions /// @{ - /** - * projects a 3-dimensional point in camera coordinates into the - * camera and returns a 2-dimensional point, no calibration applied - * @param P A point in camera coordinates - * @param Dpoint is the 2*3 Jacobian w.r.t. P - */ - static Point2 project_to_camera(const Point3& P, // - OptionalJacobian<2, 3> Dpoint = boost::none) { -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - if (P.z() <= 0) - throw CheiralityException(); -#endif - double d = 1.0 / P.z(); - const double u = P.x() * d, v = P.y() * d; - if (Dpoint) - *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; - return Point2(u, v); - } - - /// Project a point into the image and check depth - std::pair projectSafe(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); - } - typedef Eigen::Matrix Matrix2K; /** project a point from world coordinate to the image @@ -252,31 +206,25 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcal, + Dpose || Dpoint ? &Dpi_pn : 0); - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; + // If needed, apply chain rule + if (Dpose) + *Dpose = Dpi_pn * *Dpose; + if (Dpoint) + *Dpoint = Dpi_pn * *Dpoint; - // uncalibration - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) - calculateDpose(pn, d, Dpi_pn, *Dpose); - if (Dpoint) - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - return pi; - } else - return K_.uncalibrate(pn, Dcal); + return pi; } /** project a point at infinity from world coordinate to the image @@ -285,149 +233,114 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - Point2 projectPointAtInfinity(const Point3& pw, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none, + Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = project_to_camera(pc);// project the point to the camera + const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) + const Point2 pn = Base::project_to_camera(pc); // project the point to the camera return K_.uncalibrate(pn); } // world to camera coordinate Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); + const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); Matrix36 Dpc_pose; Dpc_pose.setZero(); Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate - Matrix23 Dpn_pc;// 2*3 - const Point2 pn = project_to_camera(pc, Dpn_pc); + Matrix23 Dpn_pc; // 2*3 + const Point2 pn = Base::project_to_camera(pc, Dpn_pc); // uncalibration - Matrix2 Dpi_pn;// 2*2 + Matrix2 Dpi_pn; // 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; + *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) return pi; } /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 */ Point2 project2( const Point3& pw, // OptionalJacobian<2, dimension> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); + // project to normalized coordinates + Matrix26 Dpose; + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; + // uncalibrate to pixel coordinates + Matrix2K Dcal; + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0, + Dcamera || Dpoint ? &Dpi_pn : 0); - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + // If needed, calculate derivatives + if (Dcamera) + *Dcamera << Dpi_pn * Dpose, Dcal; + if (Dpoint) + *Dpoint = Dpi_pn * (*Dpoint); - if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); - (*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib - } - if (Dpoint) { - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } - } - - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x() * depth, pn.y() * depth, depth); - return pose_.transform_from(pc); - } - - /// backproject a 2-dimensional point to a 3-dimensional point at infinity - Point3 backprojectPointAtInfinity(const Point2& p) const { - const Point2 pn = K_.calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 - return pose_.rotation().rotate(pc); + return pi; } /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ - double range( - const Point3& point, // - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ - double range( - const Pose3& pose, // - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = + boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { Matrix16 Dpose_; - double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) - *Dcamera << Dpose_, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template - double range( - const PinholeCamera& camera, // + double range(const PinholeCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, -// OptionalJacobian<1, 6 + traits::dimension::value> Dother = - boost::optional Dother = - boost::none) const { + boost::optional Dother = boost::none) const { Matrix16 Dcamera_, Dother_; - double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, Dother ? &Dother_ : 0); if (Dcamera) { Dcamera->resize(1, 6 + DimK); *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - Dother->resize(1, 6+CalibrationB::dimension); + Dother->resize(1, 6 + CalibrationB::dimension); Dother->setZero(); Dother->block(0, 0, 1, 6) = Dother_; } @@ -437,12 +350,9 @@ public: /** * Calculate range to another camera * @param camera Other camera - * @param Dcamera the optionally computed Jacobian with respect to pose - * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ - double range( - const CalibratedCamera& camera, // + double range(const CalibratedCamera& camera, OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); @@ -450,55 +360,13 @@ public: private: - /** - * Calculate Jacobian with respect to pose - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpose Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, - Eigen::MatrixBase const & Dpose) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - double uv = u * v, uu = u * u, vv = v * v; - Matrix26 Dpn_pose; - Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; - assert(Dpose.rows()==2 && Dpose.cols()==6); - const_cast&>(Dpose) =// - Dpi_pn * Dpn_pose; - } - - /** - * Calculate Jacobian with respect to point - * @param pn projection in normalized coordinates - * @param d disparity (inverse depth) - * @param Dpi_pn derivative of uncalibrate with respect to pn - * @param Dpoint Output argument, can be matrix or block, assumed right size ! - * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html - */ - template - static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, - const Matrix2& Dpi_pn, Eigen::MatrixBase const & Dpoint) { - // optimized version of derivatives, see CalibratedCamera.nb - const double u = pn.x(), v = pn.y(); - Matrix23 Dpn_point; - Dpn_point <&>(Dpoint) =// - Dpi_pn * Dpn_point; - } - /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(pose_); + ar + & boost::serialization::make_nvp("PinholeBaseK", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 72e816852..f1e698fc1 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include From 422e8e2cda89b1c3443e34bbf4ee300c35996637 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 12:38:52 +0100 Subject: [PATCH 28/36] Fixed serialization --- gtsam/geometry/PinholePose.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 7bcc22172..8d0cc6ed4 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -161,7 +161,9 @@ 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)); } }; From 19e7b6bf39ba37c19b52155f57ba5c19a9925be2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 12:39:50 +0100 Subject: [PATCH 29/36] Deal with incomplete Pose2 type by including --- gtsam/geometry/tests/testSerializationGeometry.cpp | 2 -- gtsam/geometry/tests/testSimpleCamera.cpp | 10 +++++----- .../nonlinear/tests/testSerializationNonlinear.cpp | 1 + gtsam/slam/expressions.h | 1 + gtsam/slam/tests/testDataset.cpp | 14 +++++++------- gtsam/slam/tests/testLago.cpp | 1 + tests/testManifold.cpp | 3 +-- 7 files changed, 16 insertions(+), 16 deletions(-) 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 From b662c8f644f6ce76613baaf4f380c2a84142be43 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 13:08:22 +0100 Subject: [PATCH 30/36] Got rid of warning --- gtsam/discrete/DecisionTree-inl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9319a1541..1ef3a0e82 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -478,7 +478,7 @@ 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_); From be49d2a11817b5f42033c5527a01205528de9be7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 13:29:03 +0100 Subject: [PATCH 31/36] reinstated getPose as otherwise expressions can't distinguish overloads --- gtsam/geometry/CalibratedCamera.cpp | 2 +- gtsam/geometry/CalibratedCamera.h | 2 +- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/tests/testPinholeCamera.cpp | 4 ++-- gtsam/geometry/tests/testPinholePose.cpp | 20 +++----------------- 5 files changed, 8 insertions(+), 22 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 0e5a6a3ea..33f2c84eb 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -76,7 +76,7 @@ void PinholeBase::print(const string& s) const { } /* ************************************************************************* */ -const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const { +const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const { if (H) { H->setZero(); H->block(0, 0, 6, 6) = I_6x6; diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 34edae2e1..ed0c55208 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -131,7 +131,7 @@ public: } /// return pose, with derivative - const Pose3& pose(OptionalJacobian<6, 6> H) const; + const Pose3& getPose(OptionalJacobian<6, 6> H) const; /// @} /// @name Transformations and measurement functions diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 996313c5c..eb2dceee1 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -142,7 +142,7 @@ public: } /// return pose, with derivative - const Pose3& pose(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; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index f1e698fc1..11c95d822 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -61,11 +61,11 @@ TEST( PinholeCamera, constructor) TEST(PinholeCamera, Pose) { Matrix actualH; - EXPECT(assert_equal(pose, camera.pose(actualH))); + EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative boost::function f = // - boost::bind(&Camera::pose,_1,boost::none); + boost::bind(&Camera::getPose,_1,boost::none); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index c2ba412a4..f0c364955 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -58,32 +58,18 @@ TEST( PinholePose, constructor) } //****************************************************************************** -TEST(PinholePose, Pose) { +TEST(PinholeCamera, Pose) { Matrix actualH; - EXPECT(assert_equal(pose, camera.pose(actualH))); + EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative boost::function f = // - boost::bind(&Camera::pose,_1,boost::none); + 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) { From 5714c56babaaaab996275eee1c6360f0b75e8cdb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 13:30:01 +0100 Subject: [PATCH 32/36] Make examples compile --- examples/Pose2SLAMExample_g2o.cpp | 1 + examples/Pose2SLAMExample_lago.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 564930d74..ee7fe314a 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..ec528559b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace std; From 9d7caa77fe161b5666fd29b18fee681f37cfaaab Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 13:48:15 +0100 Subject: [PATCH 33/36] Try to fix another warning --- gtsam/discrete/DecisionTree-inl.h | 50 +++++++++++++++++-------------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 1ef3a0e82..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()); @@ -483,25 +485,26 @@ namespace gtsam { 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"); +} /*********************************************************************************/ From 4a0891f34cb6b3b626e582348955db616085696c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 13:49:53 +0100 Subject: [PATCH 34/36] Fixed (case) type --- examples/Pose2SLAMExample_g2o.cpp | 2 +- examples/Pose2SLAMExample_lago.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index ee7fe314a..35f9884e1 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index ec528559b..b83dfa1db 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include using namespace std; From 39d474ec686bb27cf8fe150a54084d89fe64350b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:39:50 +0100 Subject: [PATCH 35/36] Fixed cheirality exceptions in arbitrary rotation cases --- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 +- gtsam/geometry/tests/testPinholeCamera.cpp | 14 ++++++++++++++ gtsam/geometry/tests/testPinholePose.cpp | 2 +- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 0f3fac53e..b1e265266 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -118,7 +118,7 @@ TEST( CalibratedCamera, Dproject_point_pose) // 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, 0.5)); + 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); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 11c95d822..0e610d8d6 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -238,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 index f0c364955..411273c1f 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2) // 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, 0.5)); + 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); From 61ae24508d07bc7209ee4288b8e612adcb362b71 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:52:11 +0100 Subject: [PATCH 36/36] Call Base static methods instead --- gtsam/geometry/PinholeCamera.h | 4 ++-- gtsam/geometry/PinholePose.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index eb2dceee1..1edb1a496 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -75,7 +75,7 @@ public: */ static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) { - return PinholeCamera(CalibratedCamera::LevelPose(pose2, height), K); + return PinholeCamera(Base::LevelPose(pose2, height), K); } /// PinholeCamera::level with default calibration @@ -94,7 +94,7 @@ public: */ static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { - return PinholeCamera(CalibratedCamera::LookatPose(eye, target, upVector), K); + return PinholeCamera(Base::LookatPose(eye, target, upVector), K); } /// @} diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 8d0cc6ed4..7588f517e 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -220,7 +220,7 @@ public: */ static PinholePose Level(const boost::shared_ptr& K, const Pose2& pose2, double height) { - return PinholePose(CalibratedCamera::LevelPose(pose2, height), K); + return PinholePose(Base::LevelPose(pose2, height), K); } /// PinholePose::level with default calibration @@ -240,7 +240,7 @@ public: static PinholePose Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const boost::shared_ptr& K = boost::make_shared()) { - return PinholePose(CalibratedCamera::LookatPose(eye, target, upVector), K); + return PinholePose(Base::LookatPose(eye, target, upVector), K); } /// @}