diff --git a/gtsam_unstable/geometry/TriangulationFactor.h b/gtsam_unstable/geometry/TriangulationFactor.h new file mode 100644 index 000000000..2405db5f0 --- /dev/null +++ b/gtsam_unstable/geometry/TriangulationFactor.h @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.h + * @date March 2, 2014 + * @author Frank Dellaert + */ + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration and pose are assumed known. + * @addtogroup SLAM + */ +template +class TriangulationFactor: public NoiseModelFactor1 { + +public: + + /// Camera type + typedef PinholeCamera Camera; + +protected: + + // Keep a copy of measurement and calibration for I/O + const Camera camera_; ///< Camera in which this landmark was seen + const Point2 measured_; ///< 2D measurement + + // verbosity handling for Cheirality Exceptions + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + +public: + + /// shorthand for base class type + typedef NoiseModelFactor1 Base; + + /// shorthand for this class + typedef TriangulationFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + TriangulationFactor() : + throwCheirality_(false), verboseCheirality_(false) { + } + + /** + * Constructor with exception-handling flags + * @param camera is the camera in which unknown landmark is seen + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param pointKey is the index of the landmark + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + */ + TriangulationFactor(const Camera& camera, const Point2& measured, + const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false, + bool verboseCheirality = false) : + Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( + throwCheirality), verboseCheirality_(verboseCheirality) { + } + + /** Virtual destructor */ + virtual ~TriangulationFactor() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "TriangulationFactor,"; + camera_.print("camera"); + measured_.print("z"); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol) + && this->measured_.equals(e->measured_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Point3& point, boost::optional H2 = + boost::none) const { + try { + Point2 error(camera_.project(point, boost::none, H2) - measured_); + return error.vector(); + } catch (CheiralityException& e) { + if (H2) + *H2 = zeros(2, 3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->key()) << " moved behind camera" + << std::endl; + if (throwCheirality_) + throw e; + return ones(2) * 2.0 * camera_.calibration().fx(); + } + } + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +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(camera_); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; +} // \ namespace gtsam + + diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 3061749b3..8d45311f1 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,50 +16,46 @@ * Author: cbeall3 */ -#include - -#include -#include -#include -#include - -#include #include +#include +#include #include #include -#include using namespace std; using namespace gtsam; using namespace boost::assign; -/* ************************************************************************* */ +// Some common constants -TEST( triangulation, twoPosesBundler) { - boost::shared_ptr sharedCal = // - boost::make_shared(1500, 0, 0, 640, 480); - // 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)); - PinholeCamera level_camera(level_pose, *sharedCal); +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); - PinholeCamera level_camera_right(level_pose_right, *sharedCal); +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); +// create second camera 1 meter to the right of first camera +static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); +PinholeCamera camera2(pose2, *sharedCal); - // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); - vector < Pose3 > poses; +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); +Point2 z2 = camera2.project(landmark); + +//****************************************************************************** +TEST( triangulation, twoPoses) { + + vector poses; vector measurements; - poses += level_pose, level_pose_right; - measurements += level_uv, level_uv_right; + poses += pose1, pose2; + measurements += z1, z2; bool optimize = true; double rank_tol = 1e-9; @@ -77,32 +73,48 @@ TEST( triangulation, twoPosesBundler) { EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); } -/* ************************************************************************* */ +//****************************************************************************** -TEST( triangulation, fourPoses) { - boost::shared_ptr sharedCal = // - boost::make_shared(1500, 1200, 0, 640, 480); - // 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)); - SimpleCamera level_camera(level_pose, *sharedCal); +TEST( triangulation, twoPosesBundler) { - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); - SimpleCamera level_camera_right(level_pose_right, *sharedCal); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + boost::shared_ptr bundlerCal = // + boost::make_shared(1500, 0, 0, 640, 480); + PinholeCamera camera1(pose1, *bundlerCal); + PinholeCamera camera2(pose2, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); - vector < Pose3 > poses; + vector poses; vector measurements; - poses += level_pose, level_pose_right; - measurements += level_uv, level_uv_right; + poses += pose1, pose2; + measurements += z1, z2; + + bool optimize = true; + double rank_tol = 1e-9; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); + + // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements.at(0) += Point2(0.1, 0.5); + measurements.at(1) += Point2(-0.2, 0.3); + + boost::optional triangulated_landmark_noise = triangulatePoint3(poses, + bundlerCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); +} + +//****************************************************************************** +TEST( triangulation, fourPoses) { + vector poses; + vector measurements; + + poses += pose1, pose2; + measurements += z1, z2; boost::optional triangulated_landmark = triangulatePoint3(poses, sharedCal, measurements); @@ -112,21 +124,20 @@ TEST( triangulation, fourPoses) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - sharedCal, measurements); + boost::optional triangulated_landmark_noise = // + triangulatePoint3(poses, sharedCal, measurements); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose_top = level_pose - * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - SimpleCamera camera_top(pose_top, *sharedCal); - Point2 top_uv = camera_top.project(landmark); + Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + SimpleCamera camera3(pose3, *sharedCal); + Point2 z3 = camera3.project(landmark); - poses += pose_top; - measurements += top_uv + Point2(0.1, -0.1); + poses += pose3; + measurements += z3 + Point2(0.1, -0.1); - boost::optional triangulated_3cameras = triangulatePoint3(poses, - sharedCal, measurements); + boost::optional triangulated_3cameras = // + triangulatePoint3(poses, sharedCal, measurements); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization @@ -135,48 +146,42 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); - SimpleCamera camera_180(level_pose180, *sharedCal); + Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + SimpleCamera camera4(pose4, *sharedCal); - CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); - poses += level_pose180; + poses += pose4; measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), TriangulationCheiralityException); +#endif } -/* ************************************************************************* */ - +//****************************************************************************** TEST( triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // 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)); - SimpleCamera level_camera(level_pose, K1); + SimpleCamera camera1(pose1, K1); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); Cal3_S2 K2(1600, 1300, 0, 650, 440); - SimpleCamera level_camera_right(level_pose_right, K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + SimpleCamera camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); vector cameras; vector measurements; - cameras += level_camera, level_camera_right; - measurements += level_uv, level_uv_right; + cameras += camera1, camera2; + measurements += z1, z2; - boost::optional triangulated_landmark = triangulatePoint3(cameras, - measurements); + boost::optional triangulated_landmark = // + triangulatePoint3(cameras, measurements); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) @@ -188,17 +193,16 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose_top = level_pose - * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); - SimpleCamera camera_top(pose_top, K3); - Point2 top_uv = camera_top.project(landmark); + SimpleCamera camera3(pose3, K3); + Point2 z3 = camera3.project(landmark); - cameras += camera_top; - measurements += top_uv + Point2(0.1, -0.1); + cameras += camera3; + measurements += z3 + Point2(0.1, -0.1); - boost::optional triangulated_3cameras = triangulatePoint3(cameras, - measurements); + boost::optional triangulated_3cameras = // + triangulatePoint3(cameras, measurements); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization @@ -207,47 +211,40 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), - Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Cal3_S2 K4(700, 500, 0, 640, 480); - SimpleCamera camera_180(level_pose180, K4); + SimpleCamera camera4(pose4, K4); - CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); - cameras += camera_180; + cameras += camera4; measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), TriangulationCheiralityException); +#endif } -/* ************************************************************************* */ - +//****************************************************************************** TEST( triangulation, twoIdenticalPoses) { - boost::shared_ptr sharedCal = // - boost::make_shared(1500, 1200, 0, 640, 480); // 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)); - SimpleCamera level_camera(level_pose, *sharedCal); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + SimpleCamera camera1(pose1, *sharedCal); // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); + Point2 z1 = camera1.project(landmark); - vector < Pose3 > poses; + vector poses; vector measurements; - poses += level_pose, level_pose; - measurements += level_uv, level_uv; + poses += pose1, pose1; + measurements += z1, z1; CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), TriangulationUnderconstrainedException); } -/* ************************************************************************* * - +//****************************************************************************** +/* TEST( triangulation, onePose) { // we expect this test to fail with a TriangulationUnderconstrainedException // because there's only one camera observation @@ -263,10 +260,34 @@ TEST( triangulation, twoIdenticalPoses) { CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), TriangulationUnderconstrainedException); } + */ - /* ************************************************************************* */ +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey, sharedCal); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + +// Matrix expectedH1 = numericalDerivative11( +// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, +// boost::none, boost::none), pose1); + // The expected Jacobian + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 559871059..3017fdf7a 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -18,6 +18,9 @@ #include +#include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 0cb9d4838..f767514c1 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -18,19 +18,11 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include #include - -#include -#include -#include +#include +#include +#include +#include #include @@ -60,10 +52,9 @@ public: * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); - -// Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem -// We should have a projectionfactor that knows pose is fixed +GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol); /// /** @@ -87,10 +78,9 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; - graph.push_back(GenericProjectionFactor // - (measurements[i], unit2, i, landmarkKey, sharedCal)); - graph.push_back(PriorFactor(i, pose_i, prior_model)); - values.insert(i, pose_i); + PinholeCamera camera_i(pose_i, *sharedCal); + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } @@ -116,13 +106,8 @@ std::pair triangulationGraph( static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); for (size_t i = 0; i < measurements.size(); i++) { const PinholeCamera& camera_i = cameras[i]; - boost::shared_ptr // Seems wasteful to create new object - sharedCal(new CALIBRATION(camera_i.calibration())); - graph.push_back(GenericProjectionFactor // - (measurements[i], unit2, i, landmarkKey, sharedCal)); - const Pose3& pose_i = camera_i.pose(); - graph.push_back(PriorFactor(i, pose_i, prior_model)); - values.insert(i, pose_i); + graph.push_back(TriangulationFactor // + (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); } @@ -135,7 +120,8 @@ std::pair triangulationGraph( * @param landmarkKey to refer to landmark * @return refined Point3 */ -GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey); +GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, + const Values& values, Key landmarkKey); /** * Given an initial estimate , refine a point using measurements in several cameras @@ -221,7 +207,7 @@ Point3 triangulatePoint3(const std::vector& poses, BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -271,7 +257,7 @@ Point3 triangulatePoint3( BOOST_FOREACH(const Camera& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif