diff --git a/.cproject b/.cproject index 2439190b6..b09810484 100644 --- a/.cproject +++ b/.cproject @@ -309,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,26 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -679,26 +685,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -775,6 +781,14 @@ true true + + make + -j5 + testInvDepthFactor3.run + true + true + true + make -j5 @@ -807,6 +821,14 @@ true true + + make + -j5 + testInvDepthCamera3.run + true + true + true + make -j2 @@ -953,6 +975,7 @@ make + testGraph.run true false @@ -960,6 +983,7 @@ make + testJunctionTree.run true false @@ -967,6 +991,7 @@ make + testSymbolicBayesNetB.run true false @@ -1102,6 +1127,7 @@ make + testErrors.run true false @@ -1565,7 +1591,6 @@ make - testSimulated2DOriented.run true false @@ -1605,7 +1630,6 @@ make - testSimulated2D.run true false @@ -1613,7 +1637,6 @@ make - testSimulated3D.run true false @@ -1829,7 +1852,6 @@ make - tests/testGaussianISAM2 true false @@ -1851,102 +1873,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j1 @@ -2148,6 +2074,7 @@ cpack + -G DEB true false @@ -2155,6 +2082,7 @@ cpack + -G RPM true false @@ -2162,6 +2090,7 @@ cpack + -G TGZ true false @@ -2169,6 +2098,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2310,34 +2240,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2381,6 +2375,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 64311cdca..c38015db9 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -2,6 +2,7 @@ # and also build tests set (gtsam_unstable_subdirs base + geometry discrete # linear dynamics @@ -31,6 +32,7 @@ endforeach(subdir) # assemble gtsam_unstable components set(gtsam_unstable_srcs ${base_srcs} + ${geometry_srcs} ${discrete_srcs} ${dynamics_srcs} #${linear_srcs} diff --git a/gtsam_unstable/geometry/CMakeLists.txt b/gtsam_unstable/geometry/CMakeLists.txt new file mode 100644 index 000000000..d6b6a706d --- /dev/null +++ b/gtsam_unstable/geometry/CMakeLists.txt @@ -0,0 +1,22 @@ +# Install headers +file(GLOB geometry_headers "*.h") +install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry) + +# Components to link tests in this subfolder against +set(geometry_local_libs + #geometry_unstable # No sources currently, so no convenience lib + geometry + base + ccolamd +) + +set (geometry_full_libs + gtsam-static + gtsam_unstable-static) + +# Exclude tests that don't work +set (geometry_excluded_tests "") + +# Add all tests +gtsam_add_subdir_tests(geometry_unstable "${geometry_local_libs}" "${geometry_full_libs}" "${geometry_excluded_tests}") +add_dependencies(check.unstable check.geometry_unstable) diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h new file mode 100644 index 000000000..d0b9ccd88 --- /dev/null +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -0,0 +1,179 @@ + +/** + * @file InvDepthCamera3.h + * @brief + * @author Chris Beall + * @date Apr 14, 2012 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * A pinhole camera class that has a Pose3 and a Calibration. + * @ingroup geometry + * \nosubgrouping + */ +template +class InvDepthCamera3 { +private: + Pose3 pose_; + boost::shared_ptr k_; + +public: + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + InvDepthCamera3() {} + + /** constructor with pose and calibration */ + InvDepthCamera3(const Pose3& pose, const boost::shared_ptr& k) : + pose_(pose),k_(k) {} + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~InvDepthCamera3() {} + + /// return pose + inline Pose3& pose() { return pose_; } + + /// return calibration + inline const boost::shared_ptr& calibration() const { return k_; } + + /// print + void print(const std::string& s = "") const { + pose_.print("pose3"); + k_.print("calibration"); + } + + static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) { + gtsam::Point3 ray_base(pw.vector().segment(0,3)); + double theta = pw(3), phi = pw(4); + double rho = inv.value(); // inverse depth + gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); + return ray_base + m/rho; + } + + /** project a point from world InvDepth parameterization to the image + * @param pw is a point in the world coordinate + * @param H1 is the jacobian w.r.t. [pose3 calibration] + * @param H2 is the jacobian w.r.t. inv_depth_landmark + */ + inline gtsam::Point2 project(const gtsam::LieVector& pw, + const gtsam::LieScalar& inv_depth, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + + gtsam::Point3 ray_base(pw.vector().segment(0,3)); + double theta = pw(3), phi = pw(4); + double rho = inv_depth.value(); // inverse depth + gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); + const gtsam::Point3 landmark = ray_base + m/rho; + + gtsam::PinholeCamera camera(pose_, *k_); + + if (!H1 && !H2 && !H3) { + gtsam::Point2 uv= camera.project(landmark); + return uv; + } + else { + gtsam::Matrix J2; + gtsam::Point2 uv= camera.project(landmark,H1, J2); + if (H1) { + *H1 = (*H1) * gtsam::eye(6); + } + + double cos_theta = cos(theta); + double sin_theta = sin(theta); + double cos_phi = cos(phi); + double sin_phi = sin(phi); + double rho2 = rho * rho; + + if (H2) { + double H11 = 1; + double H12 = 0; + double H13 = 0; + double H14 = -cos_phi*sin_theta/rho; + double H15 = -cos_theta*sin_phi/rho; + + double H21 = 0; + double H22 = 1; + double H23 = 0; + double H24 = cos_phi*cos_theta/rho; + double H25 = -sin_phi*sin_theta/rho; + + double H31 = 0; + double H32 = 0; + double H33 = 1; + double H34 = 0; + double H35 = cos_phi/rho; + + *H2 = J2 * gtsam::Matrix_(3,5, + H11, H12, H13, H14, H15, + H21, H22, H23, H24, H25, + H31, H32, H33, H34, H35); + } + if(H3) { + double H16 = -cos_phi*cos_theta/rho2; + double H26 = -cos_phi*sin_theta/rho2; + double H36 = -sin_phi/rho2; + *H3 = J2 * gtsam::Matrix_(3,1, + H16, + H26, + H36); + } + return uv; + } + } + + /** + * backproject a 2-dimensional point to an Inverse Depth landmark + * useful for point initialization + */ + + inline std::pair backproject(const gtsam::Point2& pi, const double depth) const { + const gtsam::Point2 pn = k_->calibrate(pi); + gtsam::Point3 pc(pn.x(), pn.y(), 1.0); + pc = pc/pc.norm(); + gtsam::Point3 pw = pose_.transform_from(pc); + const gtsam::Point3& pt = pose_.translation(); + gtsam::Point3 ray = pw - pt; + double theta = atan2(ray.y(), ray.x()); // longitude + double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); + return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi), + gtsam::LieScalar(1./depth)); + } + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** 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_); + } + /// @} +}; // \class InvDepthCamera +} // \namesapce gtsam diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp new file mode 100644 index 000000000..1e1dae189 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -0,0 +1,157 @@ +/* + * testInvDepthFactor.cpp + * + * Created on: Apr 13, 2012 + * Author: cbeall3 + */ + +#include + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); +Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +SimpleCamera level_camera(level_pose, *K); + +/* ************************************************************************* */ +TEST( InvDepthFactor, Project1) { + + // landmark 5 meters infront of camera + Point3 landmark(5, 0, 1); + + Point2 expected_uv = level_camera.project(landmark); + + InvDepthCamera3 inv_camera(level_pose, K); + LieVector inv_landmark(5, 1., 0., 1., 0., 0.); + LieScalar inv_depth(1./4); + Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); + EXPECT(assert_equal(expected_uv, actual_uv)); + EXPECT(assert_equal(Point2(640,480), actual_uv)); +} + +/* ************************************************************************* */ +TEST( InvDepthFactor, Project2) { + + // landmark 1m to the left and 1m up from camera + // inv landmark xyz is same as camera xyz, so depth actually doesn't matter + Point3 landmark(1, 1, 2); + Point2 expected = level_camera.project(landmark); + + InvDepthCamera3 inv_camera(level_pose, K); + LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1/sqrt(2))); + LieScalar inv_depth(1/sqrt(3)); + Point2 actual = inv_camera.project(diag_landmark, inv_depth); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( InvDepthFactor, Project3) { + + // landmark 1m to the left and 1m up from camera + // inv depth landmark xyz at origion + Point3 landmark(1, 1, 2); + Point2 expected = level_camera.project(landmark); + + InvDepthCamera3 inv_camera(level_pose, K); + LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2))); + LieScalar inv_depth( 1./sqrt(1+1+4)); + Point2 actual = inv_camera.project(diag_landmark, inv_depth); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( InvDepthFactor, Project4) { + + // landmark 4m to the left and 1m up from camera + // inv depth landmark xyz at origion + Point3 landmark(1, 4, 2); + Point2 expected = level_camera.project(landmark); + + InvDepthCamera3 inv_camera(level_pose, K); + LieVector diag_landmark(5, 0., 0., 0., atan(4/1), atan(2./sqrt(1+16))); + LieScalar inv_depth(1./sqrt(1+16+4)); + Point2 actual = inv_camera.project(diag_landmark, inv_depth); + EXPECT(assert_equal(expected, actual)); +} + + +/* ************************************************************************* */ +Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) { + return InvDepthCamera3(pose,K).project(landmark, inv_depth); } + +TEST( InvDepthFactor, Dproject_pose) +{ + LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); + LieScalar inv_depth(1./4); + Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); + InvDepthCamera3 inv_camera(level_pose,K); + Matrix actual; + Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); + EXPECT(assert_equal(expected,actual,1e-6)); +} + +/* ************************************************************************* */ +TEST( InvDepthFactor, Dproject_landmark) +{ + LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieScalar inv_depth(1./4); + Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); + InvDepthCamera3 inv_camera(level_pose,K); + Matrix actual; + Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); + EXPECT(assert_equal(expected,actual,1e-7)); +} + +/* ************************************************************************* */ +TEST( InvDepthFactor, Dproject_inv_depth) +{ + LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); + LieScalar inv_depth(1./4); + Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); + InvDepthCamera3 inv_camera(level_pose,K); + Matrix actual; + Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); + EXPECT(assert_equal(expected,actual,1e-7)); +} + +/* ************************************************************************* */ +TEST(InvDepthFactor, backproject) +{ + LieVector expected(5,0.,0.,1., 0.1,0.2); + LieScalar inv_depth(1./4); + InvDepthCamera3 inv_camera(level_pose,K); + Point2 z = inv_camera.project(expected, inv_depth); + + LieVector actual_vec; + LieScalar actual_inv; + boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); + EXPECT(assert_equal(expected,actual_vec,1e-7)); + EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); +} + +/* ************************************************************************* */ +TEST(InvDepthFactor, backproject2) +{ + // backwards facing camera + LieVector expected(5,-5.,-5.,2., 3., -0.1); + LieScalar inv_depth(1./10); + InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); + Point2 z = inv_camera.project(expected, inv_depth); + + LieVector actual_vec; + LieScalar actual_inv; + boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); + EXPECT(assert_equal(expected,actual_vec,1e-7)); + EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/InvDepthCamera3.h b/gtsam_unstable/slam/InvDepthCamera3.h deleted file mode 100644 index 54f5c2939..000000000 --- a/gtsam_unstable/slam/InvDepthCamera3.h +++ /dev/null @@ -1,179 +0,0 @@ - -/** - * @file InvDepthCamera3.h - * @brief - * @author Chris Beall - * @date Apr 14, 2012 - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - - /** - * A pinhole camera class that has a Pose3 and a Calibration. - * @ingroup geometry - * \nosubgrouping - */ - template - class InvDepthCamera3 { - private: - Pose3 pose_; - boost::shared_ptr k_; - - public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - InvDepthCamera3() {} - - /** constructor with pose and calibration */ - InvDepthCamera3(const Pose3& pose, const boost::shared_ptr& k) : - pose_(pose),k_(k) {} - - /// @} - /// @name Standard Interface - /// @{ - - virtual ~InvDepthCamera3() {} - - /// return pose - inline Pose3& pose() { return pose_; } - - /// return calibration - inline const boost::shared_ptr& calibration() const { return k_; } - - /// print - void print(const std::string& s = "") const { - pose_.print("pose3"); - k_.print("calibration"); - } - - static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) { - gtsam::Point3 ray_base(pw.vector().segment(0,3)); - double theta = pw(3), phi = pw(4); - double rho = inv.value(); // inverse depth - gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); - return ray_base + m/rho; - } - - /** project a point from world InvDepth parameterization to the image - * @param pw is a point in the world coordinate - * @param H1 is the jacobian w.r.t. [pose3 calibration] - * @param H2 is the jacobian w.r.t. inv_depth_landmark - */ - inline gtsam::Point2 project(const gtsam::LieVector& pw, - const gtsam::LieScalar& inv_depth, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { - - gtsam::Point3 ray_base(pw.vector().segment(0,3)); - double theta = pw(3), phi = pw(4); - double rho = inv_depth.value(); // inverse depth - gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); - const gtsam::Point3 landmark = ray_base + m/rho; - - gtsam::PinholeCamera camera(pose_, *k_); - - if (!H1 && !H2 && !H3) { - gtsam::Point2 uv= camera.project(landmark); - return uv; - } - else { - gtsam::Matrix J2; - gtsam::Point2 uv= camera.project(landmark,H1, J2); - if (H1) { - *H1 = (*H1) * gtsam::eye(6); - } - - double cos_theta = cos(theta); - double sin_theta = sin(theta); - double cos_phi = cos(phi); - double sin_phi = sin(phi); - double rho2 = rho * rho; - - if (H2) { - double H11 = 1; - double H12 = 0; - double H13 = 0; - double H14 = -cos_phi*sin_theta/rho; - double H15 = -cos_theta*sin_phi/rho; - - double H21 = 0; - double H22 = 1; - double H23 = 0; - double H24 = cos_phi*cos_theta/rho; - double H25 = -sin_phi*sin_theta/rho; - - double H31 = 0; - double H32 = 0; - double H33 = 1; - double H34 = 0; - double H35 = cos_phi/rho; - - *H2 = J2 * gtsam::Matrix_(3,5, - H11, H12, H13, H14, H15, - H21, H22, H23, H24, H25, - H31, H32, H33, H34, H35); - } - if(H3) { - double H16 = -cos_phi*cos_theta/rho2; - double H26 = -cos_phi*sin_theta/rho2; - double H36 = -sin_phi/rho2; - *H3 = J2 * gtsam::Matrix_(3,1, - H16, - H26, - H36); - } - return uv; - } - } - - /** - * backproject a 2-dimensional point to an Inverse Depth landmark - * useful for point initialization - */ - - inline std::pair backproject(const gtsam::Point2& pi, const double depth) const { - const gtsam::Point2 pn = k_->calibrate(pi); - gtsam::Point3 pc(pn.x(), pn.y(), 1.0); - pc = pc/pc.norm(); - gtsam::Point3 pw = pose_.transform_from(pc); - const gtsam::Point3& pt = pose_.translation(); - gtsam::Point3 ray = pw - pt; - double theta = atan2(ray.y(), ray.x()); // longitude - double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); - return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi), - gtsam::LieScalar(1./depth)); - } - -private: - - /// @} - /// @name Advanced Interface - /// @{ - - /** 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_); - } - /// @} - }; -} diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 17875e80d..daaf64575 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -9,103 +9,106 @@ #include #include -#include -#include +#include namespace gtsam { - template - class InvDepthFactor3: public gtsam::NoiseModelFactor3 { - protected: +/** + * Ternary factor representing a visual measurement that includes inverse depth + */ +template +class InvDepthFactor3: public gtsam::NoiseModelFactor3 { +protected: - // Keep a copy of measurement and calibration for I/O - gtsam::Point2 measured_; ///< 2D measurement - boost::shared_ptr K_; ///< shared pointer to calibration object + // Keep a copy of measurement and calibration for I/O + gtsam::Point2 measured_; ///< 2D measurement + boost::shared_ptr K_; ///< shared pointer to calibration object - public: +public: - /// shorthand for base class type - typedef gtsam::NoiseModelFactor3 Base; + /// shorthand for base class type + typedef gtsam::NoiseModelFactor3 Base; - /// shorthand for this class - typedef InvDepthFactor3 This; + /// shorthand for this class + typedef InvDepthFactor3 This; - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; - /// Default constructor - InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {} + /// Default constructor + InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {} - /** - * Constructor - * TODO: Mark argument order standard (keys, measurement, parameters) - * @param z is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation - * @param j_pose is basically the frame number - * @param j_landmark is the index of the landmark - * @param K shared pointer to the constant calibration - */ - InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model, - const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const gtsam::shared_ptrK& K) : - Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {} + /** + * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param z is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param j_pose is basically the frame number + * @param j_landmark is the index of the landmark + * @param K shared pointer to the constant calibration + */ + InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model, + const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const gtsam::shared_ptrK& K) : + Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {} - /** Virtual destructor */ - virtual ~InvDepthFactor3() {} + /** Virtual destructor */ + virtual ~InvDepthFactor3() {} - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s = "InvDepthFactor3", - const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); + /** + * print + * @param s optional string naming the factor + */ + void print(const std::string& s = "InvDepthFactor3", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /// equals + virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const { + try { + InvDepthCamera3 camera(pose, K_); + gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); + return reprojectionError.vector(); + } catch( CheiralityException& e) { + if (H1) *H1 = gtsam::zeros(2,6); + if (H2) *H2 = gtsam::zeros(2,5); + if (H3) *H2 = gtsam::zeros(2,1); + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + return gtsam::ones(2) * 2.0 * K_->fx(); } + return gtsam::Vector_(1, 0.0); + } - /// equals - virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const { - const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); - } + /** return the measurement */ + const gtsam::Point2& imagePoint() const { + return measured_; + } - /// Evaluate error h(x)-z and optionally derivatives - gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const { - try { - InvDepthCamera3 camera(pose, K_); - gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); - return reprojectionError.vector(); - } catch( CheiralityException& e) { - if (H1) *H1 = gtsam::zeros(2,6); - if (H2) *H2 = gtsam::zeros(2,5); - if (H3) *H2 = gtsam::zeros(2,1); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); - } - } + /** return the calibration object */ + inline const gtsam::Cal3_S2::shared_ptr calibration() const { + return K_; + } - /** return the measurement */ - const gtsam::Point2& imagePoint() const { - return measured_; - } +private: - /** return the calibration object */ - inline const gtsam::Cal3_S2::shared_ptr calibration() const { - return K_; - } - - private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(K_); - } - }; + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(K_); + } +}; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 11986fc6a..5567bc9ce 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -5,156 +5,25 @@ * Author: cbeall3 */ -#include -#include #include + +#include #include #include #include #include +#include + using namespace std; using namespace gtsam; -/* ************************************************************************* */ static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); +static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); -/* ************************************************************************* */ -TEST( InvDepthFactor, Project1) { - - // landmark 5 meters infront of camera - Point3 landmark(5, 0, 1); - - Point2 expected_uv = level_camera.project(landmark); - - InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark(5, 1., 0., 1., 0., 0.); - LieScalar inv_depth(1./4); - Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); - CHECK(assert_equal(expected_uv, actual_uv)); - CHECK(assert_equal(Point2(640,480), actual_uv)); - -} - -/* ************************************************************************* */ -TEST( InvDepthFactor, Project2) { - - // landmark 1m to the left and 1m up from camera - // inv landmark xyz is same as camera xyz, so depth actually doesn't matter - Point3 landmark(1, 1, 2); - Point2 expected = level_camera.project(landmark); - - InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1/sqrt(2))); - LieScalar inv_depth(1/sqrt(3)); - Point2 actual = inv_camera.project(diag_landmark, inv_depth); - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( InvDepthFactor, Project3) { - - // landmark 1m to the left and 1m up from camera - // inv depth landmark xyz at origion - Point3 landmark(1, 1, 2); - Point2 expected = level_camera.project(landmark); - - InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2))); - LieScalar inv_depth( 1./sqrt(1+1+4)); - Point2 actual = inv_camera.project(diag_landmark, inv_depth); - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( InvDepthFactor, Project4) { - - // landmark 4m to the left and 1m up from camera - // inv depth landmark xyz at origion - Point3 landmark(1, 4, 2); - Point2 expected = level_camera.project(landmark); - - InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark(5, 0., 0., 0., atan(4/1), atan(2./sqrt(1+16))); - LieScalar inv_depth(1./sqrt(1+16+4)); - Point2 actual = inv_camera.project(diag_landmark, inv_depth); - CHECK(assert_equal(expected, actual)); -} - - -/* ************************************************************************* */ -Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) { - return InvDepthCamera3(pose,K).project(landmark, inv_depth); } - -TEST( InvDepthFactor, Dproject_pose) -{ - LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); - LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); - InvDepthCamera3 inv_camera(level_pose,K); - Matrix actual; - Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); - CHECK(assert_equal(expected,actual,1e-6)); -} - -/* ************************************************************************* */ -TEST( InvDepthFactor, Dproject_landmark) -{ - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); - LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); - InvDepthCamera3 inv_camera(level_pose,K); - Matrix actual; - Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); - CHECK(assert_equal(expected,actual,1e-7)); -} - -/* ************************************************************************* */ -TEST( InvDepthFactor, Dproject_inv_depth) -{ - LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); - LieScalar inv_depth(1./4); - Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); - InvDepthCamera3 inv_camera(level_pose,K); - Matrix actual; - Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); - CHECK(assert_equal(expected,actual,1e-7)); -} - -/* ************************************************************************* */ -TEST(InvDepthFactor, backproject) -{ - LieVector expected(5,0.,0.,1., 0.1,0.2); - LieScalar inv_depth(1./4); - InvDepthCamera3 inv_camera(level_pose,K); - Point2 z = inv_camera.project(expected, inv_depth); - - LieVector actual_vec; - LieScalar actual_inv; - boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); - CHECK(assert_equal(expected,actual_vec,1e-7)); - CHECK(assert_equal(inv_depth,actual_inv,1e-7)); -} - -/* ************************************************************************* */ -TEST(InvDepthFactor, backproject2) -{ - // backwards facing camera - LieVector expected(5,-5.,-5.,2., 3., -0.1); - LieScalar inv_depth(1./10); - InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); - Point2 z = inv_camera.project(expected, inv_depth); - - LieVector actual_vec; - LieScalar actual_inv; - boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); - CHECK(assert_equal(expected,actual_vec,1e-7)); - CHECK(assert_equal(inv_depth,actual_inv,1e-7)); -} - -static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality PoseConstraint; @@ -183,7 +52,7 @@ TEST( InvDepthFactor, optimize) { LevenbergMarquardtParams lmParams; Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - CHECK(assert_equal(initial, result, 1e-9)); + EXPECT(assert_equal(initial, result, 1e-9)); /// Add a second camera @@ -214,7 +83,7 @@ TEST( InvDepthFactor, optimize) { result2.at(Symbol('l',1)), result2.at(Symbol('d',1))); - CHECK(assert_equal(landmark1, l1_result2, 1e-9)); + EXPECT(assert_equal(landmark1, l1_result2, 1e-9)); }