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.
Frank Dellaert
parent
b123c5dda2
commit
a28e04988d
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <gtsam/base/Testable.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/triangulation.h>
|
||||
|
|
@ -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<Cal3Bundler> 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<Cal3Bundler> 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<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));
|
||||
|
||||
|
||||
// 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<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));
|
||||
|
||||
|
||||
// 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<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, K);
|
||||
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses,
|
||||
measurements, K);
|
||||
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
|
||||
|
||||
// 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));
|
||||
|
||||
// 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<Cal3_S2>(K1));
|
||||
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));
|
||||
|
||||
|
||||
// 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<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));
|
||||
|
||||
|
||||
// 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<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));
|
||||
|
||||
// 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));
|
||||
|
||||
// 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<Cal3_S2>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@
|
|||
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Exception thrown by triangulateDLT when SVD returns rank < 3
|
||||
|
|
@ -73,16 +72,20 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses,
|
|||
const std::vector<boost::shared_ptr<CALIBRATION> >& 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<Pose3>& 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<Pose3,Point3,CALIBRATION> ProjectionFactor;
|
||||
typedef PriorFactor<Pose3> Pose3Prior;
|
||||
Key poseKey = Symbol('x',ij);
|
||||
boost::shared_ptr<ProjectionFactor> projectionFactor(new ProjectionFactor(measurement, noise, poseKey, landmarkKey, Ks[ij]));
|
||||
typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> 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<Pose3> 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<Pose3>& 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<Pose3>& poses,
|
|||
|
||||
assert(poses.size() == measurements.size());
|
||||
|
||||
if(poses.size() < 2)
|
||||
if (poses.size() < 2)
|
||||
throw(TriangulationUnderconstrainedException());
|
||||
|
||||
std::vector<Matrix> 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<Pose3>& poses,
|
|||
boost::shared_ptr<CALIBRATION> sharedK = boost::make_shared<CALIBRATION>(K);
|
||||
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
|
||||
BOOST_FOREACH(const Pose3& pose, poses) {
|
||||
const Point3& p_local = pose.transform_to(triangulated_point);
|
||||
if(p_local.z() <= 0)
|
||||
throw(TriangulationCheiralityException());
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
return triangulated_point;
|
||||
}
|
||||
|
|
@ -209,37 +215,37 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
template<class CALIBRATION>
|
||||
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
||||
const std::vector<Point2>& measurements,
|
||||
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks,
|
||||
double rank_tol = 1e-9, bool optimize = false) {
|
||||
const std::vector<boost::shared_ptr<CALIBRATION> >& 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<Matrix> projection_matrices;
|
||||
|
||||
// construct projection matrices from poses & calibration
|
||||
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) );
|
||||
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
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue