From da8e88d5a153236b50bec155d2e5d140d4e67932 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 13:33:19 +0100 Subject: [PATCH 01/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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/58] 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 96e63fb48008ffe181b2cc0613cef783c35898b9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 14:32:36 +0100 Subject: [PATCH 35/58] Added test for RegularHessianFactor creation --- gtsam/slam/RegularHessianFactor.h | 7 +++ .../tests/testSmartProjectionPoseFactor.cpp | 61 +++++++++++++++++++ 2 files changed, 68 insertions(+) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 8c3df87cf..a738cc256 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -172,6 +172,13 @@ public: } } + /* ************************************************************************* */ + +}; // end class RegularHessianFactor + +// traits +template struct traits > : public Testable< + RegularHessianFactor > { }; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c2855f09b..2fa3f20f4 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -380,6 +380,67 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ EXPECT(assert_equal(bodyPose3,result.at(x3))); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, FactorsWithSensorTransform ){ + + // Default cameras for simple derivatives + SimpleCamera cam1, cam2; + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // one landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + + double rankTol = 1; + double linThreshold = -1; + bool manageDegeneracy = false; + bool enableEPI = false; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + { + // RegularHessianFactor<6> + Matrix66 G11; G11.setZero(); + Matrix66 G12; G12.setZero(); + Matrix66 G22; G22.setZero(); + + Vector6 g1; g1.setZero(); + Vector6 g2; g2.setZero(); + + double f = 10; + + std::vector js; + js.push_back(x1); js.push_back(x2); + std::vector Gs; + Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G22); + std::vector gs; + gs.push_back(g1); gs.push_back(g2); + RegularHessianFactor<6> expected(js, Gs, gs, f); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected,*actual)); + } + +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ From 5120e4b77aa3ee0b04d21fe491df8afccfabeef4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 16:39:31 +0100 Subject: [PATCH 36/58] Constructor --- gtsam/slam/RegularHessianFactor.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index a738cc256..6401eda90 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -43,6 +43,15 @@ public: HessianFactor(js, Gs, gs, f) { } + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, + const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template From c3c5c0d80c802d1aba45f6c353470a1587d3e28d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 16:39:39 +0100 Subject: [PATCH 37/58] Equals --- gtsam/slam/RegularImplicitSchurFactor.h | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 75b2d44ba..95a61dcfc 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -96,11 +96,17 @@ public: /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) - return false; - else { + const This* f = dynamic_cast(&lf); + if (!f) return false; + for (size_t pos = 0; pos < size(); ++pos) { + if (keys_[pos] != f->keys_[pos]) return false; + if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; + if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; } + return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) + && equal_with_abs_tol(E_, f->E_, tol) + && equal_with_abs_tol(b_, f->b_, tol); } /// Degrees of freedom of camera @@ -460,7 +466,12 @@ public: }; -// RegularImplicitSchurFactor +// end class RegularImplicitSchurFactor + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { +}; } From 00d7e707a5b26a2cdd7d0a1a5b393d205c37dbf0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:09:31 +0100 Subject: [PATCH 38/58] printing --- gtsam/slam/RegularImplicitSchurFactor.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 95a61dcfc..bde928723 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -89,9 +89,12 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); - std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; - std::cout << " E_ \n" << E_ << std::endl; - std::cout << " b_ \n" << b_.transpose() << std::endl; + for (size_t pos = 0; pos < size(); ++pos) { + std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + } + std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; + std::cout << "E:\n" << E_ << std::endl; + std::cout << "b:\n" << b_.transpose() << std::endl; } /// equals From 3ccd02e8b108e80368bd2f8b0babdefc3756f2ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:09:43 +0100 Subject: [PATCH 39/58] Two factors unit tested --- .../tests/testSmartProjectionPoseFactor.cpp | 65 ++++++++++++------- 1 file changed, 41 insertions(+), 24 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 2fa3f20f4..38722d88b 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -381,20 +381,21 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, FactorsWithSensorTransform ){ +TEST( SmartProjectionPoseFactor, Factors ){ // Default cameras for simple derivatives - SimpleCamera cam1, cam2; + Rot3 R; + static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); + SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); - // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // - - // one landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); + // one landmarks 1m in front of camera + Point3 landmark1(0,0,10); vector measurements_cam1; // Project 2 landmarks into 2 cameras + cout << cam1.project(landmark1) << endl; + cout << cam2.project(landmark1) << endl; measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1)); @@ -403,39 +404,55 @@ TEST( SmartProjectionPoseFactor, FactorsWithSensorTransform ){ views.push_back(x1); views.push_back(x2); - double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; - bool enableEPI = false; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); + // Make sure triangulation works + LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1,*p)); + { // RegularHessianFactor<6> - Matrix66 G11; G11.setZero(); - Matrix66 G12; G12.setZero(); - Matrix66 G22; G22.setZero(); + Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; + Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; + Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; Vector6 g1; g1.setZero(); Vector6 g2; g2.setZero(); - double f = 10; + double f = 0; - std::vector js; - js.push_back(x1); js.push_back(x2); - std::vector Gs; - Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G22); - std::vector gs; - gs.push_back(g1); gs.push_back(g2); - RegularHessianFactor<6> expected(js, Gs, gs, f); + RegularHessianFactor<6> expected(x1, x2, 50 * G11, 50 * G12, g1, 50 * G22, g2, f); boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(),actual->information(),1e-8)); + CHECK(assert_equal(expected,*actual,1e-8)); + } + + { + // RegularImplicitSchurFactor<6> + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=10; E(1,1)=10; E(2,0)=10; E(2,2)=1;E(3,1)=10; + const vector > Fblocks = list_of > // + (make_pair(x1, F1))(make_pair(x2, F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); CHECK(assert_equal(expected,*actual)); } From 39d474ec686bb27cf8fe150a54084d89fe64350b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:39:50 +0100 Subject: [PATCH 40/58] 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 41/58] 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); } /// @} From 64bb6b77d7ae61051a1be5b3388a72bb255334c5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 06:14:19 +0100 Subject: [PATCH 42/58] Merged in feature/SmartCT (pull request #107) Refactoring of Smart Factors --- doc/.gitignore | 1 + examples/Pose2SLAMExampleExpressions.cpp | 10 +- examples/Pose2SLAMExample_graph.cpp | 9 +- examples/Pose2SLAMExample_graphviz.cpp | 4 +- examples/Pose2SLAMwSPCG.cpp | 69 +- examples/SFMExample.cpp | 8 +- examples/SFMExample_SmartFactor.cpp | 58 +- examples/SFMExample_SmartFactorPCG.cpp | 126 +++ gtsam.h | 10 +- gtsam/geometry/CameraSet.h | 123 +++ gtsam/geometry/PinholeCamera.h | 8 + gtsam/geometry/StereoCamera.cpp | 5 +- gtsam/geometry/StereoCamera.h | 61 +- gtsam/geometry/tests/testCameraSet.cpp | 114 +++ gtsam/linear/ConjugateGradientSolver.cpp | 24 +- gtsam/linear/ConjugateGradientSolver.h | 9 +- gtsam/linear/IterativeSolver.cpp | 101 ++- gtsam/linear/IterativeSolver.h | 231 +++-- gtsam/linear/PCGSolver.cpp | 79 +- gtsam/linear/PCGSolver.h | 57 +- gtsam/linear/SubgraphSolver.cpp | 128 +-- gtsam/linear/SubgraphSolver.h | 104 ++- .../NonlinearConjugateGradientOptimizer.cpp | 48 +- .../NonlinearConjugateGradientOptimizer.h | 163 ++-- gtsam/slam/JacobianFactorQ.h | 11 + gtsam/slam/JacobianFactorQR.h | 7 +- gtsam/slam/JacobianSchurFactor.h | 15 +- gtsam/slam/RegularHessianFactor.h | 27 +- gtsam/slam/RegularImplicitSchurFactor.h | 4 +- gtsam/slam/RegularJacobianFactor.h | 45 +- gtsam/slam/SmartFactorBase.h | 538 ++++++----- gtsam/slam/SmartProjectionFactor.h | 120 ++- gtsam/slam/SmartProjectionPoseFactor.h | 21 +- gtsam/slam/tests/testRegularHessianFactor.cpp | 22 +- .../tests/testRegularImplicitSchurFactor.cpp | 20 +- .../slam/tests/testRegularJacobianFactor.cpp | 17 +- gtsam/slam/tests/testSmartFactorBase.cpp | 71 ++ .../tests/testSmartProjectionPoseFactor.cpp | 858 ++++++++++++------ .../examples/SmartProjectionFactorExample.cpp | 2 +- .../SmartStereoProjectionFactorExample.cpp | 2 +- .../slam/SmartStereoProjectionFactor.h | 50 +- .../slam/SmartStereoProjectionPoseFactor.h | 21 +- .../testSmartStereoProjectionPoseFactor.cpp | 427 +++++---- tests/testGradientDescentOptimizer.cpp | 84 +- tests/testPCGSolver.cpp | 10 - tests/testPreconditioner.cpp | 7 +- 46 files changed, 2489 insertions(+), 1440 deletions(-) create mode 100644 doc/.gitignore create mode 100644 examples/SFMExample_SmartFactorPCG.cpp create mode 100644 gtsam/geometry/CameraSet.h create mode 100644 gtsam/geometry/tests/testCameraSet.cpp create mode 100644 gtsam/slam/tests/testSmartFactorBase.cpp diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..ac7af2e80 --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1 @@ +/html/ diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 92f94c3f3..1f6de6cb1 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -14,20 +14,18 @@ * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert - * @author Yong Dian Jian */ // The two new headers that allow using our Automatic Differentiation Expression framework #include #include -// Header order is close to far -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp +#include +#include +#include #include #include -#include -#include -#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 46ca02ee4..0c64634c5 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -16,11 +16,14 @@ * @author Frank Dellaert */ -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include -#include +#include #include -#include +#include + +// This new header allows us to read examples easily from .graph files +#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 818a9e577..314ccbdb4 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -16,11 +16,11 @@ * @author Frank Dellaert */ +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include -#include -#include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 4422586b0..2c25d2f8e 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,47 +16,15 @@ * @date June 2, 2012 */ -/** - * A simple 2D pose slam example solved using a Conjugate-Gradient method - * - The robot moves in a 2 meter square - * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) - * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position - */ - -// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent -// the robot positions -#include -#include - -// Each variable in the system (poses) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use simple integer keys -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +#include #include +// In contrast to that example, however, we will use a PCG solver here +#include + using namespace std; using namespace gtsam; @@ -66,32 +34,24 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. - // We will use another Between Factor to enforce this constraint, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); @@ -104,15 +64,18 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "subgraph solver final error = " << graph.error(result) << endl; - } + // LM is still the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.iterativeParams = boost::make_shared(); + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + cout << "subgraph solver final error = " << graph.error(result) << endl; return 0; } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..38dd1ca81 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -15,13 +15,7 @@ * @author Duy-Nguyen Ta */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data +// For loading the data, see the comments therein for scenario (camera rotates around cube) #include "SFMdata.h" // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b999e6600..fce046a59 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,51 +17,22 @@ * @author Frank Dellaert */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data -#include "SFMdata.h" - -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -#include - // In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, -// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, -// The calibration should be known. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. #include -// Also, we will initialize the robot at some location using a Prior factor. -#include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" +#include using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -127,7 +98,8 @@ int main(int argc, char* argv[]) { initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); result.print("Final results:\n"); // A smart factor represent the 3D point inside the factor, not as a variable. @@ -136,20 +108,20 @@ int main(int argc, char* argv[]) { Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point; - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - point = smart->point(result); + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional point = smart->point(result); if (point) // ignore if boost::optional return NULL landmark_result.insert(j, *point); } } landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; return 0; } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp new file mode 100644 index 000000000..49482292f --- /dev/null +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample_SmartFactorPCG.cpp + * @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient + * @author Frank Dellaert + */ + +// For an explanation of these headers, see SFMExample_SmartFactor.cpp +#include "SFMdata.h" +#include + +// These extra headers allow us a LM outer loop with PCG linear solver (inner loop) +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t j = 0; j < points.size(); ++j) { + + // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor()); + + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we need to add: + smartfactor->add(measurement, i, measurementNoise, K); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); + + // Fix the scale ambiguity by adding a prior + graph.push_back(PriorFactor(1, poses[1], poseNoise)); + + // Create the initial estimate to the solution + Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + + // We will use LM in the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + LevenbergMarquardtParams parameters; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.absoluteErrorTol = 1e-10; + parameters.relativeErrorTol = 1e-10; + parameters.maxIterations = 500; + PCGSolverParameters::shared_ptr pcg = + boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + // Following is crucial: + pcg->setEpsilon_abs(1e-10); + pcg->setEpsilon_rel(1e-10); + parameters.iterativeParams = pcg; + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + + // Display result as in SFMExample_SmartFactor.run + result.print("Final results:\n"); + Values landmark_result; + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smart = // + boost::dynamic_pointer_cast(graph[j]); + if (smart) { + boost::optional point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } + } + + landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/gtsam.h b/gtsam.h index c5528309e..f8e0af28e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2310,23 +2310,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include -template +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor); SmartProjectionPoseFactor(double rankTol); SmartProjectionPoseFactor(); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i, + const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..3a34ca1fd --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 CameraSet.h + * @brief Base class to create smart factors on poses or cameras + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#pragma once + +#include +#include // for Cheirality exception +#include +#include + +namespace gtsam { + +/** + * @brief A set of cameras, all with their own calibration + * Assumes that a camera is laid out as 6 Pose3 parameters then calibration + */ +template +class CameraSet: public std::vector { + +protected: + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + + static const int ZDim = traits::dimension; ///< Measurement dimension + static const int Dim = traits::dimension; ///< Camera dimension + +public: + + /// Definitions for blocks of F + typedef Eigen::Matrix MatrixZD; // F + typedef std::pair FBlock; // Fblocks + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) + this->at(k).print(); + } + + /// equals + virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + if (this->size() != p.size()) + return false; + bool camerasAreEqual = true; + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).equals(p.at(i), tol) == false) + camerasAreEqual = false; + break; + } + return camerasAreEqual; + } + + /** + * Project a point, with derivatives in this, point, and calibration + * throws CheiralityException + */ + std::vector project(const Point3& point, boost::optional F = + boost::none, boost::optional E = boost::none, + boost::optional H = boost::none) const { + + size_t nrCameras = this->size(); + if (F) F->resize(ZDim * nrCameras, 6); + if (E) E->resize(ZDim * nrCameras, 3); + if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6); + std::vector z(nrCameras); + + for (size_t i = 0; i < nrCameras; i++) { + Eigen::Matrix Fi; + Eigen::Matrix Ei; + Eigen::Matrix Hi; + z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); + if (F) F->block(ZDim * i, 0) = Fi; + if (E) E->block(ZDim * i, 0) = Ei; + if (H) H->block(ZDim * i, 0) = Hi; + } + return z; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & (*this); + } +}; + +template +const int CameraSet::ZDim; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 1edb1a496..bfc2bbb93 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,6 +31,14 @@ namespace gtsam { template class GTSAM_EXPORT PinholeCamera: public PinholeBaseK { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + private: typedef PinholeBaseK Base; ///< base class has 3D pose as private member diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9170f8282..eb83fd10f 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -31,7 +31,7 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,6> H3) const { + OptionalJacobian<3,0> H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -78,8 +78,7 @@ namespace gtsam { } #endif if (H3) - // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet - H3->setZero(); + throw std::runtime_error("StereoCamera::project does not support third derivative yet"); } // finally translate diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 913b1eab3..88ffbcdbd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -28,32 +28,47 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} + StereoCheiralityException() : + std::runtime_error("Stereo Cheirality Exception") { + } }; - /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry */ class GTSAM_EXPORT StereoCamera { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef StereoPoint2 Measurement; + private: Pose3 leftCamPose_; Cal3_S2Stereo::shared_ptr K_; public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ - StereoCamera() { + /// Default constructor allocates a calibration! + StereoCamera() : + K_(new Cal3_S2Stereo()) { } + /// Construct from pose and shared calibration StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + /// Return shared pointer to calibration const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -62,26 +77,28 @@ public: /// @name Testable /// @{ + /// print void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_->print(s + ".calibration."); } + /// equals bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) + && K_->equals(*camera.K_, tol); } /// @} /// @name Manifold /// @{ - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space inline size_t dim() const { return 6; } - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space static inline size_t Dim() { return 6; } @@ -100,10 +117,12 @@ public: /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } @@ -114,19 +133,16 @@ public: * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 6> H3 = boost::none) const; + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 0> H3 = boost::none) const; - /** - * - */ + /// back-project a measurement Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); return world_point; } @@ -134,7 +150,8 @@ public: /// @} private: - /** utility functions */ + + /// utility function Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; @@ -147,8 +164,10 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; } diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp new file mode 100644 index 000000000..42cf0f299 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(CameraSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + CameraSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + CHECK(assert_equal(set, set)); + CameraSet set2 = set; + set2.push_back(camera); + CHECK(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix46 actualF; + Matrix43 actualE; + Matrix43 actualH; + { + Matrix26 F1; + Matrix23 E1; + Matrix23 H1; + camera.project(p, F1, E1, H1); + actualE << E1, E1; + actualF << F1, F1; + actualH << H1, H1; + } + + // Check computed derivatives + Matrix F, E, H; + set.project(p, F, E, H); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); + CHECK(assert_equal(actualH, H)); +} + +/* ************************************************************************* */ +#include +TEST(CameraSet, Stereo) { + typedef vector ZZ; + CameraSet set; + StereoCamera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + StereoPoint2 expected(0, -1, 0); + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix66 actualF; + Matrix63 actualE; + { + Matrix36 F1; + Matrix33 E1; + camera.project(p, F1, E1); + actualE << E1, E1; + actualF << F1, F1; + } + + // Check computed derivatives + Matrix F, E; + set.project(p, F, E); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 5e7e08f61..a1b9e2d83 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -1,8 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * ConjugateGradientSolver.cpp - * - * Created on: Jun 4, 2014 - * Author: Yong-Dian Jian + * @file ConjugateGradientSolver.cpp + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 */ #include @@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) } /*****************************************************************************/ -ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator( + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; @@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla return ConjugateGradientParameters::GTSAM; } +/*****************************************************************************/ } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 20e0c5262..2596a7403 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,7 +20,6 @@ #pragma once #include -#include namespace gtsam { @@ -87,9 +86,8 @@ public: static BLASKernel blasTranslator(const std::string &s) ; }; -/*********************************************************************************************/ /* - * A template of linear preconditioned conjugate gradient method. + * A template for the linear preconditioned conjugate gradient method. * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. @@ -98,8 +96,9 @@ public: * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ -template -V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { +template +V preconditionedConjugateGradient(const S &system, const V &initial, + const ConjugateGradientParameters ¶meters) { V estimate, residual, direction, q1, q2; estimate = residual = direction = q1 = q2 = initial; diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..79ade1c8a 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -1,6 +1,17 @@ +/* ---------------------------------------------------------------------------- + + * 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 IterativeSolver.cpp - * @brief + * @brief Some support classes for iterative solvers * @date Sep 3, 2012 * @author Yong-Dian Jian */ @@ -9,18 +20,22 @@ #include #include #include +#include #include -#include using namespace std; namespace gtsam { /*****************************************************************************/ -string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +string IterativeOptimizationParameters::getVerbosity() const { + return verbosityTranslator(verbosity_); +} /*****************************************************************************/ -void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const string &src) { + verbosity_ = verbosityTranslator(src); +} /*****************************************************************************/ void IterativeOptimizationParameters::print() const { @@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const { /*****************************************************************************/ void IterativeOptimizationParameters::print(ostream &os) const { - os << "IterativeOptimizationParameters:" << endl - << "verbosity: " << verbosityTranslator(verbosity_) << endl; + os << "IterativeOptimizationParameters:" << endl << "verbosity: " + << verbosityTranslator(verbosity_) << endl; } /*****************************************************************************/ - ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { +ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); return os; } /*****************************************************************************/ -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { - string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return IterativeOptimizationParameters::SILENT; - else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; - else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator( + const string &src) { + string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") + return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") + return IterativeOptimizationParameters::ERROR; /* default is default */ - else return IterativeOptimizationParameters::SILENT; + else + return IterativeOptimizationParameters::SILENT; } /*****************************************************************************/ - string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { - if (verbosity == SILENT) return "SILENT"; - else if (verbosity == COMPLEXITY) return "COMPLEXITY"; - else if (verbosity == ERROR) return "ERROR"; - else return "UNKNOWN"; +string IterativeOptimizationParameters::verbosityTranslator( + IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) + return "SILENT"; + else if (verbosity == COMPLEXITY) + return "COMPLEXITY"; + else if (verbosity == ERROR) + return "ERROR"; + else + return "UNKNOWN"; } /*****************************************************************************/ -VectorValues IterativeSolver::optimize( - const GaussianFactorGraph &gfg, +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, boost::optional keyInfo, - boost::optional&> lambda) -{ - return optimize( - gfg, - keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); } /*****************************************************************************/ -VectorValues IterativeSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda) -{ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) - : ordering_(ordering) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : + ordering_(ordering) { initialize(fg); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) : + ordering_(Ordering::Natural(fg)) { initialize(fg); } /****************************************************************************/ -void KeyInfo::initialize(const GaussianFactorGraph &fg){ +void KeyInfo::initialize(const GaussianFactorGraph &fg) { const map colspec = fg.getKeyDimMap(); const size_t n = ordering_.size(); size_t start = 0; - for ( size_t i = 0 ; i < n ; ++i ) { + for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; insert(make_pair(key, KeyInfoEntry(i, dim, start))); - start += dim ; + start += dim; } numCols_ = start; } @@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){ /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -117,7 +136,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; @@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const { return Vector::Zero(numCols_); } - } - diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index d6e1b6e98..442a5fc6b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -9,131 +9,178 @@ * -------------------------------------------------------------------------- */ +/** + * @file IterativeSolver.h + * @brief Some support classes for iterative solvers + * @date 2010 + * @author Yong-Dian Jian + */ + #pragma once -#include -#include #include +#include + #include -#include -#include +#include +#include + #include -#include #include -#include +#include namespace gtsam { - // Forward declarations - class KeyInfo; - class KeyInfoEntry; - class GaussianFactorGraph; - class Values; - class VectorValues; +// Forward declarations +class KeyInfo; +class KeyInfoEntry; +class GaussianFactorGraph; +class Values; +class VectorValues; - /************************************************************************************/ - /** - * parameters for iterative linear solvers - */ - class GTSAM_EXPORT IterativeOptimizationParameters { +/** + * parameters for iterative linear solvers + */ +class GTSAM_EXPORT IterativeOptimizationParameters { - public: +public: - typedef boost::shared_ptr shared_ptr; - enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; + typedef boost::shared_ptr shared_ptr; + enum Verbosity { + SILENT = 0, COMPLEXITY, ERROR + } verbosity_; - public: +public: - IterativeOptimizationParameters(Verbosity v = SILENT) - : verbosity_(v) {} + IterativeOptimizationParameters(Verbosity v = SILENT) : + verbosity_(v) { + } - virtual ~IterativeOptimizationParameters() {} + virtual ~IterativeOptimizationParameters() { + } - /* utility */ - inline Verbosity verbosity() const { return verbosity_; } - std::string getVerbosity() const; - void setVerbosity(const std::string &s) ; + /* utility */ + inline Verbosity verbosity() const { + return verbosity_; + } + std::string getVerbosity() const; + void setVerbosity(const std::string &s); - /* matlab interface */ - void print() const ; + /* matlab interface */ + void print() const; - /* virtual print function */ - virtual void print(std::ostream &os) const ; + /* virtual print function */ + virtual void print(std::ostream &os) const; - /* for serialization */ - friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, + const IterativeOptimizationParameters &p); - static Verbosity verbosityTranslator(const std::string &s); - static std::string verbosityTranslator(Verbosity v); - }; + static Verbosity verbosityTranslator(const std::string &s); + static std::string verbosityTranslator(Verbosity v); +}; - /************************************************************************************/ - class GTSAM_EXPORT IterativeSolver { - public: - typedef boost::shared_ptr shared_ptr; - IterativeSolver() {} - virtual ~IterativeSolver() {} +/** + * Base class for Iterative Solvers like SubgraphSolver + */ +class GTSAM_EXPORT IterativeSolver { +public: + typedef boost::shared_ptr shared_ptr; + IterativeSolver() { + } + virtual ~IterativeSolver() { + } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, - boost::optional&> lambda = boost::none - ); + boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda - ); + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, + const std::map &lambda); - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) = 0; + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) = 0; - }; +}; - /************************************************************************************/ - /* Handy data structure for iterative solvers - * key to (index, dimension, colstart) */ - class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { - public: - typedef boost::tuple Base; - KeyInfoEntry(){} - KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - size_t index() const { return this->get<0>(); } - size_t dim() const { return this->get<1>(); } - size_t colstart() const { return this->get<2>(); } - }; +/** + * Handy data structure for iterative solvers + * key to (index, dimension, colstart) + */ +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - /************************************************************************************/ - class GTSAM_EXPORT KeyInfo : public std::map { - public: - typedef std::map Base; - KeyInfo() : numCols_(0) {} - KeyInfo(const GaussianFactorGraph &fg); - KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); +public: - std::vector colSpec() const ; - VectorValues x0() const; - Vector x0vector() const; + typedef boost::tuple Base; - inline size_t numCols() const { return numCols_; } - inline const Ordering & ordering() const { return ordering_; } + KeyInfoEntry() { + } + KeyInfoEntry(size_t idx, size_t d, Key start) : + Base(idx, d, start) { + } + size_t index() const { + return this->get<0>(); + } + size_t dim() const { + return this->get<1>(); + } + size_t colstart() const { + return this->get<2>(); + } +}; - protected: +/** + * Handy data structure for iterative solvers + */ +class GTSAM_EXPORT KeyInfo: public std::map { - void initialize(const GaussianFactorGraph &fg); +public: - Ordering ordering_; - size_t numCols_; + typedef std::map Base; - }; +protected: + Ordering ordering_; + size_t numCols_; -} + void initialize(const GaussianFactorGraph &fg); + +public: + + /// Default Constructor + KeyInfo() : + numCols_(0) { + } + + /// Construct from Gaussian factor graph, use "Natural" ordering + KeyInfo(const GaussianFactorGraph &fg); + + /// Construct from Gaussian factor graph and a given ordering + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + /// Return the total number of columns (scalar variables = sum of dimensions) + inline size_t numCols() const { + return numCols_; + } + + /// Return the ordering + inline const Ordering & ordering() const { + return ordering_; + } + + /// Return a vector of dimensions ordered by ordering() + std::vector colSpec() const; + + /// Return VectorValues with zeros, of correct dimension + VectorValues x0() const; + + /// Return zero Vector of correct dimension + Vector x0vector() const; + +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 3698edc2f..caf7d0095 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -1,16 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.cpp - * - * Created on: Feb 14, 2012 - * Author: Yong-Dian Jian - * Author: Sungtae An + * @file PCGSolver.cpp + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Feb 14, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ -#include #include +#include #include +#include +#include #include + +#include #include #include @@ -21,7 +36,7 @@ namespace gtsam { /*****************************************************************************/ void PCGSolverParameters::print(ostream &os) const { Base::print(os); - os << "PCGSolverParameters:" << endl; + os << "PCGSolverParameters:" << endl; preconditioner_->print(os); } @@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { } /*****************************************************************************/ -VectorValues PCGSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial) -{ +VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { /* build preconditioner */ preconditioner_->build(gfg, keyInfo, lambda); /* apply pcg */ - const Vector sol = preconditionedConjugateGradient( - GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), - initial.vector(keyInfo.ordering()), parameters_); + GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda); + Vector x0 = initial.vector(keyInfo.ordering()); + const Vector sol = preconditionedConjugateGradient(system, x0, parameters_); return buildVectorValues(sol, keyInfo); } /*****************************************************************************/ GaussianFactorGraphSystem::GaussianFactorGraphSystem( - const GaussianFactorGraph &gfg, - const Preconditioner &preconditioner, - const KeyInfo &keyInfo, - const std::map &lambda) - : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + const GaussianFactorGraph &gfg, const Preconditioner &preconditioner, + const KeyInfo &keyInfo, const std::map &lambda) : + gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_( + lambda) { +} /*****************************************************************************/ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { @@ -67,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { /* substract A*x */ Vector Ax = Vector::Zero(r.rows(), 1); multiply(x, Ax); - r -= Ax ; + r -= Ax; } /*****************************************************************************/ @@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ // Build a VectorValues for Vector x - VectorValues vvX = buildVectorValues(x,keyInfo_); + VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; @@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const { } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-1} x preconditioner_.solve(x, y); } /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-T} x preconditioner_.transposeSolve(x, y); } /**********************************************************************************/ -VectorValues buildVectorValues(const Vector &v, - const Ordering &ordering, - const map & dimensions) { +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const map & dimensions) { VectorValues result; DenseIndex offset = 0; - for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + for (size_t i = 0; i < ordering.size(); ++i) { const Key &key = ordering[i]; map::const_iterator it = dimensions.find(key); - if ( it == dimensions.end() ) { - throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + if (it == dimensions.end()) { + throw invalid_argument( + "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; result.insert(key, v.segment(offset, dim)); @@ -137,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { - result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + result.insert(item.first, + v.segment(item.second.colstart(), item.second.dim())); } return result; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 0201000ad..f5b278ae5 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -1,20 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.h - * - * Created on: Jan 31, 2012 - * Author: Yong-Dian Jian + * @file PCGSolver.h + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Jan 31, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ #pragma once -#include #include -#include -#include - -#include -#include -#include #include namespace gtsam { @@ -22,15 +27,19 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class Preconditioner; +class VectorValues; struct PreconditionerParameters; -/*****************************************************************************/ +/** + * Parameters for PCG + */ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; - PCGSolverParameters() {} + PCGSolverParameters() { + } virtual void print(std::ostream &os) const; @@ -42,8 +51,9 @@ public: boost::shared_ptr preconditioner_; }; -/*****************************************************************************/ -/* A virtual base class for the preconditioned conjugate gradient solver */ +/** + * A virtual base class for the preconditioned conjugate gradient solver + */ class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; @@ -57,7 +67,8 @@ protected: public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); - virtual ~PCGSolver() {} + virtual ~PCGSolver() { + } using IterativeSolver::optimize; @@ -67,7 +78,9 @@ public: }; -/*****************************************************************************/ +/** + * System class needed for calling preconditionedConjugateGradient + */ class GTSAM_EXPORT GaussianFactorGraphSystem { public: @@ -97,13 +110,17 @@ public: void getb(Vector &b) const; }; -/* utility functions */ -/**********************************************************************************/ +/// @name utility functions +/// @{ + +/// Create VectorValues from a Vector VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const std::map & dimensions); -/**********************************************************************************/ +/// Create VectorValues from a Vector and a KeyInfo class VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); +/// @} + } diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 6f10d04ad..3c7256c29 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -9,73 +9,81 @@ * -------------------------------------------------------------------------- */ -#include -#include +/** + * @file SubgraphSolver.cpp + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +#include #include #include #include #include -#include -#include -#include #include -#include -#include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(*jfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, Ab2); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) : -parameters_(parameters), ordering_(ordering) -{ + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, Ab2); } /**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); + VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } @@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) -{ - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { + GaussianFactorGraph::shared_ptr Ab1 = + boost::make_shared(), Ab2 = boost::make_shared< + GaussianFactorGraph>(); - boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + boost::tie(Ab1, Ab2) = splitGraph(jfg); if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) -{ - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2) { + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -boost::tuple +boost::tuple // SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); @@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - if ( gf->keys().size() > 2 ) { - throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + if (gf->keys().size() > 2) { + throw runtime_error( + "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } - bool augment = false ; + bool augment = false; /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; + if (gf->keys().size() == 1) + augment = true; else { - const Key u = gf->keys()[0], v = gf->keys()[1], - u_root = D.findSet(u), v_root = D.findSet(v); - if ( u_root != v_root ) { - t++; augment = true ; + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), + v_root = D.findSet(v); + if (u_root != v_root) { + t++; + augment = true; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if (augment) + At->push_back(gf); + else + Ac->push_back(gf); } return boost::tie(At, Ac); } /****************************************************************************/ -VectorValues SubgraphSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial -) { return VectorValues(); } +VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { + return VectorValues(); +} } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index ac8a9da87..318c029f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -9,27 +9,37 @@ * -------------------------------------------------------------------------- */ +/** + * @file SubgraphSolver.h + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + #pragma once - #include -#include -#include -#include namespace gtsam { - // Forward declarations - class GaussianFactorGraph; - class GaussianBayesNet; - class SubgraphPreconditioner; +// Forward declarations +class GaussianFactorGraph; +class GaussianBayesNet; +class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { +class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : Base() {} - void print() const { Base::print(); } - virtual void print(std::ostream &os) const { Base::print(os); } + SubgraphSolverParameters() : + Base() { + } + void print() const { + Base::print(); + } + virtual void print(std::ostream &os) const { + Base::print(os); + } }; /** @@ -53,8 +63,7 @@ public: * * \nosubgrouping */ - -class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { +class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { public: typedef SubgraphSolverParameters Parameters; @@ -62,41 +71,64 @@ public: protected: Parameters parameters_; Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: - /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ - SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering& ordering); - /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Ab1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, + const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering& ordering); + + /** + * The user specify the subgraph part and the constraint part + * may throw exception if A1 is underdetermined + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters, const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); /* The same as above, but the A1 is solved before */ - SubgraphSolver(const boost::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering); - virtual ~SubgraphSolver() {} + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); - VectorValues optimize () ; - VectorValues optimize (const VectorValues &initial) ; + /// Destructor + virtual ~SubgraphSolver() { + } - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) ; + /// Optimize from zero + VectorValues optimize(); + + /// Optimize from given initial values + VectorValues optimize(const VectorValues &initial); + + /** Interface that IterativeSolver subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); protected: void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2); + void initialize(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2); - boost::tuple, boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg) ; + boost::tuple, + boost::shared_ptr > + splitGraph(const GaussianFactorGraph &gfg); }; } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 107ec7d3f..6d7196ff5 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -16,36 +27,49 @@ using namespace std; namespace gtsam { -/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering - * Can be moved to NonlinearFactorGraph.h if desired */ -VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { +/** + * @brief Return the gradient vector of a nonlinear factor graph + * @param nfg the graph + * @param values a linearization point + * Can be moved to NonlinearFactorGraph.h if desired + */ +static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, + const Values &values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { +double NonlinearConjugateGradientOptimizer::System::error( + const State &state) const { return graph_.error(state); } -NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { +NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient( + const State &state) const { return gradientInPlace(graph_, state); } -NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { + +NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance( + const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step *= alpha; return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - int dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); + int dummy; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient( + System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); + // Optimize until convergence + System system(graph_); + boost::tie(state_.values, state_.iterations) = // + nonlinearConjugateGradient(system, state_.values, params_, false); state_.error = graph_.error(state_.values); return state_.values; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 1ad8fa2ab..04d4734a4 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief + * @file NonlinearConjugateGradientOptimizer.h + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG * @author Yong-Dian Jian - * @date Jun 11, 2012 + * @date June 11, 2012 */ #pragma once @@ -13,15 +24,18 @@ namespace gtsam { -/** An implementation of the nonlinear cg method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState { +/** An implementation of the nonlinear CG method using the template below */ +class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { public: typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values) - : Base(graph, values) {} + NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, + const Values& values) : + Base(graph, values) { + } }; -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { + /* a class for the nonlinearConjugateGradient template */ class System { public: @@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz const NonlinearFactorGraph &graph_; public: - System(const NonlinearFactorGraph &graph): graph_(graph) {} - double error(const State &state) const ; - Gradient gradient(const State &state) const ; - State advance(const State ¤t, const double alpha, const Gradient &g) const ; + System(const NonlinearFactorGraph &graph) : + graph_(graph) { + } + double error(const State &state) const; + Gradient gradient(const State &state) const; + State advance(const State ¤t, const double alpha, + const Gradient &g) const; }; public: + typedef NonlinearOptimizer Base; - typedef NonlinearConjugateGradientState States; typedef NonlinearOptimizerParams Parameters; typedef boost::shared_ptr shared_ptr; protected: - States state_; + NonlinearConjugateGradientState state_; Parameters params_; public: - NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params) {} + /// Constructor + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Parameters& params = Parameters()) : + Base(graph), state_(graph, initialValues), params_(params) { + } + + /// Destructor + virtual ~NonlinearConjugateGradientOptimizer() { + } - virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); - virtual const Values& optimize (); - virtual const NonlinearOptimizerState& _state() const { return state_; } - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const Values& optimize(); + virtual const NonlinearOptimizerState& _state() const { + return state_; + } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } }; /** Implement the golden-section line search algorithm */ -template +template double lineSearch(const S &system, const V currentValues, const W &gradient) { /* normalize it such that it becomes a unit vector */ @@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { // perform the golden section search algorithm to decide the the optimal step size // detail refer to http://en.wikipedia.org/wiki/Golden_section_search - const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5; - double minStep = -1.0/g, maxStep = 0, - newStep = minStep + (maxStep-minStep) / (phi+1.0) ; + const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = + 1e-5; + double minStep = -1.0 / g, maxStep = 0, newStep = minStep + + (maxStep - minStep) / (phi + 1.0); V newValues = system.advance(currentValues, newStep, gradient); double newError = system.error(newValues); while (true) { - const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ; - const double testStep = flag ? - newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep); + const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; + const double testStep = + flag ? newStep + resphi * (maxStep - newStep) : + newStep - resphi * (newStep - minStep); - if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) { - return 0.5*(minStep+maxStep); + if ((maxStep - minStep) + < tau * (std::fabs(testStep) + std::fabs(newStep))) { + return 0.5 * (minStep + maxStep); } const V testValues = system.advance(currentValues, testStep, gradient); const double testError = system.error(testValues); // update the working range - if ( testError >= newError ) { - if ( flag ) maxStep = testStep; - else minStep = testStep; - } - else { - if ( flag ) { + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { minStep = newStep; newStep = testStep; newError = testError; - } - else { + } else { maxStep = newStep; newStep = testStep; newError = testError; @@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in + * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. * * The S (system) class requires three member functions: error(state), gradient(state) and @@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * * The last parameter is a switch between gradient-descent and conjugate gradient */ -template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +template +boost::tuple nonlinearConjugateGradient(const S &system, + const V &initial, const NonlinearOptimizerParams ¶ms, + const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); @@ -129,57 +160,67 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initia // check if we're already close enough double currentError = system.error(initial); - if(currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR){ - std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl; + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; } return boost::tie(initial, iteration); } V currentValues = initial; - typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, - direction = currentGradient; + typename S::Gradient currentGradient = system.gradient(currentValues), + prevGradient, direction = currentGradient; /* do one step of gradient descent */ - V prevValues = currentValues; double prevError = currentError; + V prevValues = currentValues; + double prevError = currentError; double alpha = lineSearch(system, currentValues, direction); currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - if ( gradientDescent == true) { + if (gradientDescent == true) { direction = system.gradient(currentValues); - } - else { + } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient)); - direction = currentGradient + (beta*direction); + // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 + const double beta = std::max(0.0, + currentGradient.dot(currentGradient - prevGradient) + / prevGradient.dot(prevGradient)); + direction = currentGradient + (beta * direction); } alpha = lineSearch(system, currentValues, direction); - prevValues = currentValues; prevError = currentError; + prevValues = currentValues; + prevError = currentError; currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; - } while( ++iteration < params.maxIterations && - !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; + } while (++iteration < params.maxIterations && !singleIteration + && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, + params.errorTol, prevError, currentError, params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations) - std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR + && iteration >= params.maxIterations) + std::cout + << "nonlinearConjugateGradient: Terminating because reached maximum iterations" + << std::endl; return boost::tie(currentValues, iteration); } -} +} // \ namespace gtsam diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 923edddb7..49f5513b6 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianFactorQ.h * @date Oct 27, 2013 diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index ccd6e8756..112278766 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -7,8 +7,13 @@ #pragma once #include +#include +#include namespace gtsam { + +class GaussianBayesNet; + /** * JacobianFactor for Schur complement that uses Q noise model */ @@ -38,7 +43,7 @@ public: //gfg.print("gfg"); // eliminate the point - GaussianBayesNet::shared_ptr bn; + boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; std::vector < Key > variables; variables.push_back(pointKey); diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index 5d28bbada..d31eea05b 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianSchurFactor.h * @brief Jacobianfactor that combines and eliminates points @@ -7,11 +18,7 @@ #pragma once -#include -#include #include -#include -#include #include namespace gtsam { diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 6401eda90..c272eac8e 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -65,13 +65,14 @@ public: mutable std::vector y; /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{ - throw std::runtime_error( - "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead"); + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); } /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); BOOST_FOREACH(DVector & yi, y) @@ -104,6 +105,7 @@ public: } } + /// Raw memory version, with offsets TODO document reasoning void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { @@ -140,43 +142,38 @@ public: // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; + DMap(yvalues + offsets[keys_[i]], + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; } /** Return the diagonal of the Hessian for this factor (raw memory version) */ virtual void hessianDiagonal(double* d) const { // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal const Matrix& B = info_(pos, pos).selfadjointView(); - //DMap(d + 9 * j) += B.diagonal(); DMap(d + D * j) += B.diagonal(); } } - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + /// Add gradient at zero to d TODO: is it really the goal to add ?? virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos,size()).knownOffDiagonal(); - //DMap(d + 9 * j) += dj; + VectorD dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index bde928723..b1490d465 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,12 +7,10 @@ #pragma once -#include #include #include #include -#include -#include +#include namespace gtsam { diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index bb275ef3f..dcf709854 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -52,11 +52,17 @@ public: * factor. */ template RegularJacobianFactor(const KEYS& keys, - const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()) : + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = + SharedDiagonal()) : JacobianFactor(keys, augmentedMatrix, sigmas) { } + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x * Note: this is not assuming a fixed dimension for the variables, * but requires the vector accumulatedDims to tell the dimension of @@ -78,10 +84,10 @@ public: /// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) - for (size_t pos = 0; pos < size(); ++pos) - { + for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstMapD(x + accumulatedDims[keys_[pos]], + accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } /// Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { @@ -93,8 +99,9 @@ public: Ax *= alpha; /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + for (size_t pos = 0; pos < size(); ++pos) { + MapD(y + accumulatedDims[keys_[pos]], + accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } @@ -102,21 +109,25 @@ public: /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { - if (empty()) return; + if (empty()) + return; Vector Ax = zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } // multiply with alpha Ax *= alpha; // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; poswhiten(column_k); dj(k) = dot(column_k, column_k); - }else{ + } else { dj(k) = Ab_(j).col(k).squaredNorm(); } } @@ -156,13 +167,13 @@ public: } // Just iterate over all A matrices - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column - for (size_t k = 0; k < D; ++k){ + for (size_t k = 0; k < D; ++k) { Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b, column_k); + dj(k) = -1.0 * dot(b, column_k); } DMap(d + D * j) += dj; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 44d6902fa..a2bdfc059 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -25,30 +25,41 @@ #include #include -#include // for Cheirality exception -#include -#include -#include +#include #include #include #include namespace gtsam { -/// Base class with no internal point, completely functional -template + +/** + * @brief Base class for smart factors + * This base class has no internal point, but it has a measurement, noise model + * and an optional sensor pose. + * This class mainly computes the derivatives and returns them as a variety of factors. + * The methods take a Cameras argument, which should behave like PinholeCamera, and + * the value of a point, which is kept in the base class. + */ +template class SmartFactorBase: public NonlinearFactor { protected: - // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views + typedef typename CAMERA::Measurement Z; + + /** + * 2D measurement and noise model for each of the m views + * We keep a copy of measurements for I/O and computing the error. + * The order is kept the same as the keys that we use to create the factor. + */ + std::vector measured_; + std::vector noise_; ///< noise model used - ///< (important that the order is the same as the keys that we use to create the factor) - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits::dimension; ///< Measurement dimension + static const int ZDim = traits::dimension; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F @@ -63,27 +74,22 @@ protected: typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; - + typedef SmartFactorBase This; public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef CAMERA Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor * @param body_P_sensor is the transform from sensor to body frame (default identity) */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : + SmartFactorBase(boost::optional body_P_sensor = boost::none) : body_P_sensor_(body_P_sensor) { } @@ -92,7 +98,7 @@ public: } /** - * add a new measurement and pose key + * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark * @param noise_i is the measurement noise @@ -105,9 +111,8 @@ public: } /** - * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys and noises */ - // **************************************************************************************************** void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { @@ -118,9 +123,8 @@ public: } /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and uses the same noise model for all of them */ - // **************************************************************************************************** void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { @@ -134,7 +138,6 @@ public: * Adds an entire SfM_track (collection of cameras observing a single point). * The noise is assumed to be the same for all measurements */ - // **************************************************************************************************** template void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { @@ -144,12 +147,17 @@ public: } } + /// get the dimension (number of rows!) of the factor + virtual size_t dim() const { + return ZDim * this->measured_.size(); + } + /** return the measurements */ const std::vector& measured() const { return measured_; } - /** return the noise model */ + /** return the noise models */ const std::vector& noise() const { return noise_; } @@ -187,30 +195,38 @@ public: && body_P_sensor_->equals(*e->body_P_sensor_))); } - // **************************************************************************************************** -// /// Calculate vector of re-projection errors, before applying noise model + /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - Vector b = zero(ZDim * cameras.size()); + // Project into CameraSet + std::vector predicted; + try { + predicted = cameras.project(point); + } catch (CheiralityException&) { + std::cout << "reprojectionError: Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception + } - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z e(camera.project(point) - zi); - b[ZDim * i] = e.x(); - b[ZDim * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; + // Calculate vector of errors + size_t nrCameras = cameras.size(); + Vector b(ZDim * nrCameras); + for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) { + Z e = predicted[i] - measured_.at(i); + b.segment(row) = e.vector(); } return b; } - // **************************************************************************************************** + /// Calculate vector of re-projection errors, noise model applied + Vector whitenedError(const Cameras& cameras, const Point3& point) const { + Vector b = reprojectionError(cameras, point); + size_t nrCameras = cameras.size(); + for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) + b.segment(row) = noise_.at(i)->whiten(b.segment(row)); + return b; + } + /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. @@ -220,178 +236,233 @@ public: */ double totalReprojectionError(const Cameras& cameras, const Point3& point) const { - + Vector b = reprojectionError(cameras, point); double overallError = 0; - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z reprojectionError(camera.project(point) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; - } - return overallError; + size_t nrCameras = cameras.size(); + for (size_t i = 0; i < nrCameras; i++) + overallError += noise_.at(i)->distance(b.segment(i * ZDim)); + return 0.5 * overallError; } - // **************************************************************************************************** - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { + /** + * Compute whitenedError, returning only the derivative E, i.e., + * the stacked ZDim*3 derivatives of project with respect to the point. + * Assumes non-degenerate ! TODO explain this + */ + Vector whitenedError(const Cameras& cameras, const Point3& point, + Matrix& E) const { - int numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(ZDim, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei, boost::none); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block(ZDim * i, 0) = Ei; + // Project into CameraSet, calculating derivatives + std::vector predicted; + try { + using boost::none; + predicted = cameras.project(point, none, E, none); + } catch (CheiralityException&) { + std::cout << "whitenedError(E): Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception } - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); + // if needed, whiten + size_t m = keys_.size(); + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + + // Calculate error + const Z& zi = measured_.at(i); + Vector bi = (zi - predicted[i]).vector(); + + // if needed, whiten + SharedNoiseModel model = noise_.at(i); + if (model) { + // TODO: re-factor noiseModel to take any block/fixed vector/matrix + Vector dummy; + Matrix Ei = E.block(row, 0); + model->WhitenSystem(Ei, dummy); + E.block(row, 0) = Ei; + } + b.segment(row) = bi; + } + return b; } - // **************************************************************************************************** - /// Compute F, E only (called below in both vanilla and SVD versions) - /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { + /** + * Compute F, E, and optionally H, where F and E are the stacked derivatives + * with respect to the cameras, point, and calibration, respectively. + * The value of cameras/point are passed as parameters. + * Returns error vector b + * TODO: the treatment of body_P_sensor_ is weird: the transformation + * is applied in the caller, but the derivatives are computed here. + */ + Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F, + Matrix& E, boost::optional G = boost::none) const { - size_t numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - b = zero(ZDim * numKeys); - double f = 0; + // Project into CameraSet, calculating derivatives + std::vector predicted; + try { + predicted = cameras.project(point, F, E, G); + } catch (CheiralityException&) { + std::cout << "whitenedError(E,F): Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception + } - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); - for (size_t i = 0; i < this->measured_.size(); i++) { + // Calculate error and whiten derivatives if needed + size_t m = keys_.size(); + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Vector bi; - try { - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - if(body_P_sensor_){ - Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); - Matrix J(6, 6); - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - Fi = Fi * J; + // Calculate error + const Z& zi = measured_.at(i); + Vector bi = (zi - predicted[i]).vector(); + + // if we have a sensor offset, correct camera derivatives + if (body_P_sensor_) { + // TODO: no simpler way ?? + const Pose3& pose_i = cameras[i].pose(); + Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); + Matrix66 J; + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + F.block(row, 0) *= J; + } + + // if needed, whiten + SharedNoiseModel model = noise_.at(i); + if (model) { + // TODO, refactor noiseModel so we can take blocks + Matrix Fi = F.block(row, 0); + Matrix Ei = E.block(row, 0); + if (!G) + model->WhitenSystem(Fi, Ei, bi); + else { + Matrix Gi = G->block(row, 0); + model->WhitenSystem(Fi, Ei, Gi, bi); + G->block(row, 0) = Gi; } - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + F.block(row, 0) = Fi; + E.block(row, 0) = Ei; } - this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); - - f += bi.squaredNorm(); - if (D == 6) { // optimize only camera pose - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); - } else { - Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); - } - E.block(ZDim * i, 0) = Ei; - subInsert(b, bi, ZDim * i); + b.segment(row) = bi; } - return f; + return b; } - // **************************************************************************************************** - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, Matrix& E, - Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, - double lambda = 0.0, bool diagonalDamping = false) const { + /// Computes Point Covariance P from E + static Matrix3 PointCov(Matrix& E) { + return (E.transpose() * E).inverse(); + } - double f = computeJacobians(Fblocks, E, b, cameras, point); + /// Computes Point Covariance P, with lambda parameter + static Matrix3 PointCov(Matrix& E, double lambda, + bool diagonalDamping = false) { - // Point covariance inv(E'*E) Matrix3 EtE = E.transpose() * E; if (diagonalDamping) { // diagonal of the hessian EtE(0, 0) += lambda * EtE(0, 0); EtE(1, 1) += lambda * EtE(1, 1); EtE(2, 2) += lambda * EtE(2, 2); - }else{ + } else { EtE(0, 0) += lambda; EtE(1, 1) += lambda; EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE).inverse(); - - return f; + return (EtE).inverse(); } - // **************************************************************************************************** - // TODO, there should not be a Matrix version, really - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const Point3& point, - const double lambda = 0.0) const { + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, + const Point3& point) const { + whitenedError(cameras, point, E); + P = PointCov(E); + } - size_t numKeys = this->keys_.size(); - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, - lambda); - F = zeros(This::ZDim * numKeys, D * numKeys); + /** + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + */ + double computeJacobians(std::vector& Fblocks, Matrix& E, + Vector& b, const Cameras& cameras, const Point3& point) const { - for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras + // Project into Camera set and calculate derivatives + // TODO: if D==6 we optimize only camera pose. That is fairly hacky! + Matrix F, G; + using boost::none; + boost::optional optionalG(G); + b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG); + + // Now calculate f and divide up the F derivatives into Fblocks + double f = 0.0; + size_t m = keys_.size(); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + + // Accumulate normalized error + f += b.segment(row).squaredNorm(); + + // Get piece of F and possibly G + Matrix2D Fi; + if (D == 6) + Fi << F.block(row, 0); + else + Fi << F.block(row, 0), G.block(row, 0); + + // Push it onto Fblocks + Fblocks.push_back(KeyMatrix2D(keys_[i], Fi)); } return f; } - // **************************************************************************************************** + /// Create BIG block-diagonal matrix F from Fblocks + static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { + size_t m = Fblocks.size(); + F.resize(ZDim * m, D * m); + F.setZero(); + for (size_t i = 0; i < m; ++i) + F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; + } + + /** + * Compute F, E, and b, where F and E are the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + */ + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras, const Point3& point) const { + std::vector Fblocks; + double f = computeJacobians(Fblocks, E, b, cameras, point); + FillDiagonalF(Fblocks, F); + return f; + } + /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point, double lambda = - 0.0, bool diagonalDamping = false) const { + Vector& b, const Cameras& cameras, const Point3& point) const { Matrix E; - Matrix3 PointCov; // useless - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + double f = computeJacobians(Fblocks, E, b, cameras, point); // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); - size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns + size_t m = this->keys_.size(); + // Enull = zeros(ZDim * m, ZDim * m - 3); + Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns return f; } - // **************************************************************************************************** /// Matrix version of SVD // TODO, there should not be a Matrix version, really double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point) const { - - int numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(ZDim * numKeys, D * numKeys); - F.setZero(); - - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - + FillDiagonalF(Fblocks, F); return f; } - // **************************************************************************************************** - /// linearize returns a Hessianfactor that is an approximation of error(p) + /** + * Linearize returns a Hessianfactor that is an approximation of error(p) + */ boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -400,10 +471,9 @@ public: std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS @@ -411,14 +481,13 @@ public: std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Vector > gs(numKeys); - sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); - // schurComplement(Fblocks, E, PointCov, b, Gs, gs); + sparseSchurComplement(Fblocks, E, P, b, Gs, gs); + // schurComplement(Fblocks, E, P, b, Gs, gs); //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end()); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); + return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); #else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term @@ -426,17 +495,19 @@ public: dims.back() = 1; SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) - sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... augmentedHessian(numKeys, numKeys)(0, 0) = f; return boost::make_shared >(this->keys_, augmentedHessian); #endif } - // **************************************************************************************************** - // slow version - works on full (sparse) matrices + /** + * Do Schur complement, given Jacobian as F,E,P. + * Slow version - works on full matrices + */ void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix& PointCov, const Vector& b, + const Matrix3& P, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { // Schur complement trick // Gs = F' * F - F' * E * inv(E'*E) * E' * F @@ -446,16 +517,14 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(ZDim * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras + Matrix F; + FillDiagonalF(Fblocks, F); Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); // Populate Gs and gs int GsCount2 = 0; @@ -471,9 +540,12 @@ public: } } - // **************************************************************************************************** + /** + * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix + * Fast version - works on with sparsity + */ void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick // Gs = F' * F - F' * E * P * E' * F @@ -490,7 +562,8 @@ public: // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b + augmentedHessian(i1, numKeys) = Fi1.transpose() + * b.segment(ZDim * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) @@ -508,9 +581,12 @@ public: } // end of for over cameras } - // **************************************************************************************************** + /** + * Do Schur complement, given Jacobian as F,E,P, return Gs/gs + * Fast version - works on with sparsity + */ void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { // Schur complement trick // Gs = F' * F - F' * E * P * E' * F @@ -535,7 +611,7 @@ public: { // for i1 = i2 // D = (Dx2) * (2) gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b - -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) Gs.at(GsIndex) = Fi1.transpose() @@ -554,27 +630,12 @@ public: } // end of for over cameras } - // **************************************************************************************************** - void updateAugmentedHessian(const Cameras& cameras, const Point3& point, - const double lambda, bool diagonalDamping, - SymmetricBlockMatrix& augmentedHessian, - const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - } - - // **************************************************************************************************** + /** + * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, + * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. + */ void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, const FastVector allKeys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick @@ -584,9 +645,9 @@ public: MatrixDD matrixBlock; typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - FastMap KeySlotMap; - for (size_t slot=0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); // a single point is observed in numKeys cameras size_t numKeys = this->keys_.size(); // cameras observing current point @@ -607,7 +668,8 @@ public: // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, + aug_numKeys).knownOffDiagonal() + Fi1.transpose() * b.segment(ZDim * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) @@ -615,8 +677,11 @@ public: // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = matrixBlock + - ( Fi1.transpose() * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1) ); + augmentedHessian(aug_i1, aug_i1) = + matrixBlock + + (Fi1.transpose() + * (Fi1 + - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1)); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -629,48 +694,76 @@ public: // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); + augmentedHessian(aug_i1, aug_i2) = + augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() + - Fi1.transpose() + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; } - // **************************************************************************************************** + /** + * Add the contribution of the smart factor to a pre-allocated Hessian, + * using sparse linear algebra. More efficient than the creation of the + * Hessian without preallocation of the SymmetricBlockMatrix + */ + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Vector b; + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + /** + * Return Jacobians as RegularImplicitSchurFactor with raw access + */ boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { typename boost::shared_ptr > f( new RegularImplicitSchurFactor()); - computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), - cameras, point, lambda, diagonalDamping); + computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point); + f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping); f->initKeys(); return f; } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactorQ + */ boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(Fblocks, E, P, b); } - // **************************************************************************************************** + /** + * Return Jacobians as JacobianFactor + * TODO lambda is currently ignored + */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t numKeys = this->keys_.size(); std::vector Fblocks; Vector b; - Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); + Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point); + return boost::make_shared >(Fblocks, Enull, b); } private: @@ -683,9 +776,10 @@ private: ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; +} +; -template -const int SmartFactorBase::ZDim; +template +const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2bfcbfaa0..7fb43152a 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -58,11 +58,11 @@ enum LinearizationMode { }; /** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, D> { +template +class SmartProjectionFactor: public SmartFactorBase, + D> { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase, D> Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -102,9 +102,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; - - static const int ZDim = 2; ///< Measurement dimension + typedef SmartProjectionFactor This; public: @@ -113,7 +111,7 @@ public: /// shorthand for a pinhole camera typedef PinholeCamera Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -126,16 +124,16 @@ public: */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : + double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = + SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -252,11 +250,11 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; - size_t i=0; + size_t i = 0; BOOST_FOREACH(const Camera& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away - if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { degenerate_ = true; break; } @@ -270,8 +268,8 @@ public: i += 1; } // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) + if (dynamicOutlierRejectionThreshold_ > 0 + && totalReprojError / m > dynamicOutlierRejectionThreshold_) degenerate_ = true; } catch (TriangulationUnderconstrainedException&) { @@ -300,7 +298,8 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) { - std::cout << "createRegularImplicitSchurFactor: degenerate configuration" + std::cout + << "createRegularImplicitSchurFactor: degenerate configuration" << std::endl; } return false; @@ -321,9 +320,9 @@ public: bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector < Key > js; - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -338,7 +337,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -373,9 +372,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -383,20 +381,20 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + Matrix3 P = Base::PointCov(E, lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + gs.at(i1) = gs_vector.segment(i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * D); GsCount2++; } } @@ -420,16 +418,16 @@ public: } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -437,16 +435,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate @@ -479,27 +477,27 @@ public: return true; } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const { + return Base::computeEP(E, P, cameras, point_); + } + /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + bool computeEP(Matrix& E, Matrix& P, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + computeEP(E, P, myCameras); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras); return nonDegenerate; } @@ -538,7 +536,7 @@ public: this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); f += bi.squaredNorm(); Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); - E.block < 2, 2 > (2 * i, 0) = Ei; + E.block<2, 2>(2 * i, 0) = Ei; subInsert(b, bi, 2 * i); } return f; @@ -548,19 +546,6 @@ public: } // end else } - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, - const double lambda = 0.0) const { - - double f = computeJacobians(Fblocks, E, b, cameras); - - // Point covariance inv(E'*E) - PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); - - return f; - } - /// takes values bool computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { @@ -585,9 +570,9 @@ public: } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobians(F, E, b, cameras, point_); } /// Calculate vector of re-projection errors, before applying noise model @@ -709,7 +694,4 @@ private: } }; -template -const int SmartProjectionFactor::ZDim; - } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0801d141f..70476ba3e 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -48,10 +48,10 @@ protected: public: /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -67,7 +67,7 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -141,11 +141,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -216,7 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : public Testable > {}; +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; } // \ namespace gtsam diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 8dfb753f4..e2b8ac3cf 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -15,11 +15,10 @@ * @date March 4, 2014 */ -#include -#include - -//#include #include +#include + +#include #include #include @@ -29,8 +28,6 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -const double tol = 1e-5; - /* ************************************************************************* */ TEST(RegularHessianFactor, ConstructorNWay) { @@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay) expected.insert(1, Y.segment<2>(2)); expected.insert(3, Y.segment<2>(4)); + // VectorValues version + double alpha = 1.0; + VectorValues actualVV; + actualVV.insert(0, zero(2)); + actualVV.insert(1, zero(2)); + actualVV.insert(3, zero(2)); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + // RAW ACCESS Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; Vector fast_y = gtsam::zero(8); double xvalues[8] = {1,2,3,4,0,0,5,6}; - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(expected_y, fast_y)); // now, do it with non-zero y - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(2*expected_y, fast_y)); // check some expressions diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 29eaf2ac1..8571a345d 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -1,16 +1,24 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImplicitSchurFactor.cpp + * @file testRegularImplicitSchurFactor.cpp * @brief unit test implicit jacobian factors * @author Frank Dellaert * @date Oct 20, 2013 */ -//#include -#include -//#include #include -//#include "gtsam_unstable/slam/JacobianFactorQR.h" -#include "gtsam/slam/JacobianFactorQR.h" +#include +#include #include #include diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index b56b65d7b..5803516a1 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularJacobianFactor.cpp * @brief unit test regular jacobian factors @@ -5,13 +16,13 @@ * @date Nov 12, 2014 */ -#include -#include - #include #include #include #include +#include + +#include #include #include diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp new file mode 100644 index 000000000..b5ef18842 --- /dev/null +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartFactorBase.cpp + * @brief Unit tests for testSmartFactorBase Class + * @author Frank Dellaert + * @date Feb 2015 + */ + +#include "../SmartFactorBase.h" +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeFactor: public SmartFactorBase, 9> { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Pinhole) { + PinholeFactor f; + f.add(Point2(), 1, SharedNoiseModel()); + f.add(Point2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 2, f.dim()); +} + +/* ************************************************************************* */ +#include +class StereoFactor: public SmartFactorBase { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Stereo) { + StereoFactor f; + f.add(StereoPoint2(), 1, SharedNoiseModel()); + f.add(StereoPoint2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 38722d88b..001e6b997 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TestSmartProjectionPoseFactor.cpp + * @file testSmartProjectionPoseFactor.cpp * @brief Unit tests for ProjectionFactor Class * @author Chris Beall * @author Luca Carlone @@ -20,11 +20,12 @@ #include "../SmartProjectionPoseFactor.h" -#include #include #include -#include +#include +#include #include +#include #include using namespace std; @@ -35,37 +36,38 @@ static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; static double linThreshold = -1.0; static bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; -void projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ +void projectToMultipleCameras(SimpleCamera cam1, SimpleCamera cam2, + SimpleCamera cam3, Point3 landmark, vector& measurements_cam) { Point2 cam1_uv1 = cam1.project(landmark); Point2 cam2_uv1 = cam2.project(landmark); @@ -101,7 +103,8 @@ TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -117,21 +120,22 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ +TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, *K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera level_camera_right(level_pose_right, *K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); - // 1. Project two landmarks into two cameras and triangulate + // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); @@ -139,93 +143,114 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartFactor factor1; - factor1.add(level_uv, x1, model, K); - factor1.add(level_uv_right, x2, model, K); + SmartFactor factor; + factor.add(level_uv, x1, model, K); + factor.add(level_uv_right, x2, model, K); - double actualError = factor1.error(values); + double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); - double actualError2 = factor1.totalReprojectionError(cameras); + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // test vector of errors - //Vector actual = factor1.unwhitenedError(values); + //Vector actual = factor.unwhitenedError(values); //EXPECT(assert_equal(zero(4),actual,1e-8)); + + // Check derivatives + + // Calculate expected derivative for point (easiest to check) + boost::function f = // + boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + boost::optional point = factor.point(); + CHECK(point); + Matrix expectedE = numericalDerivative11(f, *point); + + // Calculate using computeEP + Matrix actualE, PointCov; + factor.computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = factor.whitenedError(cameras, *point, F, actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, noisy ){ +TEST( SmartProjectionPoseFactor, noisy ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, *K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera level_camera_right(level_pose_right, *K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); + Point2 pixelError(0.2, 0.2); Point2 level_uv = level_camera.project(landmark) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartFactor::shared_ptr factor(new SmartFactor()); + factor->add(level_uv, x1, model, K); + factor->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -240,7 +265,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -263,21 +288,25 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); @@ -287,25 +316,29 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3),1e-8)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ +TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses - Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); - Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1, 0, 0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam1(cameraPose1, *K); // with camera poses SimpleCamera cam2(cameraPose2, *K); SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -325,7 +358,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -335,13 +368,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ bool manageDegeneracy = false; bool enableEPI = false; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -363,12 +402,13 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, bodyPose3*noise_pose); + values.insert(x3, bodyPose3 * noise_pose); LevenbergMarquardtParams params; Values result; @@ -376,8 +416,112 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ result = optimizer.optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(bodyPose3,result.at(x3))); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(bodyPose3, result.at(x3))); + + // Check derivatives + + // Calculate expected derivative for point (easiest to check) + SmartFactor::Cameras cameras = smartFactor1->cameras(values); + boost::function f = // + boost::bind(&SmartFactor::whitenedError, *smartFactor1, cameras, _1); + boost::optional point = smartFactor1->point(); + CHECK(point); + Matrix expectedE = numericalDerivative11(f, *point); + + // Calculate using computeEP + Matrix actualE, PointCov; + smartFactor1->computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = smartFactor1->whitenedError(cameras, *point, F, + actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + + // TODO the derivatives agree with f, but returned errors are -f :-( + // TODO We should merge the two whitenedErrors functions and make derivatives optional + EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Factors ){ + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); + SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); + + // one landmarks 1m in front of camera + Point3 landmark1(0,0,10); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + cout << cam1.project(landmark1) << endl; + cout << cam2.project(landmark1) << endl; + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1,*p)); + + { + // RegularHessianFactor<6> + Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; + Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; + Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; + + Vector6 g1; g1.setZero(); + Vector6 g2; g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(),actual->information(),1e-8)); + CHECK(assert_equal(expected,*actual,1e-8)); + } + + { + // RegularImplicitSchurFactor<6> + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + const vector > Fblocks = list_of > // + (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected,*actual)); + } + } /* *************************************************************************/ @@ -460,24 +604,24 @@ TEST( SmartProjectionPoseFactor, Factors ){ /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -510,48 +654,54 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianSVD ){ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -566,13 +716,16 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -584,38 +737,39 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, landmarkDistance ){ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -630,13 +784,19 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -648,40 +808,41 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -690,29 +851,34 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark3(3, 0, 3.0); Point3 landmark4(5, -0.5, 1); - vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -725,7 +891,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); @@ -736,25 +903,25 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ){ +TEST( SmartProjectionPoseFactor, jacobianQ ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -769,13 +936,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -787,39 +957,40 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -831,60 +1002,74 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ NonlinearFactorGraph graph; // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); + values.insert(x3, pose3 * noise_pose); values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); - if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); + if (isDebugTest) + values.at(x3).print("Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); + if (isDebugTest) + result.at(x3).print("Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, CheckHessian){ +TEST( SmartProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -915,57 +1100,65 @@ TEST( SmartProjectionPoseFactor, CheckHessian){ graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -979,68 +1172,81 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); double rankTol = 50; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K2); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); - values.insert(x2, pose2*noise_pose); + values.insert(x2, pose2 * noise_pose); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -1057,68 +1263,82 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ double rankTol = 10; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Hessian ){ +TEST( SmartProjectionPoseFactor, Hessian ) { // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // three landmarks ~5 meters infront of camera @@ -1132,15 +1352,18 @@ TEST( SmartProjectionPoseFactor, Hessian ){ measurements_cam1.push_back(cam2_uv1); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); + smartFactor1->add(measurements_cam1, views, model, K2); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor1->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); // compute triangulation from linearization point // compute reprojection errors (sum squared) @@ -1148,26 +1371,25 @@ TEST( SmartProjectionPoseFactor, Hessian ){ // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); Point3 landmark1(5, 0.5, 1.2); @@ -1184,54 +1406,62 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K2); Point3 landmark1(5, 0.5, 1.2); @@ -1243,62 +1473,72 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); - boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor1.add(measurement1, poseKey1, model, Kbundler); + SmartFactorBundler factor(rankTol, linThreshold); + boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + factor.add(measurement1, poseKey1, model, Kbundler); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ){ +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); PinholeCamera cam1(pose1, *Kbundler); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); PinholeCamera cam2(pose2, *Kbundler); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1330,7 +1570,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ measurements_cam3.push_back(cam2_uv3); measurements_cam3.push_back(cam3_uv3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -1353,50 +1593,56 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); - if(isDebugTest) tictoc_print_(); - } + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); + if (isDebugTest) + tictoc_print_(); +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); PinholeCamera cam1(pose1, *Kbundler); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); PinholeCamera cam2(pose2, *Kbundler); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1430,56 +1676,72 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ double rankTol = 10; - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor1( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, Kbundler); - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor2( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, Kbundler); - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor3( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, Kbundler); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c8a4e7123..dc921a7da 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 770c5f18c..cfed9ae0c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartStereoProjectionPoseFactor SmartFactor; + typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5d2ba3323..11513d19f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -62,10 +62,9 @@ enum LinearizationMode { /** * SmartStereoProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? */ -template -class SmartStereoProjectionFactor: public SmartFactorBase { +template +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -95,7 +94,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -105,7 +104,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; enum {ZDim = 3}; ///< Dimension trait of measurement type @@ -118,7 +117,7 @@ public: typedef StereoCamera Camera; /// Vector of cameras - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -131,7 +130,7 @@ public: */ SmartStereoProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : @@ -370,7 +369,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -408,9 +407,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -418,9 +416,9 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + Matrix3 P = Base::PointCov(E,lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; if (isDebug) std::cout << "H:\n" << H << std::endl; @@ -515,6 +513,11 @@ public: return true; } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { + Base::computeEP(E, PointCov, cameras, point_); + } + /// Takes values bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { Cameras myCameras; @@ -524,18 +527,13 @@ public: return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras, 0.0); return nonDegenerate; } @@ -621,9 +619,9 @@ public: // } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobians(F, E, b, cameras, point_); } /// Calculate vector of re-projection errors, before applying noise model @@ -746,9 +744,9 @@ private: }; /// traits -template -struct traits > : - public Testable > { +template +struct traits > : + public Testable > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2e3bc866b..3e0c6476f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -50,10 +50,10 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -69,7 +69,7 @@ public: */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -143,11 +143,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -216,9 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionPoseFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4691da384..4cc8e66ff 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,42 +32,44 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = true; +static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; static double b = 1; -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); -static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); +static Cal3_S2Stereo::shared_ptr K2( + new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; static double linThreshold = -1.0; // static bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(3)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); -typedef SmartStereoProjectionPoseFactor SmartFactor; -typedef SmartStereoProjectionPoseFactor SmartFactorBundler; +typedef SmartStereoProjectionPoseFactor SmartFactor; +typedef SmartStereoProjectionPoseFactor SmartFactorBundler; -vector stereo_projectToMultipleCameras( - const StereoCamera& cam1, const StereoCamera& cam2, - const StereoCamera& cam3, Point3 landmark){ +vector stereo_projectToMultipleCameras(const StereoCamera& cam1, + const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { vector measurements_cam; @@ -83,7 +85,7 @@ vector stereo_projectToMultipleCameras( /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr,"Test 1 Complete"); + fprintf(stderr, "Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } @@ -108,7 +110,8 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) { TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -124,15 +127,16 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ +TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera @@ -164,75 +168,78 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, noisy ){ +TEST( SmartStereoProjectionPoseFactor, noisy ) { // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - StereoPoint2 pixelError(0.2,0.2,0); + StereoPoint2 pixelError(0.2, 0.2, 0); StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; StereoPoint2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); SmartFactor::shared_ptr factor1(new SmartFactor()); factor1->add(level_uv, x1, model, K); factor1->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor1->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { + cout + << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" + << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K2); // three landmarks ~5 meters infront of camera @@ -241,11 +248,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -268,21 +278,25 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartStereoProjectionPoseFactor); + gttic_ (SmartStereoProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); @@ -292,28 +306,29 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3))); + if (isDebugTest) + tictoc_print_(); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -322,17 +337,23 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -344,38 +365,39 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -384,18 +406,26 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -407,40 +437,41 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -450,28 +481,35 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark4(5, -0.5, 1); // 1. Project four landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark4); + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier - measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -484,7 +522,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); @@ -495,19 +534,19 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -546,8 +585,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -564,13 +603,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ // // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -606,7 +645,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -627,23 +666,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ //} // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +TEST( SmartStereoProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // create second camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // create third camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -651,12 +690,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); - // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - + // Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + // Create smartfactors double rankTol = 10; SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); @@ -668,38 +710,47 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); smartFactor3->add(measurements_cam3, views, model, K); + // Create graph to optimize NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + // TODO: next line throws Cheirality exception on Mac + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); @@ -709,13 +760,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -745,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -754,7 +805,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); @@ -775,7 +826,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // // result.print("results of 3 camera, 3 landmark optimization \n"); // if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} @@ -784,13 +835,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -826,7 +877,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -836,8 +887,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -858,7 +909,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // // result.print("results of 3 camera, 3 landmark optimization \n"); // if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} @@ -867,12 +918,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -892,7 +943,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // SmartFactor::shared_ptr smartFactor1(new SmartFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -908,29 +959,30 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); smartFactorInstance->add(measurements_cam1, views, model, K); @@ -940,46 +992,54 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -990,49 +1050,60 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 81bcac344..e45f234aa 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -1,7 +1,14 @@ +/** + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Test simple CG optimizer + * @author Yong-Dian Jian + * @date June 11, 2012 + */ + /** * @file testGradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @brief Small test of NonlinearConjugateGradientOptimizer + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -18,89 +25,74 @@ #include #include - using namespace std; using namespace gtsam; - +// Generate a small PoseSLAM problem boost::tuple generateProblem() { // 1. Create graph container and add factors to it - NonlinearFactorGraph graph ; + NonlinearFactorGraph graph; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); + Pose2 x1(0.5, 0.0, 0.2); + initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1, -0.2); + initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); + initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI); + initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1, -M_PI_2); + initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } /* ************************************************************************* */ -TEST(optimize, GradientDescentOptimizer) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { NonlinearFactorGraph graph; Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - // cout << "initial error = " << graph.error(initialEstimate) << endl ; +// cout << "initial error = " << graph.error(initialEstimate) << endl; - // Single Step Optimization using Levenberg-Marquardt NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; - - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); -// cout << "gd1 solver final error = " << graph.error(result) << endl; - - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); - - CHECK(1); -} - -/* ************************************************************************* */ -TEST(optimize, ConjugateGradientOptimizer) { - - NonlinearFactorGraph graph; - Values initialEstimate; - - boost::tie(graph, initialEstimate) = generateProblem(); -// cout << "initial error = " << graph.error(initialEstimate) << endl ; - - // Single Step Optimization using Levenberg-Marquardt - NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ + param.maxIterations = 500; /* requires a larger number of iterations to converge */ param.verbosity = NonlinearOptimizerParams::SILENT; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); // cout << "cg final error = " << graph.error(result) << endl; - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index c90b09db1..66b022c82 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -17,21 +17,11 @@ */ #include -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include #include -#include #include #include diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 89c3d5cf8..f00916475 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -18,11 +18,12 @@ #include -#include -#include #include -#include +#include +#include +#include #include +#include using namespace std; using namespace gtsam; From a356d5d2b7cd298aec73d195666090c60206c058 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 06:20:53 +0100 Subject: [PATCH 43/58] include Pose2 --- examples/Pose2SLAMExample_graph.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 0c64634c5..c3d901507 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -19,6 +19,7 @@ // For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include +#include #include #include From b0974d6f41453c36e0d5ebba3d2f103f199a6831 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 06:21:09 +0100 Subject: [PATCH 44/58] Remove duplicate test --- .../tests/testSmartProjectionPoseFactor.cpp | 79 ------------------- 1 file changed, 79 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 001e6b997..72fd031e6 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -524,85 +524,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Factors ){ - - // Default cameras for simple derivatives - Rot3 R; - static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); - SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); - - // one landmarks 1m in front of camera - Point3 landmark1(0,0,10); - - vector measurements_cam1; - - // Project 2 landmarks into 2 cameras - cout << cam1.project(landmark1) << endl; - cout << cam2.project(landmark1) << endl; - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); - - // Create smart factors - std::vector views; - views.push_back(x1); - views.push_back(x2); - - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - CHECK(p); - EXPECT(assert_equal(landmark1,*p)); - - { - // RegularHessianFactor<6> - Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; - Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; - Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; - - Vector6 g1; g1.setZero(); - Vector6 g2; g2.setZero(); - - double f = 0; - - RegularHessianFactor<6> expected(x1, x2, 50 * G11, 50 * G12, g1, 50 * G22, g2, f); - - boost::shared_ptr > actual = - smartFactor1->createHessianFactor(cameras, 0.0); - CHECK(assert_equal(expected.information(),actual->information(),1e-8)); - CHECK(assert_equal(expected,*actual,1e-8)); - } - - { - // RegularImplicitSchurFactor<6> - Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; - Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=10; E(1,1)=10; E(2,0)=10; E(2,2)=1;E(3,1)=10; - const vector > Fblocks = list_of > // - (make_pair(x1, F1))(make_pair(x2, F2)); - Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; b.setZero(); - - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); - - boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); - CHECK(actual); - CHECK(assert_equal(expected,*actual)); - } - -} - - /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; From c1a4409e8938b0deacac62e9c42b64b0ca7db06e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 06:32:55 +0100 Subject: [PATCH 45/58] traits --- gtsam/slam/JacobianFactorQ.h | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 49f5513b6..d33f3325b 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -35,15 +35,16 @@ public: } /// Empty constructor with keys - JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorQ(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + JacobianSchurFactor() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } @@ -58,14 +59,21 @@ public: // Calculate pre-computed Jacobian matrices // TODO: can we do better ? typedef std::pair KeyMatrix; - std::vector < KeyMatrix > QF; + std::vector QF; QF.reserve(m); // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); + QF.push_back( + KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); } }; +// end class JacobianFactorQ + +// traits +template struct traits > : public Testable< + JacobianFactorQ > { +}; } From 8b914189cb34db3bee9f0e0c35a6cc785d9ab5a9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 06:33:05 +0100 Subject: [PATCH 46/58] Check JacobianFactorQ --- .../tests/testSmartProjectionPoseFactor.cpp | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 72fd031e6..2ca06acbf 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -485,8 +485,17 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(p); EXPECT(assert_equal(landmark1,*p)); + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + const vector > Fblocks = list_of > // + (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + { // RegularHessianFactor<6> + // TODO, calculate G from F Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; @@ -506,14 +515,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ { // RegularImplicitSchurFactor<6> - Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; - Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; - const vector > Fblocks = list_of > // - (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); - Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; b.setZero(); - RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); boost::shared_ptr > actual = @@ -522,6 +523,16 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(assert_equal(expected,*actual)); } + { + // createJacobianQFactor<6,2> + JacobianFactorQ<6, 2> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected, *actual)); + } + } /* *************************************************************************/ From d09c7aa1054842fb0f32c5a29a79ea5fe0e6fe2c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 06:48:34 +0100 Subject: [PATCH 47/58] Test createJacobianSVDFactor --- .../tests/testSmartProjectionPoseFactor.cpp | 54 +++++++++++-------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 2ca06acbf..aebea3959 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -460,8 +460,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ vector measurements_cam1; // Project 2 landmarks into 2 cameras - cout << cam1.project(landmark1) << endl; - cout << cam2.project(landmark1) << endl; measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1)); @@ -485,17 +483,8 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(p); EXPECT(assert_equal(landmark1,*p)); - Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; - Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; - Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; - const vector > Fblocks = list_of > // - (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); - Matrix3 P = (E.transpose() * E).inverse(); - Vector4 b; b.setZero(); - { - // RegularHessianFactor<6> - // TODO, calculate G from F + // createHessianFactor Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; @@ -508,27 +497,48 @@ TEST( SmartProjectionPoseFactor, Factors ){ RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); boost::shared_ptr > actual = - smartFactor1->createHessianFactor(cameras, 0.0); - CHECK(assert_equal(expected.information(),actual->information(),1e-8)); - CHECK(assert_equal(expected,*actual,1e-8)); + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(), actual->information(), 1e-8)); + CHECK(assert_equal(expected, *actual, 1e-8)); } { - // RegularImplicitSchurFactor<6> + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + const vector > Fblocks = list_of > // + (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + + // createRegularImplicitSchurFactor RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); - CHECK(assert_equal(expected,*actual)); + CHECK(assert_equal(expected, *actual)); + + // createJacobianQFactor + JacobianFactorQ < 6, 2 > expectedQ(Fblocks, E, P, b); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expectedQ, *actualQ)); } { - // createJacobianQFactor<6,2> - JacobianFactorQ<6, 2> expected(Fblocks, E, P, b); + // createJacobianSVDFactor + Matrix16 A1, A2; + A1 << -1000, 0, 0, 0, 100, 0; + A2 << 1000, 0, 100, 0, -100, 0; + Vector1 b; b.setZero(); + double s = sin(M_PI_4); + JacobianFactor expected(x1, s*A1, x2, s*A2, b); - boost::shared_ptr > actual = - smartFactor1->createJacobianQFactor(cameras, 0.0); + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); CHECK(actual); CHECK(assert_equal(expected, *actual)); } From 772db8850a3fdaa994a0665cfc47b0b112e05638 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 07:05:11 +0100 Subject: [PATCH 48/58] SVD and HessianFactor checked using same information --- .../slam/tests/testSmartProjectionPoseFactor.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index aebea3959..474009cfb 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -483,18 +483,22 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(p); EXPECT(assert_equal(landmark1,*p)); + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -1000, 0, 0, 0, 100, 0; + A2 << 1000, 0, 100, 0, -100, 0; { // createHessianFactor - Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; - Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; - Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; + Matrix66 G11 = 0.5*A1.transpose()*A1; + Matrix66 G12 = 0.5*A1.transpose()*A2; + Matrix66 G22 = 0.5*A2.transpose()*A2; Vector6 g1; g1.setZero(); Vector6 g2; g2.setZero(); double f = 0; - RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); @@ -530,9 +534,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ { // createJacobianSVDFactor - Matrix16 A1, A2; - A1 << -1000, 0, 0, 0, 100, 0; - A2 << 1000, 0, 100, 0, -100, 0; Vector1 b; b.setZero(); double s = sin(M_PI_4); JacobianFactor expected(x1, s*A1, x2, s*A2, b); @@ -542,7 +543,6 @@ TEST( SmartProjectionPoseFactor, Factors ){ CHECK(actual); CHECK(assert_equal(expected, *actual)); } - } /* *************************************************************************/ From 78fd7de1b9db549af5838391f47b6bc5c0dc2575 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 07:06:41 +0100 Subject: [PATCH 49/58] Formatting and comments --- gtsam/slam/JacobianSchurFactor.h | 38 ++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index d31eea05b..e7f99a736 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -23,7 +23,9 @@ namespace gtsam { /** - * JacobianFactor for Schur complement that uses Q noise model + * JacobianFactor for Schur complement + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + * Provides raw memory access versions of linear operator. */ template class JacobianSchurFactor: public JacobianFactor { @@ -44,10 +46,11 @@ public: */ Vector operator*(const double* x) const { Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; + if (empty()) + return Ax; // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; @@ -57,17 +60,17 @@ public: * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { Vector E = alpha * (model_ ? model_->whiten(e) : e); // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; + if (empty()) + return; Vector Ax = zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } // multiply with alpha Ax *= alpha; // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos Date: Sun, 22 Feb 2015 07:21:42 +0100 Subject: [PATCH 50/58] Removed ZDim parameter from JacobianSchurFactor --- gtsam/slam/JacobianFactorQ.h | 18 ++++++++------- gtsam/slam/JacobianFactorQR.h | 20 ++++++++-------- gtsam/slam/JacobianFactorSVD.h | 39 +++++++++++++++++++------------- gtsam/slam/JacobianSchurFactor.h | 5 +--- 4 files changed, 45 insertions(+), 37 deletions(-) diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index d33f3325b..c18ad07d1 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -24,9 +24,11 @@ namespace gtsam { * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: @@ -37,7 +39,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -49,10 +51,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, const SharedDiagonal& model = + SharedDiagonal()) : + Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -62,7 +64,7 @@ public: std::vector QF; QF.reserve(m); // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) QF.push_back( KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 112278766..cf72fd0b3 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -18,24 +18,26 @@ class GaussianBayesNet; * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) { gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, b.segment(ZDim * i), model); i += 1; @@ -45,7 +47,7 @@ public: // eliminate the point boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; - std::vector < Key > variables; + std::vector variables; variables.push_back(pointKey); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); //fg->print("fg"); @@ -53,6 +55,6 @@ public: JacobianFactor::operator=(JacobianFactor(*fg)); } }; -// class +// end class JacobianFactorQR -}// gtsam +}// end namespace gtsam diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e0a5f4566..37043f448 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -12,43 +12,50 @@ namespace gtsam { * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public JacobianSchurFactor { + + typedef JacobianSchurFactor Base; + typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 + typedef std::pair KeyMatrixZD; + typedef std::pair KeyMatrix; public: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 - typedef std::pair KeyMatrix2D; - typedef std::pair KeyMatrix; - /// Default constructor - JacobianFactorSVD() {} + JacobianFactorSVD() { + } /// Empty constructor with keys - JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorSVD(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + JacobianFactorSVD(const std::vector& Fblocks, + const Matrix& Enull, const Vector& b, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { size_t numKeys = Enull.rows() / ZDim; - size_t j = 0, m2 = ZDim*numKeys-3; + size_t j = 0, m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + QF.push_back( + KeyMatrix(it.first, + (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index e7f99a736..6a218a9a7 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -27,14 +27,11 @@ namespace gtsam { * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD * Provides raw memory access versions of linear operator. */ -template +template class JacobianSchurFactor: public JacobianFactor { public: - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; From 3d8f9805770cdab263ad45bf3cb619911c9cc788 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 08:09:46 +0100 Subject: [PATCH 51/58] Formatting and redundancy --- gtsam/slam/RegularHessianFactor.h | 36 ++++++++++--------------------- 1 file changed, 11 insertions(+), 25 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index c272eac8e..5765f67fd 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file RegularHessianFactor.h - * @brief HessianFactor class with constant sized blcoks - * @author Richard Roberts - * @date Dec 8, 2010 + * @file RegularHessianFactor.h + * @brief HessianFactor class with constant sized blocks + * @author Sungtae An + * @date March 2014 */ #pragma once @@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor { private: - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix VectorD; + // Use Eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; public: @@ -61,7 +63,6 @@ public: } // Scratch space for multiplyHessianAdd - typedef Eigen::Matrix DVector; mutable std::vector y; /** y += alpha * A'*A*x */ @@ -78,9 +79,6 @@ public: BOOST_FOREACH(DVector & yi, y) yi.setZero(); - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after @@ -109,11 +107,6 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - // Create a vector of temporary y values, corresponding to rows i std::vector y; y.reserve(size()); @@ -149,10 +142,6 @@ public: /** Return the diagonal of the Hessian for this factor (raw memory version) */ virtual void hessianDiagonal(double* d) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; @@ -165,22 +154,19 @@ public: /// Add gradient at zero to d TODO: is it really the goal to add ?? virtual void gradientAtZero(double* d) const { - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - // Loop over all variables in the factor for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos, size()).knownOffDiagonal(); + DVector dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } /* ************************************************************************* */ -}; // end class RegularHessianFactor +}; +// end class RegularHessianFactor // traits template struct traits > : public Testable< From e15cfb3d339dfdd5f46ccc944ca9746e6eae855f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 08:10:34 +0100 Subject: [PATCH 52/58] Grabbed some methods from JacobianSchurFactor, added VectorValues versions --- gtsam/slam/RegularJacobianFactor.h | 165 ++++++++++++++++++----------- 1 file changed, 106 insertions(+), 59 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index dcf709854..38e1407f0 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -21,25 +21,34 @@ #include #include #include -#include namespace gtsam { +/** + * JacobianFactor with constant sized blocks + * Provides raw memory access versions of linear operator. + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + */ template class RegularJacobianFactor: public JacobianFactor { private: - /** Use eigen magic to access raw memory */ + // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; typedef Eigen::Map ConstDMap; public: + /// Default constructor + RegularJacobianFactor() {} + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ + * collection of keys and matrices making up the factor. + * TODO Verify terms are regular + */ template RegularJacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : @@ -49,7 +58,9 @@ public: /** Constructor with arbitrary number keys, and where the augmented matrix is given all together * instead of in block terms. Note that only the active view of the provided augmented matrix * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ + * factor. + * TODO Verify complies to regular + */ template RegularJacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = @@ -63,52 +74,11 @@ public: JacobianFactor::multiplyHessianAdd(alpha, x, y); } - /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x - * Note: this is not assuming a fixed dimension for the variables, - * but requires the vector accumulatedDims to tell the dimension of - * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - * then accumulatedDims is [0 3 9 11 13] - * NOTE: size of accumulatedDims is size of keys + 1!! */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { - - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - /// Just iterate over all A matrices and multiply in correct config part (looping over keys) - /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) - for (size_t pos = 0; pos < size(); ++pos) { - Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], - accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - /// multiply with alpha - Ax *= alpha; - - /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos) { - MapD(y + accumulatedDims[keys_[pos]], - accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - } - } - - /** Raw memory access version of multiplyHessianAdd */ + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { - if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -131,10 +101,13 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } - /** Raw memory access version of hessianDiagonal - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void hessianDiagonal(double* d) const { + /// Expose base class hessianDiagonal + virtual VectorValues hessianDiagonal() const { + return JacobianFactor::hessianDiagonal(); + } + + /// Raw memory access version of hessianDiagonal + void hessianDiagonal(double* d) const { // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -152,10 +125,13 @@ public: } } - /** Raw memory access version of gradientAtZero - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void gradientAtZero(double* d) const { + /// Expose base class gradientAtZero + virtual VectorValues gradientAtZero() const { + return JacobianFactor::gradientAtZero(); + } + + /// Raw memory access version of gradientAtZero + void gradientAtZero(double* d) const { // Get vector b not weighted Vector b = getb(); @@ -179,7 +155,78 @@ public: } } + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; + } + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) + return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + return model_ ? model_->whiten(Ax) : Ax; + } + + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO Frank asks: why is this here if not regular ???? + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } + } + }; +// end class RegularJacobianFactor -} - +}// end namespace gtsam From 82ce0b24061ab9d4abf49bd2fa32ddb7f440d653 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 08:10:58 +0100 Subject: [PATCH 53/58] Now are all RegularJacobianFactors --- gtsam/slam/JacobianFactorQ.h | 6 +++--- gtsam/slam/JacobianFactorQR.h | 6 +++--- gtsam/slam/JacobianFactorSVD.h | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index c18ad07d1..ed6213058 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -17,16 +17,16 @@ #pragma once -#include "JacobianSchurFactor.h" +#include "RegularJacobianFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; typedef std::pair KeyMatrixZD; diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index cf72fd0b3..4c1b0ff14 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,7 +6,7 @@ */ #pragma once -#include +#include #include #include @@ -18,9 +18,9 @@ class GaussianBayesNet; * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; typedef std::pair KeyMatrixZD; diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 37043f448..b4389d681 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,16 +5,16 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include "gtsam/slam/RegularJacobianFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; + typedef RegularJacobianFactor Base; typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 typedef std::pair KeyMatrixZD; typedef std::pair KeyMatrix; From fb3e5133c2f78fd5397246d02457f7cf6bc3ffaa Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 08:11:45 +0100 Subject: [PATCH 54/58] testSmartProjectionPoseFactor target --- .cproject | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/.cproject b/.cproject index 8eb74b58b..19d3912b0 100644 --- a/.cproject +++ b/.cproject @@ -1293,6 +1293,7 @@ make + testSimulated2DOriented.run true false @@ -1332,6 +1333,7 @@ make + testSimulated2D.run true false @@ -1339,6 +1341,7 @@ make + testSimulated3D.run true false @@ -1442,7 +1445,6 @@ make - testErrors.run true false @@ -1544,6 +1546,14 @@ true true + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + make -j3 @@ -1745,7 +1755,6 @@ cpack - -G DEB true false @@ -1753,7 +1762,6 @@ cpack - -G RPM true false @@ -1761,7 +1769,6 @@ cpack - -G TGZ true false @@ -1769,7 +1776,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1961,6 +1967,7 @@ make + tests/testGaussianISAM2 true false @@ -2112,7 +2119,6 @@ make - tests/testBayesTree.run true false @@ -2120,7 +2126,6 @@ make - testBinaryBayesNet.run true false @@ -2168,7 +2173,6 @@ make - testSymbolicBayesNet.run true false @@ -2176,7 +2180,6 @@ make - tests/testSymbolicFactor.run true false @@ -2184,7 +2187,6 @@ make - testSymbolicFactorGraph.run true false @@ -2200,7 +2202,6 @@ make - tests/testBayesTree true false @@ -3320,7 +3321,6 @@ make - testGraph.run true false @@ -3328,7 +3328,6 @@ make - testJunctionTree.run true false @@ -3336,7 +3335,6 @@ make - testSymbolicBayesNetB.run true false From 2151e86badab9314a96158a38a5761a51f301c89 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 08:13:29 +0100 Subject: [PATCH 55/58] Remove obsolete factor -> replaced by RegularJacobianFactor --- gtsam/slam/JacobianSchurFactor.h | 103 ------------------------------- 1 file changed, 103 deletions(-) delete mode 100644 gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 6a218a9a7..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,103 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement - * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD - * Provides raw memory access versions of linear operator. - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @brief double* Matrix-vector multiply, i.e. y = A*x - * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way - */ - Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); - if (empty()) - return Ax; - - // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); - - return model_ ? model_->whiten(Ax) : Ax; - } - - /** - * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e - * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way - */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { - Vector E = alpha * (model_ ? model_->whiten(e) : e); - // Just iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; - } - - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - JacobianFactor::multiplyHessianAdd(alpha, x, y); - } - - /** - * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) - * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way - */ - void multiplyHessianAdd(double alpha, const double* x, double* y) const { - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); - - // Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; - } - -}; -// end class JacobianSchurFactor - -}// end namespace gtsam From ac8869848201ab2433043b22a2ff07dbb5ab1127 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 10:15:24 +0100 Subject: [PATCH 56/58] Added a quick fix to unblock develop - not the solution I want. --- gtsam/geometry/CalibratedCamera.cpp | 3 + gtsam/geometry/CalibratedCamera.h | 134 +++++++++++++++++++++++++++- 2 files changed, 136 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 33f2c84eb..e0d57feff 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -23,6 +23,7 @@ using namespace std; namespace gtsam { +#ifndef PINHOLEBASE_LINKING_FIX /* ************************************************************************* */ Matrix26 PinholeBase::Dpose(const Point2& pn, double d) { // optimized version of derivatives, see CalibratedCamera.nb @@ -129,6 +130,8 @@ Point3 PinholeBase::backproject_from_camera(const Point2& p, return Point3(p.x() * depth, p.y() * depth, depth); } +#endif + /* ************************************************************************* */ 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 ed0c55208..1e8287a72 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,7 +19,10 @@ #pragma once #include - +#define PINHOLEBASE_LINKING_FIX +#ifdef PINHOLEBASE_LINKING_FIX +#include +#endif namespace gtsam { class Point2; @@ -43,6 +46,8 @@ private: Pose3 pose_; ///< 3D pose of camera +#ifndef PINHOLEBASE_LINKING_FIX + protected: /// @name Derivatives @@ -160,6 +165,133 @@ public: /// backproject a 2-dimensional point to a 3-dimensional point at given depth static Point3 backproject_from_camera(const Point2& p, const double depth); +#else + +public: + + PinholeBase() { + } + + explicit PinholeBase(const Pose3& pose) : + pose_(pose) { + } + + explicit PinholeBase(const Vector &v) : + pose_(Pose3::Expmap(v)) { + } + + const Pose3& pose() const { + return pose_; + } + + /* ************************************************************************* */ + static Matrix26 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; + } + + /* ************************************************************************* */ + static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt) { + // optimized version of derivatives, see CalibratedCamera.nb + const double u = pn.x(), v = pn.y(); + Matrix23 Dpn_point; + Dpn_point << // + Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), // + /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2); + Dpn_point *= d; + return Dpn_point; + } + + /* ************************************************************************* */ + static Pose3 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); + } + + /* ************************************************************************* */ + static Pose3 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 equals(const PinholeBase &camera, double tol) const { + return pose_.equals(camera.pose(), tol); + } + + /* ************************************************************************* */ + void print(const std::string& s) const { + pose_.print(s + ".pose"); + } + + /* ************************************************************************* */ + const Pose3& getPose(OptionalJacobian<6, 6> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; + } + return pose_; + } + + /* ************************************************************************* */ + static Point2 project_to_camera(const Point3& pc, + OptionalJacobian<2, 3> Dpoint = boost::none) { + 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); + } + + /* ************************************************************************* */ + 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(pn, pc.z() > 0); + } + + /* ************************************************************************* */ + Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint) const { + + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); + #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw CheiralityException(); + #endif + const Point2 pn = project_to_camera(q); + + if (Dpose || Dpoint) { + const double d = 1.0 / q.z(); + if (Dpose) + *Dpose = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt); + } + return pn; + } + + /* ************************************************************************* */ + static Point3 backproject_from_camera(const Point2& p, + const double depth) { + return Point3(p.x() * depth, p.y() * depth, depth); + } + +#endif + private: /** Serialization function */ From 951b568fb203a9b5e1b74f1cedf4f37a5fa2fd69 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 10:41:29 +0100 Subject: [PATCH 57/58] Added default argument --- gtsam/geometry/CalibratedCamera.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 1e8287a72..0e6f83fdf 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -227,7 +227,7 @@ public: } /* ************************************************************************* */ - bool equals(const PinholeBase &camera, double tol) const { + bool equals(const PinholeBase &camera, double tol=1e-9) const { return pose_.equals(camera.pose(), tol); } From 8d5e61a1bf8642042e20a408c61206e13a92151c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 23:29:40 +0100 Subject: [PATCH 58/58] Deprecated project with three derivatives, it's bogus: StereoCamera holds a pointer to a fixed calibration, and hence is similar to the new "PinholePose". --- gtsam/geometry/StereoCamera.cpp | 50 +++++++++++------------ gtsam/geometry/StereoCamera.h | 43 +++++++++---------- gtsam/geometry/tests/testStereoCamera.cpp | 8 +--- 3 files changed, 48 insertions(+), 53 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index eb83fd10f..9e5b88b31 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -29,16 +29,15 @@ namespace gtsam { } /* ************************************************************************* */ - StereoPoint2 StereoCamera::project(const Point3& point, - OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,0> H3) const { + StereoPoint2 StereoCamera::project(const Point3& point) const { + return project2(point); + } + + /* ************************************************************************* */ + StereoPoint2 StereoCamera::project2(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { -#ifdef STEREOCAMERA_CHAIN_RULE - const Point3 q = leftCamPose_.transform_to(point, H1, H2); -#else - // omit derivatives const Point3 q = leftCamPose_.transform_to(point); -#endif if ( q.z() <= 0 ) throw StereoCheiralityException(); @@ -56,12 +55,6 @@ namespace gtsam { // check if derivatives need to be computed if (H1 || H2) { -#ifdef STEREOCAMERA_CHAIN_RULE - // just implement chain rule - Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian - if (H1) *H1 = D_project_point*(*H1); - if (H2) *H2 = D_project_point*(*H2); -#else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; @@ -76,9 +69,6 @@ namespace gtsam { fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; *H2 << d * (*H2); } -#endif - if (H3) - throw std::runtime_error("StereoCamera::project does not support third derivative yet"); } // finally translate @@ -86,15 +76,23 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { - double d = 1.0 / P.z(), d2 = d*d; - const Cal3_S2Stereo& K = *K_; - double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - Matrix3 m; - m << f_x*d, 0.0, -d2*f_x* P.x(), - f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y(); - return m; + StereoPoint2 StereoCamera::project(const Point3& point, + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,0> H3) const { + if (H3) + throw std::runtime_error( + "StereoCamera::project does not support third derivative - BTW use project2"); + return project2(point,H1,H2); + } + + /* ************************************************************************* */ + Point3 StereoCamera::backproject(const StereoPoint2& z) const { + Vector measured = z.vector(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); + Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return world_point; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 88ffbcdbd..ea28ecfc7 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -17,9 +17,6 @@ #pragma once -#include - -#include #include #include #include @@ -127,33 +124,37 @@ public: return K_->baseline(); } - /* - * project 3D point and compute optional derivatives + /// Project 3D point to StereoPoint2 (uL,uR,v) + StereoPoint2 project(const Point3& point) const; + + /** Project 3D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; + + /// back-project a measurement + Point3 backproject(const StereoPoint2& z) const; + + /// @} + /// @name Deprecated + /// @{ + + /** Project 3D point and compute optional derivatives + * @deprecated, use project2 - this class has fixed calibration * @param H1 derivative with respect to pose * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = - boost::none, OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 0> H3 = boost::none) const; - - /// back-project a measurement - Point3 backproject(const StereoPoint2& z) const { - Vector measured = z.vector(); - double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); - double X = Z * (measured[0] - K_->px()) / K_->fx(); - double Y = Z * (measured[2] - K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; - } + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = + boost::none) const; /// @} private: - /// utility function - Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; - friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index c77509b91..45f26c244 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -96,16 +96,12 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_ TEST( StereoCamera, Dproject) { Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); - Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none); + Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); CHECK(assert_equal(expected1,actual1,1e-7)); Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); - Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none); + Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); - - Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K); - Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3); -// CHECK(assert_equal(expected3,actual3,1e-8)); } /* ************************************************************************* */