diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp index 9bac653df..3061749b3 100644 --- a/gtsam_unstable/geometry/tests/testTriangulation.cpp +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -28,22 +28,25 @@ #include #include +#include using namespace std; using namespace gtsam; using namespace boost::assign; /* ************************************************************************* */ + TEST( triangulation, twoPosesBundler) { - Cal3Bundler K(1500, 0, 0, 640, 480); + 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, K); + PinholeCamera 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)); - PinholeCamera level_camera_right(level_pose_right, K); + PinholeCamera level_camera_right(level_pose_right, *sharedCal); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); @@ -52,7 +55,7 @@ TEST( triangulation, twoPosesBundler) { Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); - vector poses; + vector < Pose3 > poses; vector measurements; poses += level_pose, level_pose_right; @@ -62,7 +65,7 @@ TEST( triangulation, twoPosesBundler) { double rank_tol = 1e-9; boost::optional triangulated_landmark = triangulatePoint3(poses, - measurements, K, rank_tol, optimize); + 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) @@ -70,21 +73,23 @@ TEST( triangulation, twoPosesBundler) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - measurements, K, rank_tol, optimize); + sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); } /* ************************************************************************* */ + TEST( triangulation, fourPoses) { - Cal3_S2 K(1500, 1200, 0, 640, 480); + 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, K); + 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, K); + SimpleCamera level_camera_right(level_pose_right, *sharedCal); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); @@ -93,14 +98,14 @@ TEST( triangulation, fourPoses) { Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); - vector poses; + vector < Pose3 > poses; vector measurements; poses += level_pose, level_pose_right; measurements += level_uv, level_uv_right; boost::optional triangulated_landmark = triangulatePoint3(poses, - measurements, K); + sharedCal, 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) @@ -108,43 +113,43 @@ TEST( triangulation, fourPoses) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional triangulated_landmark_noise = triangulatePoint3(poses, - measurements, K); + 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, K); + SimpleCamera camera_top(pose_top, *sharedCal); 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); + sharedCal, measurements); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, - measurements, K, 1e-9, true); + sharedCal, measurements, 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)); - SimpleCamera camera_180(level_pose180, K); + SimpleCamera camera_180(level_pose180, *sharedCal); - CHECK_EXCEPTION(camera_180.project(landmark) - ;, CheiralityException); + CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException); poses += level_pose180; measurements += Point2(400, 400); - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), + CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), 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) @@ -164,26 +169,22 @@ TEST( triangulation, fourPoses_distinct_Ks) { Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); - vector poses; + vector cameras; vector measurements; - poses += level_pose, level_pose_right; + cameras += level_camera, level_camera_right; measurements += level_uv, level_uv_right; - std::vector > 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(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) 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(cameras, measurements); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise @@ -193,17 +194,16 @@ TEST( triangulation, fourPoses_distinct_Ks) { SimpleCamera camera_top(pose_top, K3); Point2 top_uv = camera_top.project(landmark); - poses += pose_top; + cameras += camera_top; 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(cameras, + measurements); 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(cameras, + measurements, 1e-9, true); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way @@ -212,24 +212,23 @@ TEST( triangulation, fourPoses_distinct_Ks) { 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; + cameras += camera_180; measurements += Point2(400, 400); - Ks.push_back(boost::make_shared(K4)); - - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks), + CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), TriangulationCheiralityException); } /* ************************************************************************* */ + TEST( triangulation, twoIdenticalPoses) { - Cal3_S2 K(1500, 1200, 0, 640, 480); + 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, K); + SimpleCamera level_camera(level_pose, *sharedCal); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); @@ -237,34 +236,35 @@ TEST( triangulation, twoIdenticalPoses) { // 1. Project two landmarks into two cameras and triangulate Point2 level_uv = level_camera.project(landmark); - vector poses; + vector < Pose3 > poses; vector measurements; poses += level_pose, level_pose; measurements += level_uv, level_uv; - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), + 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 +/* ************************************************************************* * - Cal3_S2 K(1500, 1200, 0, 640, 480); + TEST( triangulation, onePose) { + // we expect this test to fail with a TriangulationUnderconstrainedException + // because there's only one camera observation - vector poses; - vector measurements; + Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480); - poses += Pose3(); - measurements += Point2(); + vector poses; + vector measurements; - CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), - TriangulationUnderconstrainedException); -} + poses += Pose3(); + measurements += Point2(); -/* ************************************************************************* */ + CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), + TriangulationUnderconstrainedException); + } + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index afbf03ffd..96bf2ddf9 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -18,21 +18,21 @@ #pragma once -#include #include #include -#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include -#include -#include -#include -#include -#include - -#include +#include namespace gtsam { @@ -53,24 +53,15 @@ public: } }; -/* ************************************************************************* */ -// See Hartley and Zisserman, 2nd Ed., page 312 /** - * - * @param poses Camera poses + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements - * @param Ks vector of calibrations * @param rank_tol SVD rank tolerance - * @param Flag to turn on nonlinear refinement of triangulation * @return Triangulated Point3 */ -template -Point3 triangulateDLT(const std::vector& poses, - const std::vector& projection_matrices, - const std::vector& measurements, - const std::vector >& Ks, double rank_tol, - bool optimize) { +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -97,55 +88,143 @@ Point3 triangulateDLT(const std::vector& poses, throw(TriangulationUnderconstrainedException()); // Create 3D point from eigenvector - Point3 point = Point3(sub((v / v(3)), 0, 3)); + return Point3(sub((v / v(3)), 0, 3)); +} - if (optimize) { - // Create a factor graph - NonlinearFactorGraph graph; - gtsam::Values values; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); +// 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 - // 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; - ProjectionFactor projectionFactor(z_i, unit2, i, landmarkKey, Ks[i]); - graph.push_back(projectionFactor); - - // Prior on pose - // 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(i, poses[i]); - i++; - } - - // Optimize - LevenbergMarquardtParams params; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - params.lambdaInitial = 1; - params.lambdaFactor = 10; - params.maxIterations = 100; - params.absoluteErrorTol = 1.0; - params.verbosityLM = LevenbergMarquardtParams::SILENT; - params.verbosity = NonlinearOptimizerParams::SILENT; - params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - Values result = optimizer.optimize(); - - point = result.at(landmarkKey); +/// +/** + * Create a factor graph with projection factors from poses and one calibration + * @param poses Camera poses + * @param sharedCal shared pointer to single calibration object + * @param measurements 2D measurements + * @param landmarkKey to refer to landmark + * @param initialEstimate + * @return graph and initial values + */ +template +std::pair triangulationGraph( + const std::vector& poses, boost::shared_ptr sharedCal, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + 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); } + return std::make_pair(graph, values); +} - return point; +/** + * Create a factor graph with projection factors from pinhole cameras + * (each camera has a pose and calibration) + * @param cameras pinhole cameras + * @param measurements 2D measurements + * @param landmarkKey to refer to landmark + * @param initialEstimate + * @return graph and initial values + */ +template +std::pair triangulationGraph( + const std::vector >& cameras, + const std::vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + 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); + } + return std::make_pair(graph, values); +} + +/// +/** + * Optimize for triangulation + * @param graph nonlinear factors for projection + * @param values initial values + * @param landmarkKey to refer to landmark + * @return refined Point3 + */ +Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, + Key landmarkKey) { + // Maybe we should consider Gauss-Newton? + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + params.lambdaInitial = 1; + params.lambdaFactor = 10; + params.maxIterations = 100; + params.absoluteErrorTol = 1.0; + params.verbosityLM = LevenbergMarquardtParams::SILENT; + params.verbosity = NonlinearOptimizerParams::SILENT; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; + + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values result = optimizer.optimize(); + + return result.at(landmarkKey); +} + +/** + * Given an initial estimate , refine a point using measurements in several cameras + * @param poses Camera poses + * @param sharedCal shared pointer to single calibration object + * @param measurements 2D measurements + * @param initialEstimate + * @return refined Point3 + */ +template +Point3 triangulateNonlinear(const std::vector& poses, + boost::shared_ptr sharedCal, + const std::vector& measurements, const Point3& initialEstimate) { + + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, + Symbol('p', 0), initialEstimate); + + return optimize(graph, values, Symbol('p', 0)); +} + +/** + * Given an initial estimate , refine a point using measurements in several cameras + * @param cameras pinhole cameras + * @param measurements 2D measurements + * @param initialEstimate + * @return refined Point3 + */ +template +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& measurements, const Point3& initialEstimate) { + + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph(cameras, measurements, + Symbol('p', 0), initialEstimate); + + return optimize(graph, values, Symbol('p', 0)); } /** @@ -154,49 +233,46 @@ Point3 triangulateDLT(const std::vector& poses, * resulting point lies in front of all cameras, but has no other checks * to verify the quality of the triangulation. * @param poses A vector of camera poses + * @param sharedCal shared pointer to single calibration object * @param measurements A vector of camera measurements - * @param K The camera calibration (Same for all cameras involved) * @param rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @return Returns a Point3 on success, boost::none otherwise. + * @return Returns a Point3 */ template Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const CALIBRATION& K, - double rank_tol = 1e-9, bool optimize = false) { + boost::shared_ptr sharedCal, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { assert(poses.size() == measurements.size()); - if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); - std::vector projection_matrices; - // construct projection matrices from poses & calibration + std::vector projection_matrices; 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; + sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); } - // create vector with shared pointer to calibration (all the same in this case) - boost::shared_ptr sharedK = boost::make_shared(K); - std::vector > Ks(poses.size(), sharedK); + // Triangulate linearly + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - Point3 triangulated_point = triangulateDLT(poses, projection_matrices, - measurements, Ks, rank_tol, optimize); + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(poses, sharedCal, measurements, point); #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()); + const Point3& p_local = pose.transform_to(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); } #endif - return triangulated_point; + return point; } /** @@ -205,46 +281,48 @@ Point3 triangulatePoint3(const std::vector& poses, * above, except that each camera has its own calibration. The function * checks that the resulting point lies in front of all cameras, but has * no other checks to verify the quality of the triangulation. - * @param poses A vector of camera poses + * @param cameras pinhole cameras * @param measurements A vector of camera measurements - * @param Ks Vector of camera calibrations * @param rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @return Returns a Point3 on success, boost::none otherwise. + * @return Returns a Point3 */ template -Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, - const std::vector >& Ks, double rank_tol = - 1e-9, bool optimize = false) { +Point3 triangulatePoint3( + const std::vector >& cameras, + const std::vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { - assert(poses.size() == measurements.size()); - assert(poses.size() == Ks.size()); + size_t m = cameras.size(); + assert(measurements.size()==m); - if (poses.size() < 2) + if (m < 2) throw(TriangulationUnderconstrainedException()); - std::vector projection_matrices; - // construct projection matrices from poses & calibration - for (size_t i = 0; i < poses.size(); i++) { + typedef PinholeCamera Camera; + std::vector projection_matrices; + BOOST_FOREACH(const Camera& camera, cameras) 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; - } + camera.calibration().K() + * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); - Point3 triangulated_point = triangulateDLT(poses, projection_matrices, - measurements, Ks, rank_tol, optimize); + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(cameras, measurements, point); + +#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); + BOOST_FOREACH(const Camera& camera, cameras) { + const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } +#endif - return triangulated_point; + return point; } } // \namespace gtsam