From ba6f8576630501209fc1cc116fda6774fa77ff84 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 13:02:21 -0500 Subject: [PATCH 1/9] Removed some copy/paste --- .../geometry/tests/testTriangulation.cpp | 45 +++++++------------ 1 file changed, 15 insertions(+), 30 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 3061749b3..91361a8dd 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -34,22 +34,28 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +// Some common constants +static const Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + gtsam::Point3(0, 0, 1)); +static const Pose3 level_pose_right = level_pose + * Pose3(Rot3(), Point3(1, 0, 0)); + +// landmark ~5 meters infront of camera +Point3 landmark(5, 0.5, 1.2); + +boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + /* ************************************************************************* */ TEST( triangulation, twoPosesBundler) { - boost::shared_ptr sharedCal = // + boost::shared_ptr bundlerCal = // 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); + PinholeCamera level_camera(level_pose, *bundlerCal); // 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); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + PinholeCamera level_camera_right(level_pose_right, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate Point2 level_uv = level_camera.project(landmark); @@ -80,20 +86,12 @@ TEST( triangulation, twoPosesBundler) { /* ************************************************************************* */ 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); // 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); - // 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); @@ -153,18 +151,12 @@ TEST( triangulation, fourPoses) { 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); // 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); - // 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); @@ -223,16 +215,9 @@ TEST( triangulation, fourPoses_distinct_Ks) { /* ************************************************************************* */ 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); - // 1. Project two landmarks into two cameras and triangulate Point2 level_uv = level_camera.project(landmark); From 68401cf2161b62e6c40ab8b65b5a630dd7244d52 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 13:16:36 -0500 Subject: [PATCH 2/9] removed Cheirality testing in that GTSAM mode --- .../geometry/tests/testTriangulation.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 91361a8dd..118673831 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -35,26 +35,28 @@ using namespace gtsam; using namespace boost::assign; // Some common constants + +// Looking along X-axis, 1 meter above ground plane (x-y) static const Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), gtsam::Point3(0, 0, 1)); + +// create second camera 1 meter to the right of first camera static const Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); // landmark ~5 meters infront of camera -Point3 landmark(5, 0.5, 1.2); +static const Point3 landmark(5, 0.5, 1.2); -boost::shared_ptr sharedCal = // +static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); /* ************************************************************************* */ TEST( triangulation, twoPosesBundler) { + boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0, 0, 640, 480); - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) PinholeCamera level_camera(level_pose, *bundlerCal); - - // create second camera 1 meter to the right of first camera PinholeCamera level_camera_right(level_pose_right, *bundlerCal); // 1. Project two landmarks into two cameras and triangulate @@ -71,7 +73,7 @@ TEST( triangulation, twoPosesBundler) { double rank_tol = 1e-9; boost::optional triangulated_landmark = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); + 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) @@ -79,7 +81,7 @@ TEST( triangulation, twoPosesBundler) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - sharedCal, measurements, rank_tol, optimize); + bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); } @@ -137,6 +139,7 @@ TEST( triangulation, fourPoses) { Point3(0, 0, 1)); SimpleCamera camera_180(level_pose180, *sharedCal); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); poses += level_pose180; @@ -144,6 +147,7 @@ TEST( triangulation, fourPoses) { CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), TriangulationCheiralityException); +#endif } /* ************************************************************************* */ @@ -204,12 +208,14 @@ TEST( triangulation, fourPoses_distinct_Ks) { Cal3_S2 K4(700, 500, 0, 640, 480); SimpleCamera camera_180(level_pose180, K4); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); cameras += camera_180; measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), TriangulationCheiralityException); +#endif } /* ************************************************************************* */ @@ -231,8 +237,8 @@ TEST( triangulation, twoIdenticalPoses) { TriangulationUnderconstrainedException); } -/* ************************************************************************* * - +/* ************************************************************************* */ +/* TEST( triangulation, onePose) { // we expect this test to fail with a TriangulationUnderconstrainedException // because there's only one camera observation @@ -248,7 +254,7 @@ TEST( triangulation, twoIdenticalPoses) { CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), TriangulationUnderconstrainedException); } - +*/ /* ************************************************************************* */ int main() { TestResult tr; From 7b93cd207c0d1eca43916803bd24f15c0f6c3a75 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 13:34:43 -0500 Subject: [PATCH 3/9] fixed header bloat --- .../geometry/tests/testTriangulation.cpp | 11 ++----- gtsam_unstable/geometry/triangulation.cpp | 3 ++ gtsam_unstable/geometry/triangulation.h | 31 ++++++++----------- 3 files changed, 18 insertions(+), 27 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 118673831..9d7cf72a2 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,19 +16,12 @@ * Author: cbeall3 */ -#include - -#include -#include -#include -#include - -#include #include +#include +#include #include #include -#include using namespace std; using namespace gtsam; 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..49998d117 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 @@ -60,7 +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); +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 @@ -135,7 +129,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 +216,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 @@ -256,9 +251,9 @@ Point3 triangulatePoint3( typedef PinholeCamera Camera; std::vector projection_matrices; BOOST_FOREACH(const Camera& camera, cameras) - projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); + projection_matrices.push_back( + camera.calibration().K() + * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -271,7 +266,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 From bf779af3d153fa41b3591647689113137c434755 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 13:44:18 -0500 Subject: [PATCH 4/9] added twoPoses test --- .../geometry/tests/testTriangulation.cpp | 60 +++++++++++++------ 1 file changed, 41 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 9d7cf72a2..24c725b72 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -29,19 +29,51 @@ using namespace boost::assign; // Some common constants +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + // Looking along X-axis, 1 meter above ground plane (x-y) static const Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), gtsam::Point3(0, 0, 1)); +PinholeCamera level_camera(level_pose, *sharedCal); // create second camera 1 meter to the right of first camera static const Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +PinholeCamera level_camera_right(level_pose_right, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); -static const boost::shared_ptr sharedCal = // - boost::make_shared(1500, 1200, 0, 640, 480); +// 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); + +/* ************************************************************************* */ + +TEST( triangulation, twoPoses) { + + vector poses; + vector measurements; + + poses += level_pose, level_pose_right; + measurements += level_uv, level_uv_right; + + bool optimize = true; + double rank_tol = 1e-9; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + sharedCal, 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, + sharedCal, measurements, rank_tol, optimize); + EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); +} /* ************************************************************************* */ @@ -56,7 +88,7 @@ TEST( triangulation, twoPosesBundler) { Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); - vector < Pose3 > poses; + vector poses; vector measurements; poses += level_pose, level_pose_right; @@ -81,17 +113,7 @@ TEST( triangulation, twoPosesBundler) { /* ************************************************************************* */ TEST( triangulation, fourPoses) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera level_camera(level_pose, *sharedCal); - - // create second camera 1 meter to the right of first camera - SimpleCamera level_camera_right(level_pose_right, *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); - - vector < Pose3 > poses; + vector poses; vector measurements; poses += level_pose, level_pose_right; @@ -133,7 +155,7 @@ TEST( triangulation, fourPoses) { SimpleCamera camera_180(level_pose180, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); + CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); poses += level_pose180; measurements += Point2(400, 400); @@ -202,7 +224,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { SimpleCamera camera_180(level_pose180, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); + CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); cameras += camera_180; measurements += Point2(400, 400); @@ -220,7 +242,7 @@ TEST( triangulation, twoIdenticalPoses) { // 1. Project two landmarks into two cameras and triangulate Point2 level_uv = level_camera.project(landmark); - vector < Pose3 > poses; + vector poses; vector measurements; poses += level_pose, level_pose; @@ -247,8 +269,8 @@ TEST( triangulation, twoIdenticalPoses) { CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), TriangulationUnderconstrainedException); } -*/ - /* ************************************************************************* */ + */ +/* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); From dfee108e53aba7942213ca1761c0efc018ab3ed4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 13:49:42 -0500 Subject: [PATCH 5/9] Some more refactoring --- .../geometry/tests/testTriangulation.cpp | 111 +++++++++--------- 1 file changed, 53 insertions(+), 58 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 24c725b72..5f39b694b 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -33,21 +33,20 @@ static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) -static const 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 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); // create second camera 1 meter to the right of first camera -static const Pose3 level_pose_right = level_pose - * Pose3(Rot3(), Point3(1, 0, 0)); -PinholeCamera level_camera_right(level_pose_right, *sharedCal); +static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); +PinholeCamera camera2(pose2, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); // 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); /* ************************************************************************* */ @@ -56,8 +55,8 @@ 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; @@ -81,18 +80,18 @@ TEST( triangulation, twoPosesBundler) { boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0, 0, 640, 480); - PinholeCamera level_camera(level_pose, *bundlerCal); - PinholeCamera level_camera_right(level_pose_right, *bundlerCal); + 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 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; @@ -116,8 +115,8 @@ TEST( triangulation, fourPoses) { vector poses; vector measurements; - poses += level_pose, level_pose_right; - measurements += level_uv, level_uv_right; + poses += pose1, pose2; + measurements += z1, z2; boost::optional triangulated_landmark = triangulatePoint3(poses, sharedCal, measurements); @@ -127,21 +126,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 @@ -150,14 +148,13 @@ 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); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); - poses += level_pose180; + poses += pose4; measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), @@ -170,24 +167,24 @@ TEST( triangulation, fourPoses) { 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) - SimpleCamera level_camera(level_pose, K1); + SimpleCamera camera1(pose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - SimpleCamera level_camera_right(level_pose_right, K2); + 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) @@ -199,17 +196,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 @@ -218,15 +214,14 @@ 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); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); - cameras += camera_180; + cameras += camera4; measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), TriangulationCheiralityException); @@ -237,16 +232,16 @@ TEST( triangulation, fourPoses_distinct_Ks) { TEST( triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera level_camera(level_pose, *sharedCal); + 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 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); From f3ee25f1a8824c503739661e291729e751de979a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 14:56:50 -0500 Subject: [PATCH 6/9] TriangulationFactor, first version --- .../geometry/tests/testTriangulation.cpp | 227 +++++++++++++++++- 1 file changed, 215 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 5f39b694b..ed4bb962a 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,6 +16,189 @@ * Author: cbeall3 */ +#include +#include +#include +#include + +namespace gtsam { + +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration and pose are assumed known. + * i.e. the main building block for visual SLAM. + * TODO: refactor to avoid large copy/paste + * TODO: even better, make GTSAM designate certain variables as constant + * @addtogroup SLAM + */ +template +class TriangulationFactor: public NoiseModelFactor1 { +protected: + + // Keep a copy of measurement and calibration for I/O + const Pose3 pose_; ///< Pose where this landmark was seen + const Point2 measured_; ///< 2D measurement + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + // 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 + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + TriangulationFactor(const Pose3& pose, const Point2& measured, + const SharedNoiseModel& model, Key pointKey, + const boost::shared_ptr& K, + boost::optional body_P_sensor = boost::none) : + Base(model, pointKey), pose_(pose), measured_(measured), K_(K), body_P_sensor_( + body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { + } + + /** + * Constructor with exception-handling flags + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + TriangulationFactor(const Pose3& pose, const Point2& measured, + const SharedNoiseModel& model, Key poseKey, Key pointKey, + const boost::shared_ptr& K, bool throwCheirality, + bool verboseCheirality, boost::optional body_P_sensor = boost::none) : + Base(model, pointKey), pose_(pose), measured_(measured), K_(K), body_P_sensor_( + body_P_sensor), 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, z = "; + measured_.print(); + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + 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->measured_.equals(e->measured_, tol) + && this->K_->equals(*e->K_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) + || (body_P_sensor_ && e->body_P_sensor_ + && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Point3& point, boost::optional H2 = + boost::none) const { + try { + if (body_P_sensor_) { + PinholeCamera camera(pose_.compose(*body_P_sensor_), *K_); + Point2 reprojectionError( + camera.project(point, boost::none, H2) - measured_); + return reprojectionError.vector(); + } else { + PinholeCamera camera(pose_, *K_); + Point2 reprojectionError( + camera.project(point, boost::none, H2) - measured_); + return reprojectionError.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 * K_->fx(); + } + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** 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(measured_); + ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; +} // \ namespace gtsam + #include #include #include @@ -48,8 +231,7 @@ static const Point3 landmark(5, 0.5, 1.2); Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); -/* ************************************************************************* */ - +//****************************************************************************** TEST( triangulation, twoPoses) { vector poses; @@ -74,7 +256,7 @@ TEST( triangulation, twoPoses) { EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( triangulation, twoPosesBundler) { @@ -109,8 +291,7 @@ TEST( triangulation, twoPosesBundler) { EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); } -/* ************************************************************************* */ - +//****************************************************************************** TEST( triangulation, fourPoses) { vector poses; vector measurements; @@ -162,8 +343,7 @@ TEST( triangulation, fourPoses) { #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) @@ -228,8 +408,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { #endif } -/* ************************************************************************* */ - +//****************************************************************************** TEST( triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) SimpleCamera camera1(pose1, *sharedCal); @@ -247,7 +426,7 @@ TEST( triangulation, twoIdenticalPoses) { TriangulationUnderconstrainedException); } -/* ************************************************************************* */ +//****************************************************************************** /* TEST( triangulation, onePose) { // we expect this test to fail with a TriangulationUnderconstrainedException @@ -265,9 +444,33 @@ TEST( triangulation, twoIdenticalPoses) { 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(pose1, 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); } -/* ************************************************************************* */ +//****************************************************************************** From a7f98a8316843d06e12189babc151a479f8d95b0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 15:33:56 -0500 Subject: [PATCH 7/9] Drastically simplified by passing cameras --- .../geometry/tests/testTriangulation.cpp | 95 ++++++------------- 1 file changed, 28 insertions(+), 67 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index ed4bb962a..ce8a3a5bd 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -31,15 +31,19 @@ namespace gtsam { * TODO: even better, make GTSAM designate certain variables as constant * @addtogroup SLAM */ -template -class TriangulationFactor: public NoiseModelFactor1 { +template +class TriangulationFactor: public NoiseModelFactor1 { + +public: + + /// Camera type + typedef PinholeCamera Camera; + protected: // Keep a copy of measurement and calibration for I/O - const Pose3 pose_; ///< Pose where this landmark was seen + const Camera camera_; ///< Camera in which this landmark was seen const Point2 measured_; ///< 2D measurement - boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) @@ -48,10 +52,10 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; /// shorthand for this class - typedef TriangulationFactor This; + typedef TriangulationFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -61,43 +65,20 @@ public: throwCheirality_(false), verboseCheirality_(false) { } - /** - * Constructor - * TODO: Mark argument order standard (keys, measurement, parameters) - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation - * @param poseKey is the index of the camera - * @param pointKey is the index of the landmark - * @param K shared pointer to the constant calibration - * @param body_P_sensor is the transform from body to sensor frame (default identity) - */ - TriangulationFactor(const Pose3& pose, const Point2& measured, - const SharedNoiseModel& model, Key pointKey, - const boost::shared_ptr& K, - boost::optional body_P_sensor = boost::none) : - Base(model, pointKey), pose_(pose), measured_(measured), K_(K), body_P_sensor_( - body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { - } - /** * Constructor with exception-handling flags - * TODO: Mark argument order standard (keys, measurement, parameters) + * @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 poseKey is the index of the camera * @param pointKey is the index of the landmark - * @param K shared pointer to the constant calibration * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - TriangulationFactor(const Pose3& pose, const Point2& measured, - const SharedNoiseModel& model, Key poseKey, Key pointKey, - const boost::shared_ptr& K, bool throwCheirality, - bool verboseCheirality, boost::optional body_P_sensor = boost::none) : - Base(model, pointKey), pose_(pose), measured_(measured), K_(K), body_P_sensor_( - body_P_sensor), throwCheirality_(throwCheirality), verboseCheirality_( - verboseCheirality) { + 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 */ @@ -117,39 +98,25 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "TriangulationFactor, z = "; - measured_.print(); - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); + 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->measured_.equals(e->measured_, tol) - && this->K_->equals(*e->K_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + 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 { - if (body_P_sensor_) { - PinholeCamera camera(pose_.compose(*body_P_sensor_), *K_); - Point2 reprojectionError( - camera.project(point, boost::none, H2) - measured_); - return reprojectionError.vector(); - } else { - PinholeCamera camera(pose_, *K_); - Point2 reprojectionError( - camera.project(point, boost::none, H2) - measured_); - return reprojectionError.vector(); - } + Point2 error(camera_.project(point, boost::none, H2) - measured_); + return error.vector(); } catch (CheiralityException& e) { if (H2) *H2 = zeros(2, 3); @@ -159,8 +126,8 @@ public: << std::endl; if (throwCheirality_) throw e; + return ones(2) * 2.0 * camera_.calibration().fx(); } - return ones(2) * 2.0 * K_->fx(); } /** return the measurement */ @@ -168,11 +135,6 @@ public: return measured_; } - /** return the calibration object */ - inline const boost::shared_ptr calibration() const { - return K_; - } - /** return verbosity */ inline bool verboseCheirality() const { return verboseCheirality_; @@ -190,9 +152,8 @@ private: 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(K_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); ar & BOOST_SERIALIZATION_NVP(throwCheirality_); ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } @@ -450,8 +411,8 @@ 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(pose1, z1, model, pointKey, sharedCal); + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey, sharedCal); // Use the factor to calculate the Jacobians Matrix HActual; From 5c466a79142486366a303282acee488c9751f804 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 15:36:29 -0500 Subject: [PATCH 8/9] Moved to header file --- gtsam_unstable/geometry/TriangulationFactor.h | 159 ++++++++++++++++++ .../geometry/tests/testTriangulation.cpp | 145 +--------------- 2 files changed, 160 insertions(+), 144 deletions(-) create mode 100644 gtsam_unstable/geometry/TriangulationFactor.h 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 ce8a3a5bd..f2c957835 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,150 +16,7 @@ * Author: cbeall3 */ -#include -#include -#include -#include - -namespace gtsam { - -/** - * Non-linear factor for a constraint derived from a 2D measurement. - * The calibration and pose are assumed known. - * i.e. the main building block for visual SLAM. - * TODO: refactor to avoid large copy/paste - * TODO: even better, make GTSAM designate certain variables as constant - * @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 - +#include #include #include #include From b1013163e76e40894266ca3445d589bba328be4f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 15:51:02 -0500 Subject: [PATCH 9/9] Switched to TriangulationFactors: huge improvement --- .../geometry/tests/testTriangulation.cpp | 1 - gtsam_unstable/geometry/triangulation.h | 29 +++++++------------ 2 files changed, 10 insertions(+), 20 deletions(-) diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index f2c957835..8d45311f1 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -16,7 +16,6 @@ * Author: cbeall3 */ -#include #include #include #include diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 49998d117..f767514c1 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -18,10 +18,10 @@ #pragma once -#include +#include +#include #include #include -#include #include #include @@ -56,9 +56,6 @@ 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 - /// /** * Create a factor graph with projection factors from poses and one calibration @@ -81,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); } @@ -110,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); } @@ -251,9 +242,9 @@ Point3 triangulatePoint3( typedef PinholeCamera Camera; std::vector projection_matrices; BOOST_FOREACH(const Camera& camera, cameras) - projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); + projection_matrices.push_back( + camera.calibration().K() + * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);