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);
98 const std::vector<Unit3>& measurements,
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,
134 for (
size_t i = 0;
i < measurements.size();
i++) {
135 const Pose3& pose_i = poses[
i];
139 (camera_i, measurements[
i],
model, landmarkKey);
153 template<
class CAMERA>
156 const typename CAMERA::MeasurementVector& measurements,
Key landmarkKey,
157 const Point3& initialEstimate,
164 for (
size_t i = 0;
i < measurements.size();
i++) {
165 const CAMERA& camera_i = cameras[
i];
167 (camera_i, measurements[
i],
model?
model : unit, landmarkKey);
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>
218 (cameras, measurements,
Symbol(
'p', 0), initialEstimate,
model);
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) {
319 const size_t nrMeasurements = measurements.size();
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>(
327 cameras[ii].calibration(), measurements[ii]);
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) {
384 const size_t nrMeasurements = measurements.size();
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>
400 Point3Vector calibratedMeasurements(measurements.size());
401 for (
size_t ii = 0; ii < measurements.size(); ++ii) {
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) {
427 assert(poses.size() == measurements.size());
435 const double measurementSigma =
model ?
model->sigmas().mean() : 1
e-4;
440 auto calibratedMeasurements =
441 calibrateMeasurementsShared<CALIBRATION>(*
sharedCal, measurements);
450 auto undistortedMeasurements =
451 undistortMeasurements<CALIBRATION>(*
sharedCal, measurements);
454 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
459 point = triangulateNonlinear<CALIBRATION>
462 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
487 template <
class CAMERA>
489 const typename CAMERA::MeasurementVector& measurements,
490 double rank_tol = 1
e-9,
bool optimize =
false,
492 const bool useLOST =
false) {
493 size_t m = cameras.size();
494 assert(measurements.size() ==
m);
503 const double measurementSigma =
model ?
model->sigmas().mean() : 1
e-4;
508 std::vector<Pose3> poses;
509 poses.reserve(cameras.size());
514 auto calibratedMeasurements =
515 calibrateMeasurements<CAMERA>(cameras, measurements);
524 auto undistortedMeasurements =
525 undistortMeasurements<CAMERA>(cameras, measurements);
528 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
533 point = triangulateNonlinear<CAMERA>(cameras, measurements,
point,
model);
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)