82 lines
2.4 KiB
C++
82 lines
2.4 KiB
C++
|
/* ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
* 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 <gtsam_unstable/geometry/triangulation.h>
|
||
|
|
#include <gtsam/geometry/SimpleCamera.h>
|
||
|
|
#include <boost/foreach.hpp>
|
||
|
|
#include <boost/assign.hpp>
|
||
|
|
#include <boost/assign/std/vector.hpp>
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
using namespace boost::assign;
|
||
|
|
|
||
|
|
namespace gtsam {
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
// See Hartley and Zisserman, 2nd Ed., page 312
|
||
|
|
Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
|
||
|
|
const vector<Point2>& 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<Point3> triangulatePoint3(const vector<Pose3>& poses,
|
||
|
|
const vector<Point2>& measurements, const Cal3_S2& K) {
|
||
|
|
|
||
|
|
assert(poses.size() == measurements.size());
|
||
|
|
|
||
|
|
if(poses.size() < 2)
|
||
|
|
return boost::none;
|
||
|
|
|
||
|
|
vector<Matrix> 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
|