diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index d9e6d9ae3..9bac653df 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include @@ -32,20 +34,20 @@ using namespace gtsam; using namespace boost::assign; /* ************************************************************************* */ -TEST( triangulation, fourPoses) { - Cal3_S2 K(1500, 1200, 0, 640, 480); +TEST( triangulation, twoPosesBundler) { + Cal3Bundler K(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)); - SimpleCamera level_camera(level_pose, K); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + gtsam::Point3(0, 0, 1)); + PinholeCamera level_camera(level_pose, K); // 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, K); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + PinholeCamera level_camera_right(level_pose_right, K); // 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); @@ -56,61 +58,108 @@ TEST( triangulation, fourPoses) { poses += level_pose, level_pose_right; measurements += level_uv, level_uv_right; - boost::optional triangulated_landmark = triangulatePoint3(poses, measurements, K); + bool optimize = true; + double rank_tol = 1e-9; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + measurements, K, 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(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, measurements, K); + boost::optional triangulated_landmark_noise = triangulatePoint3(poses, + measurements, K, rank_tol, optimize); + EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); +} + +/* ************************************************************************* */ +TEST( triangulation, fourPoses) { + Cal3_S2 K(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, K); + + // 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, K); + + // 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); + + vector poses; + vector measurements; + + poses += level_pose, level_pose_right; + measurements += level_uv, level_uv_right; + + boost::optional triangulated_landmark = triangulatePoint3(poses, + measurements, K); + 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, + measurements, K); 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 pose_top = level_pose + * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); SimpleCamera camera_top(pose_top, K); Point2 top_uv = camera_top.project(landmark); poses += pose_top; measurements += top_uv + Point2(0.1, -0.1); - boost::optional triangulated_3cameras = triangulatePoint3(poses, measurements, K); + boost::optional triangulated_3cameras = triangulatePoint3(poses, + measurements, K); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, measurements, K, 1e-9, true); + boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, + measurements, K, 1e-9, true); 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 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera camera_180(level_pose180, K); - CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera_180.project(landmark) + ;, CheiralityException); poses += level_pose180; - measurements += Point2(400,400); + measurements += Point2(400, 400); - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationCheiralityException); + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), + TriangulationCheiralityException); } /* ************************************************************************* */ 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)); + 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)); + 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); @@ -125,20 +174,21 @@ TEST( triangulation, fourPoses_distinct_Ks) { Ks.push_back(boost::make_shared(K1)); Ks.push_back(boost::make_shared(K2)); - boost::optional triangulated_landmark = triangulatePoint3(poses, measurements, Ks); + boost::optional triangulated_landmark = triangulatePoint3(poses, + measurements, Ks); 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(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional triangulated_landmark_noise = triangulatePoint3(poses, measurements, Ks); + boost::optional triangulated_landmark_noise = triangulatePoint3(poses, + measurements, 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 pose_top = level_pose + * 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); @@ -147,32 +197,38 @@ TEST( triangulation, fourPoses_distinct_Ks) { measurements += top_uv + Point2(0.1, -0.1); Ks.push_back(boost::make_shared(K3)); - boost::optional triangulated_3cameras = triangulatePoint3(poses, measurements, Ks); + boost::optional triangulated_3cameras = triangulatePoint3(poses, + measurements, Ks); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, measurements, Ks, 1e-9, true); + boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, + measurements, Ks, 1e-9, true); 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 level_pose180 = 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); - CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); + CHECK_EXCEPTION(camera_180.project(landmark) + ;, CheiralityException); poses += level_pose180; - measurements += Point2(400,400); + measurements += Point2(400, 400); Ks.push_back(boost::make_shared(K4)); - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks), TriangulationCheiralityException); + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks), + TriangulationCheiralityException); } /* ************************************************************************* */ TEST( triangulation, twoIdenticalPoses) { Cal3_S2 K(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)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + gtsam::Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, K); // landmark ~5 meters infront of camera @@ -187,7 +243,8 @@ TEST( triangulation, twoIdenticalPoses) { poses += level_pose, level_pose; measurements += level_uv, level_uv; - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationUnderconstrainedException); + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), + TriangulationUnderconstrainedException); } /* ************************************************************************* */ @@ -203,10 +260,13 @@ TEST( triangulation, onePose) { poses += Pose3(); measurements += Point2(); - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationUnderconstrainedException); + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), + TriangulationUnderconstrainedException); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index db3e5fc4d..22dbbc3d2 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -34,14 +34,13 @@ #include - namespace gtsam { /// Exception thrown by triangulateDLT when SVD returns rank < 3 class GTSAM_UNSTABLE_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { public: TriangulationUnderconstrainedException() : - std::runtime_error("Triangulation Underconstrained Exception.") { + std::runtime_error("Triangulation Underconstrained Exception.") { } }; @@ -49,8 +48,8 @@ public: class GTSAM_UNSTABLE_EXPORT TriangulationCheiralityException: public std::runtime_error { public: TriangulationCheiralityException() : - std::runtime_error( - "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { + std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { } }; @@ -73,16 +72,20 @@ Point3 triangulateDLT(const std::vector& poses, const std::vector >& Ks, double rank_tol, bool optimize) { - Matrix A = zeros(projection_matrices.size() *2, 4); + // number of cameras + size_t m = projection_matrices.size(); - for(size_t i=0; i< projection_matrices.size(); i++) { - size_t row = i*2; + // Allocate DLT matrix + Matrix A = zeros(m * 2, 4); + + for (size_t i = 0; i < m; i++) { + size_t row = i * 2; const Matrix& projection = projection_matrices.at(i); const Point2& p = measurements.at(i); // build system of equations A.row(row) = p.x() * projection.row(2) - projection.row(0); - A.row(row+1) = p.y() * projection.row(2) - projection.row(1); + A.row(row + 1) = p.y() * projection.row(2) - projection.row(1); } int rank; double error; @@ -90,39 +93,41 @@ Point3 triangulateDLT(const std::vector& poses, boost::tie(rank, error, v) = DLT(A, rank_tol); // std::cout << "s " << s.transpose() << std:endl; - if(rank < 3) + if (rank < 3) throw(TriangulationUnderconstrainedException()); - Point3 point = Point3(sub( (v / v(3)),0,3)); + // Create 3D point from eigenvector + Point3 point = Point3(sub((v / v(3)), 0, 3)); if (optimize) { + // Create a factor graph NonlinearFactorGraph graph; gtsam::Values values; - static SharedNoiseModel noise(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6))); - int ij = 0; - Key landmarkKey = Symbol('l',0); + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); - BOOST_FOREACH(const Point2 &measurement, measurements) { + // Initial landmark value + Key landmarkKey = Symbol('p', 0); + values.insert(landmarkKey, point); + + // Create all projection factors, as well as priors on poses + Key i = 0; + BOOST_FOREACH(const Point2 &z_i, measurements) { // Factor for pose i - typedef GenericProjectionFactor ProjectionFactor; - typedef PriorFactor Pose3Prior; - Key poseKey = Symbol('x',ij); - boost::shared_ptr projectionFactor(new ProjectionFactor(measurement, noise, poseKey, landmarkKey, Ks[ij])); + typedef GenericProjectionFactor ProjectionFactor; + ProjectionFactor projectionFactor(z_i, unit2, i, landmarkKey, Ks[i]); graph.push_back(projectionFactor); // Prior on pose - graph.push_back(Pose3Prior(poseKey, poses[ij], prior_model)); + // Frank says: this is a terrible idea: we turn a 3dof problem into a much more difficult problem + typedef PriorFactor Pose3Prior; + graph.push_back(Pose3Prior(i, poses[i], prior_model)); // Initial pose values - values.insert( poseKey, poses[ij]); - - ij++; + values.insert(i, poses[i]); + i++; } - // Initial landmark value - values.insert(landmarkKey, point); - // Optimize LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -143,7 +148,6 @@ Point3 triangulateDLT(const std::vector& poses, return point; } - /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -163,14 +167,15 @@ Point3 triangulatePoint3(const std::vector& poses, assert(poses.size() == measurements.size()); - if(poses.size() < 2) + if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); std::vector projection_matrices; // construct projection matrices from poses & calibration - BOOST_FOREACH(const Pose3& pose, poses){ - projection_matrices.push_back( K.K() * sub(pose.inverse().matrix(),0,3,0,4) ); + BOOST_FOREACH(const Pose3& pose, poses) { + projection_matrices.push_back( + K.K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); // std::cout << "Calibration i \n" << K.K() << std::endl; // std::cout << "rank_tol i \n" << rank_tol << std::endl; } @@ -179,16 +184,17 @@ Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedK = boost::make_shared(K); std::vector > Ks(poses.size(), sharedK); - Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, Ks, rank_tol, optimize); + Point3 triangulated_point = triangulateDLT(poses, projection_matrices, + measurements, Ks, rank_tol, optimize); - #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(triangulated_point); - if(p_local.z() <= 0) - throw(TriangulationCheiralityException()); + if(p_local.z() <= 0) + throw(TriangulationCheiralityException()); } - #endif +#endif return triangulated_point; } @@ -209,37 +215,37 @@ Point3 triangulatePoint3(const std::vector& poses, template Point3 triangulatePoint3(const std::vector& poses, const std::vector& measurements, - const std::vector >& Ks, - double rank_tol = 1e-9, bool optimize = false) { + const std::vector >& Ks, double rank_tol = + 1e-9, bool optimize = false) { assert(poses.size() == measurements.size()); assert(poses.size() == Ks.size()); - if(poses.size() < 2) + if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); std::vector projection_matrices; // construct projection matrices from poses & calibration - for(size_t i = 0; iK() * sub(poses.at(i).inverse().matrix(),0,3,0,4) ); + for (size_t i = 0; i < poses.size(); i++) { + projection_matrices.push_back( + Ks.at(i)->K() * sub(poses.at(i).inverse().matrix(), 0, 3, 0, 4)); // std::cout << "2Calibration i \n" << Ks.at(i)->K() << std::endl; // std::cout << "2rank_tol i \n" << rank_tol << std::endl; } - Point3 triangulated_point = triangulateDLT(poses, projection_matrices, measurements, Ks, rank_tol, optimize); + Point3 triangulated_point = triangulateDLT(poses, projection_matrices, + measurements, Ks, rank_tol, optimize); // verify that the triangulated point lies infront of all cameras BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(triangulated_point); - if(p_local.z() <= 0) + if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } return triangulated_point; } - } // \namespace gtsam -