diff --git a/gtsam_unstable/geometry/tests/testTriangulation.cpp b/gtsam_unstable/geometry/tests/testTriangulation.cpp new file mode 100644 index 000000000..3f910e2a4 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testTriangulation.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulation.cpp + * + * Created on: July 30th, 2013 + * Author: cbeall3 + */ + +#include + +#include +#include + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST( triangulation, twoPoses) { + 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)); + 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); + EXPECT(assert_equal(landmark, *triangulated_3cameras, 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); + + CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); + + poses += level_pose180; + measurements += Point2(400,400); + + boost::optional triangulated_4cameras = triangulatePoint3(poses, measurements, K); + EXPECT(boost::none == triangulated_4cameras); + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp new file mode 100644 index 000000000..28270afdf --- /dev/null +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -0,0 +1,81 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file triangulation.cpp + * @brief Functions for triangulation + * @author Chris Beall + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; + +namespace gtsam { + +/* ************************************************************************* */ +// See Hartley and Zisserman, 2nd Ed., page 312 +Point3 triangulateDLT(const vector& projection_matrices, + const vector& measurements) { + + Matrix A = Matrix_(projection_matrices.size() *2, 4); + + for(size_t i=0; i< projection_matrices.size(); 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); + } + int rank; + double error; + Vector v; + boost::tie(rank, error, v) = DLT(A); + return Point3(sub( (v / v(3)),0,3)); +} + +/* ************************************************************************* */ +boost::optional triangulatePoint3(const vector& poses, + const vector& measurements, const Cal3_S2& K) { + + assert(poses.size() == measurements.size()); + + if(poses.size() < 2) + return boost::none; + + vector projection_matrices; + + // construct projection matrices from poses & calibration + BOOST_FOREACH(const Pose3& pose, poses) + projection_matrices += K.matrix() * sub(pose.inverse().matrix(),0,3,0,4); + + Point3 triangulated_point = triangulateDLT(projection_matrices, measurements); + + // 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) + return boost::none; + } + + return triangulated_point; +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h new file mode 100644 index 000000000..240e5ace0 --- /dev/null +++ b/gtsam_unstable/geometry/triangulation.h @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file triangulation.h + * @brief Functions for triangulation + * @date July 31, 2013 + * @author Chris Beall + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Function to triangulate 3D landmark point from an arbitrary number + * of poses (at least 2) using the DLT. 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 measurements A vector of camera measurements + * @param K The camera calibration + * @return Returns a Point3 on success, boost::none otherwise. + */ +boost::optional triangulatePoint3(const std::vector& poses, + const std::vector& measurements, const Cal3_S2& K); + + +} // \namespace gtsam + +