Loading...
Searching...
No Matches
triangulation.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>
namespace gtsam {
class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
public:
TriangulationUnderconstrainedException() :
std::runtime_error("Triangulation Underconstrained Exception.") {
}
};
class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
public:
TriangulationCheiralityException() :
std::runtime_error(
"Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
}
};
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const Point2Vector& measurements, double rank_tol = 1e-9);
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const Point2Vector& measurements,
double rank_tol = 1e-9);
GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements,
double rank_tol = 1e-9);
GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3Vector& calibratedMeasurements,
const SharedIsotropic& measurementNoise);
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate,
const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera> > //
(camera_i, measurements[i], model, landmarkKey);
}
return std::make_pair(graph, values);
}
template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(
traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.emplace_shared<TriangulationFactor<CAMERA> > //
(camera_i, measurements[i], model? model : unit, landmarkKey);
}
return std::make_pair(graph, values);
}
GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
const Values& values, Key landmarkKey);
template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0));
}
template<class CAMERA>
Point3 triangulateNonlinear(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CAMERA> //
(cameras, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0));
}
template<class CAMERA>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (const CAMERA &camera: cameras) {
projection_matrices.push_back(camera.cameraProjectionMatrix());
}
return projection_matrices;
}
// overload, assuming pinholePose
template<class CALIBRATION>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (size_t i = 0; i < poses.size(); i++) {
PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
projection_matrices.push_back(camera.cameraProjectionMatrix());
}
return projection_matrices;
}
template <class CALIBRATION>
Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
const auto& K = cal.K();
return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
}
template <class CALIBRATION, class MEASUREMENT>
MEASUREMENT undistortMeasurementInternal(
const CALIBRATION& cal, const MEASUREMENT& measurement,
boost::optional<Cal3_S2> pinholeCal = boost::none) {
if (!pinholeCal) {
pinholeCal = createPinholeCalibration(cal);
}
return pinholeCal->uncalibrate(cal.calibrate(measurement));
}
template <class CALIBRATION>
Point2Vector undistortMeasurements(const CALIBRATION& cal,
const Point2Vector& measurements) {
Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
Point2Vector undistortedMeasurements;
// Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted.
std::transform(measurements.begin(), measurements.end(),
std::back_inserter(undistortedMeasurements),
[&cal, &pinholeCalibration](const Point2& measurement) {
return undistortMeasurementInternal<CALIBRATION>(
cal, measurement, pinholeCalibration);
});
return undistortedMeasurements;
}
template <>
inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
const Point2Vector& measurements) {
return measurements;
}
template <class CAMERA>
typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements) {
const size_t nrMeasurements = measurements.size();
assert(nrMeasurements == cameras.size());
typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < nrMeasurements; ++ii) {
// Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted.
undistortedMeasurements[ii] =
undistortMeasurementInternal<typename CAMERA::CalibrationType>(
cameras[ii].calibration(), measurements[ii]);
}
return undistortedMeasurements;
}
template <class CAMERA = PinholeCamera<Cal3_S2>>
inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
return measurements;
}
template <class CAMERA = SphericalCamera>
inline SphericalCamera::MeasurementVector undistortMeasurements(
const CameraSet<SphericalCamera>& cameras,
const SphericalCamera::MeasurementVector& measurements) {
return measurements;
}
template <class CALIBRATION>
inline Point3Vector calibrateMeasurementsShared(
const CALIBRATION& cal, const Point2Vector& measurements) {
Point3Vector calibratedMeasurements;
// Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted.
std::transform(measurements.begin(), measurements.end(),
std::back_inserter(calibratedMeasurements),
[&cal](const Point2& measurement) {
Point3 p;
p << cal.calibrate(measurement), 1.0;
return p;
});
return calibratedMeasurements;
}
template <class CAMERA>
inline Point3Vector calibrateMeasurements(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements) {
const size_t nrMeasurements = measurements.size();
assert(nrMeasurements == cameras.size());
Point3Vector calibratedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < nrMeasurements; ++ii) {
calibratedMeasurements[ii]
<< cameras[ii].calibration().calibrate(measurements[ii]),
1.0;
}
return calibratedMeasurements;
}
template <class CAMERA = SphericalCamera>
inline Point3Vector calibrateMeasurements(
const CameraSet<SphericalCamera>& cameras,
const SphericalCamera::MeasurementVector& measurements) {
Point3Vector calibratedMeasurements(measurements.size());
for (size_t ii = 0; ii < measurements.size(); ++ii) {
calibratedMeasurements[ii] << measurements[ii].point3();
}
return calibratedMeasurements;
}
template <class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool useLOST = false) {
assert(poses.size() == measurements.size());
if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
// Triangulate linearly
Point3 point;
if (useLOST) {
// Reduce input noise model to an isotropic noise model using the mean of
// the diagonal.
const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
SharedIsotropic measurementNoise =
noiseModel::Isotropic::Sigma(2, measurementSigma);
// calibrate the measurements to obtain homogenous coordinates in image
// plane.
auto calibratedMeasurements =
calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
} else {
// construct projection matrices from poses & calibration
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
// Undistort the measurements, leaving only the pinhole elements in effect.
auto undistortedMeasurements =
undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
point =
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
}
// Then refine using non-linear optimization
if (optimize)
point = triangulateNonlinear<CALIBRATION> //
(poses, sharedCal, measurements, point, model);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
for (const Pose3& pose : poses) {
const Point3& p_local = pose.transformTo(point);
if (p_local.z() <= 0) throw(TriangulationCheiralityException());
}
#endif
return point;
}
template <class CAMERA>
Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool useLOST = false) {
size_t m = cameras.size();
assert(measurements.size() == m);
if (m < 2) throw(TriangulationUnderconstrainedException());
// Triangulate linearly
Point3 point;
if (useLOST) {
// Reduce input noise model to an isotropic noise model using the mean of
// the diagonal.
const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
SharedIsotropic measurementNoise =
noiseModel::Isotropic::Sigma(2, measurementSigma);
// construct poses from cameras.
std::vector<Pose3> poses;
poses.reserve(cameras.size());
for (const auto& camera : cameras) poses.push_back(camera.pose());
// calibrate the measurements to obtain homogenous coordinates in image
// plane.
auto calibratedMeasurements =
calibrateMeasurements<CAMERA>(cameras, measurements);
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
} else {
// construct projection matrices from poses & calibration
auto projection_matrices = projectionMatricesFromCameras(cameras);
// Undistort the measurements, leaving only the pinhole elements in effect.
auto undistortedMeasurements =
undistortMeasurements<CAMERA>(cameras, measurements);
point =
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
}
// Then refine using non-linear optimization
if (optimize) {
point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
}
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
for (const CAMERA& camera : cameras) {
const Point3& p_local = camera.pose().transformTo(point);
if (p_local.z() <= 0) throw(TriangulationCheiralityException());
}
#endif
return point;
}
template <class CALIBRATION>
Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false,
const SharedNoiseModel& model = nullptr,
const bool useLOST = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION>> //
(cameras, measurements, rank_tol, optimize, model, useLOST);
}
struct GTSAM_EXPORT TriangulationParameters {
double rankTolerance;
bool enableEPI;
double landmarkDistanceThreshold; //
double dynamicOutlierRejectionThreshold;
SharedNoiseModel noiseModel;
TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1,
const SharedNoiseModel& _noiseModel = nullptr) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
noiseModel(_noiseModel){
}
// stream to output
friend std::ostream &operator<<(std::ostream &os,
const TriangulationParameters& p) {
os << "rankTolerance = " << p.rankTolerance << std::endl;
os << "enableEPI = " << p.enableEPI << std::endl;
os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
<< std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
os << "noise model" << std::endl;
return os;
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(rankTolerance);
ar & BOOST_SERIALIZATION_NVP(enableEPI);
ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
}
};
class TriangulationResult : public boost::optional<Point3> {
public:
enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
Status status;
private:
TriangulationResult(Status s) : status(s) {}
public:
TriangulationResult() {}
TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
static TriangulationResult FarPoint() {
return TriangulationResult(FAR_POINT);
}
static TriangulationResult BehindCamera() {
return TriangulationResult(BEHIND_CAMERA);
}
bool valid() const { return status == VALID; }
bool degenerate() const { return status == DEGENERATE; }
bool outlier() const { return status == OUTLIER; }
bool farPoint() const { return status == FAR_POINT; }
bool behindCamera() const { return status == BEHIND_CAMERA; }
// stream to output
friend std::ostream& operator<<(std::ostream& os,
const TriangulationResult& result) {
if (result)
os << "point = " << *result << std::endl;
else
os << "no point, status = " << result.status << std::endl;
return os;
}
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int version) {
ar& BOOST_SERIALIZATION_NVP(status);
}
};
template<class CAMERA>
TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters& params) {
size_t m = cameras.size();
// if we have a single pose the corresponding factor is uninformative
if (m < 2)
return TriangulationResult::Degenerate();
else
// We triangulate the 3D position of the landmark
try {
Point3 point =
triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
params.enableEPI, params.noiseModel);
// Check landmark distance and re-projection errors to avoid outliers
size_t i = 0;
double maxReprojError = 0.0;
for(const CAMERA& camera: cameras) {
const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0
&& distance3(pose.translation(), point)
> params.landmarkDistanceThreshold)
return TriangulationResult::FarPoint();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
// Only needed if this was not yet handled by exception
const Point3& p_local = pose.transformTo(point);
if (p_local.z() <= 0)
return TriangulationResult::BehindCamera();
#endif
// Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0) {
const typename CAMERA::Measurement& zi = measured.at(i);
Point2 reprojectionError = camera.reprojectionError(point, zi);
maxReprojError = std::max(maxReprojError, reprojectionError.norm());
}
i += 1;
}
// Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0
&& maxReprojError > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Outlier();
// all good!
return TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) {
// This exception is thrown if
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
return TriangulationResult::BehindCamera();
}
}
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
Pose2.h
2D Pose
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
Cal3Fisheye.h
Calibration of a fisheye camera.
PinholeCamera.h
Base class for all pinhole cameras.
Cal3Bundler.h
Calibration used by Bundler.
SphericalCamera.h
Calibrated camera with spherical projection.
Cal3Unified.h
Unified Calibration Model, see Mei07icra for details.
CameraSet.h
Base class to create smart factors on poses or cameras.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::calibrateMeasurementsShared
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using shared camer...
Definition triangulation.h:357
gtsam::triangulateLOST
Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise)
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv....
Definition triangulation.cpp:92
gtsam::undistortMeasurements
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
Remove distortion for measurements so as if the measurements came from a pinhole camera.
Definition triangulation.h:282
gtsam::createPinholeCalibration
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
Create a pinhole calibration from a different Cal3 object, removing distortion.
Definition triangulation.h:253
gtsam::undistortMeasurementInternal
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none)
Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
Definition triangulation.h:261
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
gtsam::triangulateNonlinear
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
Given an initial estimate , refine a point using measurements in several cameras.
Definition triangulation.h:187
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition Point3.cpp:27
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::triangulatePoint3
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DL...
Definition triangulation.h:421
gtsam::triangulationGraph
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
Create a factor graph with projection factors from poses and one calibration.
Definition triangulation.h:122
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::calibrateMeasurements
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using camera intri...
Definition triangulation.h:381
gtsam::triangulateDLT
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition triangulation.cpp:127
gtsam::triangulateHomogeneousDLT
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition triangulation.cpp:27
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition Pose3.cpp:371
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
gtsam::TriangulationUnderconstrainedException
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition triangulation.h:39
gtsam::TriangulationCheiralityException
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition triangulation.h:47
gtsam::TriangulationParameters
Definition triangulation.h:556
gtsam::TriangulationParameters::TriangulationParameters
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel &_noiseModel=nullptr)
Constructor.
Definition triangulation.h:586
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition triangulation.h:573
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition triangulation.h:558
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition triangulation.h:566
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition triangulation.h:560
gtsam::TriangulationParameters::noiseModel
SharedNoiseModel noiseModel
used in the nonlinear triangulation
Definition triangulation.h:575
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
gtsam::TriangulationResult::TriangulationResult
TriangulationResult()
Default constructor, only for serialization.
Definition triangulation.h:638
gtsam::TriangulationResult::TriangulationResult
TriangulationResult(const Point3 &p)
Constructor.
Definition triangulation.h:643
gtsam::TriangulationResult::access
friend class boost::serialization::access
Serialization function.
Definition triangulation.h:671
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition FactorGraph.h:192
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
gtsam::TriangulationFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition TriangulationFactor.h:33
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
Symbol.h