252 lines
8.5 KiB
C++
252 lines
8.5 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.h
|
|
* @brief Functions for triangulation
|
|
* @date July 31, 2013
|
|
* @author Chris Beall
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <vector>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/geometry/Cal3_S2.h>
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/assign.hpp>
|
|
#include <boost/assign/std/vector.hpp>
|
|
#include <gtsam_unstable/base/dllexport.h>
|
|
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
|
|
|
#include <gtsam/slam/ProjectionFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/// Exception thrown by triangulateDLT when SVD returns rank < 3
|
|
class TriangulationUnderconstrainedException: public std::runtime_error {
|
|
public:
|
|
TriangulationUnderconstrainedException() :
|
|
std::runtime_error("Triangulation Underconstrained Exception.") {
|
|
}
|
|
};
|
|
|
|
/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
|
|
class TriangulationCheiralityException: public std::runtime_error {
|
|
public:
|
|
TriangulationCheiralityException() :
|
|
std::runtime_error(
|
|
"Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
|
|
}
|
|
};
|
|
|
|
/* ************************************************************************* */
|
|
// See Hartley and Zisserman, 2nd Ed., page 312
|
|
/**
|
|
*
|
|
* @param poses Camera poses
|
|
* @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<class CALIBRATION>
|
|
Point3 triangulateDLT(const std::vector<Pose3>& poses,
|
|
const std::vector<Matrix>& projection_matrices,
|
|
const std::vector<Point2>& measurements,
|
|
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
|
|
bool optimize) {
|
|
|
|
// number of cameras
|
|
size_t m = projection_matrices.size();
|
|
|
|
// 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);
|
|
}
|
|
int rank;
|
|
double error;
|
|
Vector v;
|
|
boost::tie(rank, error, v) = DLT(A, rank_tol);
|
|
// std::cout << "s " << s.transpose() << std:endl;
|
|
|
|
if (rank < 3)
|
|
throw(TriangulationUnderconstrainedException());
|
|
|
|
// 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 unit2(noiseModel::Unit::Create(2));
|
|
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
|
|
|
|
// 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;
|
|
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<Pose3> 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<Point3>(landmarkKey);
|
|
}
|
|
|
|
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
|
|
* 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 (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.
|
|
*/
|
|
template<class CALIBRATION>
|
|
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|
const std::vector<Point2>& measurements, const CALIBRATION& K,
|
|
double rank_tol = 1e-9, bool optimize = false) {
|
|
|
|
assert(poses.size() == measurements.size());
|
|
|
|
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));
|
|
// std::cout << "Calibration i \n" << K.K() << std::endl;
|
|
// std::cout << "rank_tol i \n" << rank_tol << std::endl;
|
|
}
|
|
|
|
// create vector with shared pointer to calibration (all the same in this case)
|
|
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);
|
|
|
|
#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
|
|
|
|
return triangulated_point;
|
|
}
|
|
|
|
/**
|
|
* Function to triangulate 3D landmark point from an arbitrary number
|
|
* of poses (at least 2) using the DLT. This function is similar to the one
|
|
* 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 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.
|
|
*/
|
|
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) {
|
|
|
|
assert(poses.size() == measurements.size());
|
|
assert(poses.size() == Ks.size());
|
|
|
|
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));
|
|
// 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);
|
|
|
|
// 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());
|
|
}
|
|
|
|
return triangulated_point;
|
|
}
|
|
|
|
} // \namespace gtsam
|
|
|