12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Some documentation, and extra optimize test. In debug mode the unit test fails because it does not throw the right exception, but everything else seems fine.

release/4.3a0
Frank Dellaert 2013-11-01 01:04:34 +00:00
parent b123c5dda2
commit a28e04988d
2 changed files with 149 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
gtsam_unstable/geometry/tests/testTriangulation.cpp
View File

@ -20,6 +20,8 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h> #include <gtsam_unstable/geometry/InvDepthCamera3.h>
#include <gtsam_unstable/geometry/triangulation.h> #include <gtsam_unstable/geometry/triangulation.h>
@ -32,20 +34,20 @@ using namespace gtsam;
using namespace boost::assign; using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( triangulation, fourPoses) { TEST( triangulation, twoPosesBundler) {
Cal3_S2 K(1500, 1200, 0, 640, 480); Cal3Bundler K(1500, 0, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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),
SimpleCamera level_camera(level_pose, K); gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3Bundler> level_camera(level_pose, K);
// create second camera 1 meter to the right of first camera // 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));
SimpleCamera level_camera_right(level_pose_right, K); PinholeCamera<Cal3Bundler> level_camera_right(level_pose_right, K);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2); Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark);
@ -56,61 +58,108 @@ TEST( triangulation, fourPoses) {
poses += level_pose, level_pose_right; poses += level_pose, level_pose_right;
measurements += level_uv, level_uv_right; measurements += level_uv, level_uv_right;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, measurements, K); bool optimize = true;
double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
measurements, K, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 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); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, measurements, K); boost::optional<Point3> 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<Pose3> poses;
vector<Point2> measurements;
poses += level_pose, level_pose_right;
measurements += level_uv, level_uv_right;
boost::optional<Point3> 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<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
measurements, K);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 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); SimpleCamera camera_top(pose_top, K);
Point2 top_uv = camera_top.project(landmark); Point2 top_uv = camera_top.project(landmark);
poses += pose_top; poses += pose_top;
measurements += top_uv + Point2(0.1, -0.1); measurements += top_uv + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, K); boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses,
measurements, K);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, measurements, K, 1e-9, true); boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
measurements, K, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 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); SimpleCamera camera_180(level_pose180, K);
CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); CHECK_EXCEPTION(camera_180.project(landmark)
;, CheiralityException);
poses += level_pose180; 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) { TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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); SimpleCamera level_camera(level_pose, K1);
// create second camera 1 meter to the right of first camera // 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); Cal3_S2 K2(1600, 1300, 0, 650, 440);
SimpleCamera level_camera_right(level_pose_right, K2); SimpleCamera level_camera_right(level_pose_right, K2);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2); Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.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<Cal3_S2>(K1)); Ks.push_back(boost::make_shared<Cal3_S2>(K1));
Ks.push_back(boost::make_shared<Cal3_S2>(K2)); Ks.push_back(boost::make_shared<Cal3_S2>(K2));
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, measurements, Ks); boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
measurements, Ks);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 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); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, measurements, Ks); boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
measurements, Ks);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 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); Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera_top(pose_top, K3); SimpleCamera camera_top(pose_top, K3);
Point2 top_uv = camera_top.project(landmark); Point2 top_uv = camera_top.project(landmark);
@ -147,32 +197,38 @@ TEST( triangulation, fourPoses_distinct_Ks) {
measurements += top_uv + Point2(0.1, -0.1); measurements += top_uv + Point2(0.1, -0.1);
Ks.push_back(boost::make_shared<Cal3_S2>(K3)); Ks.push_back(boost::make_shared<Cal3_S2>(K3));
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, Ks); boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses,
measurements, Ks);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, measurements, Ks, 1e-9, true); boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
measurements, Ks, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 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); Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera_180(level_pose180, K4); SimpleCamera camera_180(level_pose180, K4);
CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); CHECK_EXCEPTION(camera_180.project(landmark)
;, CheiralityException);
poses += level_pose180; poses += level_pose180;
measurements += Point2(400,400); measurements += Point2(400, 400);
Ks.push_back(boost::make_shared<Cal3_S2>(K4)); Ks.push_back(boost::make_shared<Cal3_S2>(K4));
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks), TriangulationCheiralityException); CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks),
TriangulationCheiralityException);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( triangulation, twoIdenticalPoses) { TEST( triangulation, twoIdenticalPoses) {
Cal3_S2 K(1500, 1200, 0, 640, 480); Cal3_S2 K(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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); SimpleCamera level_camera(level_pose, K);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
@ -187,7 +243,8 @@ TEST( triangulation, twoIdenticalPoses) {
poses += level_pose, level_pose; poses += level_pose, level_pose;
measurements += level_uv, level_uv; 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(); poses += Pose3();
measurements += Point2(); 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);
}
/* ************************************************************************* */ /* ************************************************************************* */

94
gtsam_unstable/geometry/triangulation.h
View File

@ -34,14 +34,13 @@
#include <gtsam/slam/ProjectionFactor.h> #include <gtsam/slam/ProjectionFactor.h>
namespace gtsam { namespace gtsam {
/// Exception thrown by triangulateDLT when SVD returns rank < 3 /// Exception thrown by triangulateDLT when SVD returns rank < 3
class GTSAM_UNSTABLE_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { class GTSAM_UNSTABLE_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
public: public:
TriangulationUnderconstrainedException() : 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 { class GTSAM_UNSTABLE_EXPORT TriangulationCheiralityException: public std::runtime_error {
public: public:
TriangulationCheiralityException() : TriangulationCheiralityException() :
std::runtime_error( std::runtime_error(
"Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
} }
}; };
@ -73,16 +72,20 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses,
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol, const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
bool optimize) { 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++) { // Allocate DLT matrix
size_t row = i*2; 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 Matrix& projection = projection_matrices.at(i);
const Point2& p = measurements.at(i); const Point2& p = measurements.at(i);
// build system of equations // build system of equations
A.row(row) = p.x() * projection.row(2) - projection.row(0); 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; int rank;
double error; double error;
@ -90,39 +93,41 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses,
boost::tie(rank, error, v) = DLT(A, rank_tol); boost::tie(rank, error, v) = DLT(A, rank_tol);
// std::cout << "s " << s.transpose() << std:endl; // std::cout << "s " << s.transpose() << std:endl;
if(rank < 3) if (rank < 3)
throw(TriangulationUnderconstrainedException()); 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) { if (optimize) {
// Create a factor graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
gtsam::Values values; gtsam::Values values;
static SharedNoiseModel noise(noiseModel::Unit::Create(2)); static SharedNoiseModel unit2(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))); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
int ij = 0;
Key landmarkKey = Symbol('l',0);
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 // Factor for pose i
typedef GenericProjectionFactor<Pose3,Point3,CALIBRATION> ProjectionFactor; typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> ProjectionFactor;
typedef PriorFactor<Pose3> Pose3Prior; ProjectionFactor projectionFactor(z_i, unit2, i, landmarkKey, Ks[i]);
Key poseKey = Symbol('x',ij);
boost::shared_ptr<ProjectionFactor> projectionFactor(new ProjectionFactor(measurement, noise, poseKey, landmarkKey, Ks[ij]));
graph.push_back(projectionFactor); graph.push_back(projectionFactor);
// Prior on pose // 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<Pose3> Pose3Prior;
graph.push_back(Pose3Prior(i, poses[i], prior_model));
// Initial pose values // Initial pose values
values.insert( poseKey, poses[ij]); values.insert(i, poses[i]);
i++;
ij++;
} }
// Initial landmark value
values.insert(landmarkKey, point);
// Optimize // Optimize
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@ -143,7 +148,6 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses,
return point; return point;
} }
/** /**
* Function to triangulate 3D landmark point from an arbitrary number * Function to triangulate 3D landmark point from an arbitrary number
* of poses (at least 2) using the DLT. The function checks that the * of poses (at least 2) using the DLT. The function checks that the
@ -163,14 +167,15 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
if(poses.size() < 2) if (poses.size() < 2)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
std::vector<Matrix> projection_matrices; std::vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
BOOST_FOREACH(const Pose3& pose, poses){ BOOST_FOREACH(const Pose3& pose, poses) {
projection_matrices.push_back( K.K() * sub(pose.inverse().matrix(),0,3,0,4) ); 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 << "Calibration i \n" << K.K() << std::endl;
// std::cout << "rank_tol i \n" << rank_tol << std::endl; // std::cout << "rank_tol i \n" << rank_tol << std::endl;
} }
@ -179,16 +184,17 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedK = boost::make_shared<CALIBRATION>(K); boost::shared_ptr<CALIBRATION> sharedK = boost::make_shared<CALIBRATION>(K);
std::vector<boost::shared_ptr<CALIBRATION> > Ks(poses.size(), sharedK); std::vector<boost::shared_ptr<CALIBRATION> > 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 // verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) { BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(triangulated_point); const Point3& p_local = pose.transform_to(triangulated_point);
if(p_local.z() <= 0) if(p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
} }
#endif #endif
return triangulated_point; return triangulated_point;
} }
@ -209,37 +215,37 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, const std::vector<Point2>& measurements,
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol =
double rank_tol = 1e-9, bool optimize = false) { 1e-9, bool optimize = false) {
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
assert(poses.size() == Ks.size()); assert(poses.size() == Ks.size());
if(poses.size() < 2) if (poses.size() < 2)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
std::vector<Matrix> projection_matrices; std::vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
for(size_t i = 0; i<poses.size(); i++){ 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) ); 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 << "2Calibration i \n" << Ks.at(i)->K() << std::endl;
// std::cout << "2rank_tol i \n" << rank_tol << 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 // verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) { BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(triangulated_point); const Point3& p_local = pose.transform_to(triangulated_point);
if(p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
} }
return triangulated_point; return triangulated_point;
} }
} // \namespace gtsam } // \namespace gtsam