Go to the documentation of this file.
45 std::runtime_error(
"Triangulation Underconstrained Exception.") {
54 "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
79 const std::vector<Unit3>&
measurements,
double rank_tol = 1
e-9);
91 double rank_tol = 1
e-9);
99 double rank_tol = 1
e-9);
114 double rank_tol = 1
e-9);
125 template<
class CALIBRATION>
127 const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION>
sharedCal,
129 const Point3& initialEstimate,
135 const Pose3& pose_i = poses[
i];
153 template<
class CAMERA>
156 const typename CAMERA::MeasurementVector&
measurements,
Key landmarkKey,
157 const Point3& initialEstimate,
165 const CAMERA& camera_i = cameras[
i];
190 template<
class CALIBRATION>
197 const auto [
graph,
values] = triangulationGraph<CALIBRATION>
210 template<
class CAMERA>
213 const typename CAMERA::MeasurementVector&
measurements,
const Point3& initialEstimate,
217 const auto [
graph,
values] = triangulationGraph<CAMERA>
223 template<
class CAMERA>
224 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
226 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
227 for (
const CAMERA &
camera: cameras) {
228 projection_matrices.push_back(
camera.cameraProjectionMatrix());
230 return projection_matrices;
234 template<
class CALIBRATION>
236 const std::vector<Pose3> &poses, std::shared_ptr<CALIBRATION>
sharedCal) {
237 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
238 for (
size_t i = 0;
i < poses.size();
i++) {
240 projection_matrices.push_back(
camera.cameraProjectionMatrix());
242 return projection_matrices;
252 template <
class CALIBRATION>
254 const auto&
K =
cal.K();
255 return Cal3_S2(
K(0, 0),
K(1, 1),
K(0, 1),
K(0, 2),
K(1, 2));
260 template <
class CALIBRATION,
class MEASUREMENT>
263 std::optional<Cal3_S2> pinholeCal = {}) {
281 template <
class CALIBRATION>
289 std::back_inserter(undistortedMeasurements),
291 return undistortMeasurementInternal<CALIBRATION>(
292 cal, measurement, pinholeCalibration);
294 return undistortedMeasurements;
315 template <
class CAMERA>
318 const typename CAMERA::MeasurementVector&
measurements) {
320 assert(nrMeasurements == cameras.size());
321 typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
322 for (
size_t ii = 0; ii < nrMeasurements; ++ii) {
325 undistortedMeasurements[ii] =
326 undistortMeasurementInternal<typename CAMERA::CalibrationType>(
329 return undistortedMeasurements;
333 template <
class CAMERA = PinholeCamera<Cal3_S2>>
341 template <
class CAMERA = SphericalCamera>
356 template <
class CALIBRATION>
363 std::back_inserter(calibratedMeasurements),
366 p << cal.calibrate(measurement), 1.0;
369 return calibratedMeasurements;
380 template <
class CAMERA>
383 const typename CAMERA::MeasurementVector&
measurements) {
385 assert(nrMeasurements == cameras.size());
387 for (
size_t ii = 0; ii < nrMeasurements; ++ii) {
388 calibratedMeasurements[ii]
389 << cameras[ii].calibration().calibrate(
measurements[ii]),
392 return calibratedMeasurements;
396 template <
class CAMERA = SphericalCamera>
402 calibratedMeasurements[ii] <<
measurements[ii].point3();
404 return calibratedMeasurements;
420 template <
class CALIBRATION>
424 double rank_tol = 1
e-9,
bool optimize =
false,
426 const bool useLOST =
false) {
435 const double measurementSigma =
model ?
model->sigmas().mean() : 1
e-4;
440 auto calibratedMeasurements =
450 auto undistortedMeasurements =
454 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
459 point = triangulateNonlinear<CALIBRATION>
462 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
487 template <
class CAMERA>
490 double rank_tol = 1
e-9,
bool optimize =
false,
492 const bool useLOST =
false) {
493 size_t m = cameras.size();
503 const double measurementSigma =
model ?
model->sigmas().mean() : 1
e-4;
508 std::vector<Pose3> poses;
509 poses.reserve(cameras.size());
514 auto calibratedMeasurements =
524 auto undistortedMeasurements =
528 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
536 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
538 for (
const CAMERA&
camera : cameras) {
548 template <
class CALIBRATION>
551 double rank_tol = 1
e-9,
bool optimize =
false,
553 const bool useLOST =
false) {
554 return triangulatePoint3<PinholeCamera<CALIBRATION>>
594 const bool _enableEPI =
false,
double _landmarkDistanceThreshold = -1,
595 double _dynamicOutlierRejectionThreshold = -1,
596 const bool _useLOST =
false,
598 rankTolerance(_rankTolerance), enableEPI(_enableEPI),
599 landmarkDistanceThreshold(_landmarkDistanceThreshold),
600 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
602 noiseModel(_noiseModel){
608 os <<
"rankTolerance = " <<
p.rankTolerance << std::endl;
609 os <<
"enableEPI = " <<
p.enableEPI << std::endl;
610 os <<
"landmarkDistanceThreshold = " <<
p.landmarkDistanceThreshold
612 os <<
"dynamicOutlierRejectionThreshold = "
613 <<
p.dynamicOutlierRejectionThreshold << std::endl;
614 os <<
"useLOST = " <<
p.useLOST << std::endl;
615 os <<
"noise model" << std::endl;
621 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
622 friend class boost::serialization::access;
624 template<
class ARCHIVE>
625 void serialize(ARCHIVE & ar,
const unsigned int version) {
626 ar & BOOST_SERIALIZATION_NVP(rankTolerance);
627 ar & BOOST_SERIALIZATION_NVP(enableEPI);
628 ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
629 ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
672 if (!has_value())
throw std::runtime_error(
"TriangulationResult has no value");
679 os <<
"point = " << *
result << std::endl;
681 os <<
"no point, status = " <<
result.status << std::endl;
686 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
687 friend class boost::serialization::access;
689 template <
class ARCHIVE>
690 void serialize(ARCHIVE& ar,
const unsigned int version) {
691 ar& BOOST_SERIALIZATION_NVP(
status);
697 template<
class CAMERA>
699 const typename CAMERA::MeasurementVector&
measured,
702 size_t m = cameras.size();
711 triangulatePoint3<CAMERA>(cameras,
measured,
params.rankTolerance,
716 double maxReprojError = 0.0;
717 for(
const CAMERA&
camera: cameras) {
719 if (
params.landmarkDistanceThreshold > 0
721 >
params.landmarkDistanceThreshold)
723 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
727 if (p_local.z() <= 0)
731 if (
params.dynamicOutlierRejectionThreshold > 0) {
734 maxReprojError =
std::max(maxReprojError, reprojectionError.norm());
739 if (
params.dynamicOutlierRejectionThreshold > 0
740 && maxReprojError >
params.dynamicOutlierRejectionThreshold)
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromCameras(const CameraSet< CAMERA > &cameras)
Point2Vector MeasurementVector
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Base class to create smart factors on poses or cameras.
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Calibrated camera with spherical projection.
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
The most common 5DOF 3D->2D calibration.
double dynamicOutlierRejectionThreshold
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
static TriangulationResult FarPoint()
TriangulationUnderconstrainedException()
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, std::optional< Cal3_S2 > pinholeCal={})
ofstream os("timeSchurFactors.csv")
static const SmartProjectionParams params
A set of cameras, all with their own calibration.
GenericMeasurement< Point2, Point2 > Measurement
static TriangulationResult Outlier()
double rankTolerance
(the rank is the number of singular values of the triangulation matrix which are larger than rankTole...
STL compatible allocator to use with types requiring a non standrad alignment.
TriangulationResult(const Point3 &p)
const gtsam::Point3 & get() const
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
static shared_ptr Create(size_t dim)
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Base class for all pinhole cameras.
SharedNoiseModel noiseModel
used in the nonlinear triangulation
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromPoses(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal)
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters ¶ms)
triangulateSafe: extensive checking of the outcome
void insert(Key j, const Vector &value)
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
static const std::shared_ptr< Cal3_S2 > sharedCal
noiseModel::Base::shared_ptr SharedNoiseModel
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const TriangulationResult &result)
static TriangulationResult Degenerate()
Unified Calibration Model, see Mei07icra for details.
noiseModel::Diagonal::shared_ptr model
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
std::vector< Unit3 > MeasurementVector
friend std::ostream & operator<<(std::ostream &os, const TriangulationParameters &p)
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
static TriangulationResult BehindCamera()
TriangulationResult(Status s)
std::vector< double > measurements
Exception thrown by triangulateDLT when SVD returns rank < 3.
Factor Graph consisting of non-linear factors.
double landmarkDistanceThreshold
bool behindCamera() const
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
TriangulationCheiralityException()
const Pose3 & pose() const
return pose, constant version
PinholeCamera< Cal3Bundler0 > Camera
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
The most common 5DOF 3D->2D calibration.
noiseModel::Isotropic::shared_ptr SharedIsotropic
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise, double rank_tol)
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv....
static const CalibratedCamera camera(kDefaultPose)
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const bool _useLOST=false, const SharedNoiseModel &_noiseModel=nullptr)
Calibration used by Bundler.
static Point2 measurement(323.0, 240.0)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
std::vector< Point3, Eigen::aligned_allocator< Point3 > > Point3Vector
Calibration of a fisheye camera.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:54