From dfee108e53aba7942213ca1761c0efc018ab3ed4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Mar 2014 13:49:42 -0500 Subject: [PATCH] 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);