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); }
+/* ************************************************************************* */
+
+