Added rank threshold to triangulateLOST
Travis Driver
parent
0a7c1d8712
commit
3eec88f60e
|
|
@ -25,9 +25,9 @@
|
|||
namespace gtsam {
|
||||
|
||||
Vector4 triangulateHomogeneousDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
|
||||
projection_matrices,
|
||||
const Point2Vector& measurements, double rank_tol) {
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const Point2Vector& measurements,
|
||||
double rank_tol) {
|
||||
// number of cameras
|
||||
size_t m = projection_matrices.size();
|
||||
|
||||
|
|
@ -56,9 +56,9 @@ Vector4 triangulateHomogeneousDLT(
|
|||
}
|
||||
|
||||
Vector4 triangulateHomogeneousDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
|
||||
projection_matrices,
|
||||
const std::vector<Unit3>& measurements, double rank_tol) {
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements,
|
||||
double rank_tol) {
|
||||
// number of cameras
|
||||
size_t m = projection_matrices.size();
|
||||
|
||||
|
|
@ -68,9 +68,7 @@ Vector4 triangulateHomogeneousDLT(
|
|||
for (size_t i = 0; i < m; i++) {
|
||||
size_t row = i * 2;
|
||||
const Matrix34& projection = projection_matrices.at(i);
|
||||
const Point3& p =
|
||||
measurements.at(i)
|
||||
.point3(); // to get access to x,y,z of the bearing vector
|
||||
const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
|
||||
|
||||
// build system of equations
|
||||
A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
|
||||
|
|
@ -85,7 +83,8 @@ Vector4 triangulateHomogeneousDLT(
|
|||
|
||||
Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
||||
const Point3Vector& calibratedMeasurements,
|
||||
const SharedIsotropic& measurementNoise) {
|
||||
const SharedIsotropic& measurementNoise,
|
||||
double rank_tol) {
|
||||
size_t m = calibratedMeasurements.size();
|
||||
assert(m == poses.size());
|
||||
|
||||
|
|
@ -105,36 +104,39 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
|||
const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
|
||||
|
||||
// Note: Setting q_i = 1.0 gives same results as DLT.
|
||||
const double q_i = wZi.cross(wZj).norm() /
|
||||
(measurementNoise->sigma() * d_ij.cross(wZj).norm());
|
||||
const double q_i = wZi.cross(wZj).norm() / (measurementNoise->sigma() * d_ij.cross(wZj).norm());
|
||||
|
||||
const Matrix23 coefficientMat =
|
||||
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
|
||||
const Matrix23 coefficientMat = q_i *
|
||||
skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
|
||||
wTi.rotation().matrix().transpose();
|
||||
|
||||
A.block<2, 3>(2 * i, 0) << coefficientMat;
|
||||
b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
|
||||
}
|
||||
return A.colPivHouseholderQr().solve(b);
|
||||
|
||||
Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
|
||||
A_Qr.setThreshold(rank_tol);
|
||||
|
||||
// if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
|
||||
|
||||
return A_Qr.solve(b);
|
||||
}
|
||||
|
||||
Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
|
||||
projection_matrices,
|
||||
const Point2Vector& measurements, double rank_tol) {
|
||||
Vector4 v =
|
||||
triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const Point2Vector& measurements,
|
||||
double rank_tol) {
|
||||
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
|
||||
// Create 3D point from homogeneous coordinates
|
||||
return Point3(v.head<3>() / v[3]);
|
||||
}
|
||||
|
||||
Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
|
||||
projection_matrices,
|
||||
const std::vector<Unit3>& measurements, double rank_tol) {
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements,
|
||||
double rank_tol) {
|
||||
// contrary to previous triangulateDLT, this is now taking Unit3 inputs
|
||||
Vector4 v =
|
||||
triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
|
||||
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
|
||||
// Create 3D point from homogeneous coordinates
|
||||
return Point3(v.head<3>() / v[3]);
|
||||
}
|
||||
|
|
@ -146,8 +148,7 @@ Point3 triangulateDLT(
|
|||
* @param landmarkKey to refer to landmark
|
||||
* @return refined Point3
|
||||
*/
|
||||
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
|
||||
Key landmarkKey) {
|
||||
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) {
|
||||
// Maybe we should consider Gauss-Newton?
|
||||
LevenbergMarquardtParams params;
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
|
|
|
|||
|
|
@ -20,19 +20,19 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "gtsam/geometry/Point3.h"
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/Cal3DS2.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/geometry/SphericalCamera.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/TriangulationFactor.h>
|
||||
#include "gtsam/geometry/Point3.h"
|
||||
|
||||
#include <optional>
|
||||
|
||||
|
|
@ -41,18 +41,17 @@ namespace gtsam {
|
|||
/// Exception thrown by triangulateDLT when SVD returns rank < 3
|
||||
class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error {
|
||||
public:
|
||||
TriangulationUnderconstrainedException() :
|
||||
std::runtime_error("Triangulation Underconstrained Exception.") {
|
||||
}
|
||||
TriangulationUnderconstrainedException()
|
||||
: std::runtime_error("Triangulation Underconstrained Exception.") {}
|
||||
};
|
||||
|
||||
/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
|
||||
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.") {
|
||||
}
|
||||
TriangulationCheiralityException()
|
||||
: std::runtime_error(
|
||||
"Triangulation Cheirality Exception: The resulting landmark is behind one or more "
|
||||
"cameras.") {}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -64,11 +63,13 @@ public:
|
|||
*/
|
||||
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const Point2Vector& measurements, double rank_tol = 1e-9);
|
||||
const Point2Vector& measurements,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
|
||||
* (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
|
||||
* (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and
|
||||
* Zisserman)
|
||||
* @param projection_matrices Projection matrices (K*P^-1)
|
||||
* @param measurements Unit3 bearing measurements
|
||||
* @param rank_tol SVD rank tolerance
|
||||
|
|
@ -76,7 +77,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
|
|||
*/
|
||||
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
|
||||
const std::vector<Unit3>& measurements,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
|
||||
|
|
@ -85,16 +87,16 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
|
|||
* @param rank_tol SVD rank tolerance
|
||||
* @return Triangulated Point3
|
||||
*/
|
||||
GTSAM_EXPORT Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
GTSAM_EXPORT Point3
|
||||
triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const Point2Vector& measurements,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* overload of previous function to work with Unit3 (projected to canonical camera)
|
||||
*/
|
||||
GTSAM_EXPORT Point3 triangulateDLT(
|
||||
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
GTSAM_EXPORT Point3
|
||||
triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
|
||||
const std::vector<Unit3>& measurements,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
|
|
@ -110,7 +112,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
|
|||
*/
|
||||
GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
||||
const Point3Vector& calibratedMeasurements,
|
||||
const SharedIsotropic& measurementNoise);
|
||||
const SharedIsotropic& measurementNoise,
|
||||
double rank_tol = 1e-9);
|
||||
|
||||
/**
|
||||
* Create a factor graph with projection factors from poses and one calibration
|
||||
|
|
@ -123,8 +126,10 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
|
|||
*/
|
||||
template <class CALIBRATION>
|
||||
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
||||
const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal,
|
||||
const Point2Vector& measurements, Key landmarkKey,
|
||||
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)) {
|
||||
Values values;
|
||||
|
|
@ -152,14 +157,15 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
|||
template <class CAMERA>
|
||||
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
||||
const CameraSet<CAMERA>& cameras,
|
||||
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
|
||||
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));
|
||||
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>> //
|
||||
|
|
@ -176,7 +182,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
|
|||
* @return refined Point3
|
||||
*/
|
||||
GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
|
||||
const Values& values, Key landmarkKey);
|
||||
const Values& values,
|
||||
Key landmarkKey);
|
||||
|
||||
/**
|
||||
* Given an initial estimate , refine a point using measurements in several cameras
|
||||
|
|
@ -189,9 +196,9 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
|
|||
template <class CALIBRATION>
|
||||
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
|
||||
std::shared_ptr<CALIBRATION> sharedCal,
|
||||
const Point2Vector& measurements, const Point3& initialEstimate,
|
||||
const Point2Vector& measurements,
|
||||
const Point3& initialEstimate,
|
||||
const SharedNoiseModel& model = nullptr) {
|
||||
|
||||
// Create a factor graph and initial values
|
||||
const auto [graph, values] = triangulationGraph<CALIBRATION> //
|
||||
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
|
||||
|
|
@ -207,11 +214,10 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
|
|||
* @return refined Point3
|
||||
*/
|
||||
template <class CAMERA>
|
||||
Point3 triangulateNonlinear(
|
||||
const CameraSet<CAMERA>& cameras,
|
||||
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
|
||||
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
|
||||
const auto [graph, values] = triangulationGraph<CAMERA> //
|
||||
(cameras, measurements, Symbol('p', 0), initialEstimate, model);
|
||||
|
|
@ -220,8 +226,8 @@ Point3 triangulateNonlinear(
|
|||
}
|
||||
|
||||
template <class CAMERA>
|
||||
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
|
||||
projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
|
||||
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());
|
||||
|
|
@ -257,8 +263,8 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
|
|||
/** Internal undistortMeasurement to be used by undistortMeasurement and
|
||||
* undistortMeasurements */
|
||||
template <class CALIBRATION, class MEASUREMENT>
|
||||
MEASUREMENT undistortMeasurementInternal(
|
||||
const CALIBRATION& cal, const MEASUREMENT& measurement,
|
||||
MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
|
||||
const MEASUREMENT& measurement,
|
||||
std::optional<Cal3_S2> pinholeCal = {}) {
|
||||
if (!pinholeCal) {
|
||||
pinholeCal = createPinholeCalibration(cal);
|
||||
|
|
@ -278,13 +284,13 @@ MEASUREMENT undistortMeasurementInternal(
|
|||
* @return measurements with the effect of the distortion of sharedCal removed.
|
||||
*/
|
||||
template <class CALIBRATION>
|
||||
Point2Vector undistortMeasurements(const CALIBRATION& cal,
|
||||
const Point2Vector& measurements) {
|
||||
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::transform(measurements.begin(),
|
||||
measurements.end(),
|
||||
std::back_inserter(undistortedMeasurements),
|
||||
[&cal, &pinholeCalibration](const Point2& measurement) {
|
||||
return undistortMeasurementInternal<CALIBRATION>(
|
||||
|
|
@ -295,8 +301,7 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal,
|
|||
|
||||
/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
|
||||
template <>
|
||||
inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
|
||||
const Point2Vector& measurements) {
|
||||
inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) {
|
||||
return measurements;
|
||||
}
|
||||
|
||||
|
|
@ -313,16 +318,14 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
|
|||
*/
|
||||
template <class CAMERA>
|
||||
typename CAMERA::MeasurementVector undistortMeasurements(
|
||||
const CameraSet<CAMERA>& cameras,
|
||||
const typename CAMERA::MeasurementVector& measurements) {
|
||||
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>(
|
||||
undistortedMeasurements[ii] = undistortMeasurementInternal<typename CAMERA::CalibrationType>(
|
||||
cameras[ii].calibration(), measurements[ii]);
|
||||
}
|
||||
return undistortedMeasurements;
|
||||
|
|
@ -353,12 +356,13 @@ inline SphericalCamera::MeasurementVector undistortMeasurements(
|
|||
* @return homogeneous measurements in image plane
|
||||
*/
|
||||
template <class CALIBRATION>
|
||||
inline Point3Vector calibrateMeasurementsShared(
|
||||
const CALIBRATION& cal, const Point2Vector& measurements) {
|
||||
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::transform(measurements.begin(),
|
||||
measurements.end(),
|
||||
std::back_inserter(calibratedMeasurements),
|
||||
[&cal](const Point2& measurement) {
|
||||
Point3 p;
|
||||
|
|
@ -377,24 +381,20 @@ inline Point3Vector calibrateMeasurementsShared(
|
|||
* @return homogeneous measurements in image plane
|
||||
*/
|
||||
template <class CAMERA>
|
||||
inline Point3Vector calibrateMeasurements(
|
||||
const CameraSet<CAMERA>& cameras,
|
||||
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;
|
||||
calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0;
|
||||
}
|
||||
return calibratedMeasurements;
|
||||
}
|
||||
|
||||
/** Specialize for SphericalCamera to do nothing. */
|
||||
template <class CAMERA = SphericalCamera>
|
||||
inline Point3Vector calibrateMeasurements(
|
||||
const CameraSet<SphericalCamera>& cameras,
|
||||
inline Point3Vector calibrateMeasurements(const CameraSet<SphericalCamera>& cameras,
|
||||
const SphericalCamera::MeasurementVector& measurements) {
|
||||
Point3Vector calibratedMeasurements(measurements.size());
|
||||
for (size_t ii = 0; ii < measurements.size(); ++ii) {
|
||||
|
|
@ -420,7 +420,8 @@ template <class CALIBRATION>
|
|||
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
||||
std::shared_ptr<CALIBRATION> sharedCal,
|
||||
const Point2Vector& measurements,
|
||||
double rank_tol = 1e-9, bool optimize = false,
|
||||
double rank_tol = 1e-9,
|
||||
bool optimize = false,
|
||||
const SharedNoiseModel& model = nullptr,
|
||||
const bool useLOST = false) {
|
||||
assert(poses.size() == measurements.size());
|
||||
|
|
@ -432,24 +433,21 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
// 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);
|
||||
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);
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
|
||||
} 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);
|
||||
auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
|
||||
|
||||
point =
|
||||
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
|
||||
point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
|
||||
}
|
||||
|
||||
// Then refine using non-linear optimization
|
||||
|
|
@ -485,7 +483,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
template <class CAMERA>
|
||||
Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
|
||||
const typename CAMERA::MeasurementVector& measurements,
|
||||
double rank_tol = 1e-9, bool optimize = false,
|
||||
double rank_tol = 1e-9,
|
||||
bool optimize = false,
|
||||
const SharedNoiseModel& model = nullptr,
|
||||
const bool useLOST = false) {
|
||||
size_t m = cameras.size();
|
||||
|
|
@ -499,8 +498,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
|
|||
// 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);
|
||||
SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
|
||||
|
||||
// construct poses from cameras.
|
||||
std::vector<Pose3> poses;
|
||||
|
|
@ -509,20 +507,17 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
|
|||
|
||||
// calibrate the measurements to obtain homogenous coordinates in image
|
||||
// plane.
|
||||
auto calibratedMeasurements =
|
||||
calibrateMeasurements<CAMERA>(cameras, measurements);
|
||||
auto calibratedMeasurements = calibrateMeasurements<CAMERA>(cameras, measurements);
|
||||
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
|
||||
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
|
||||
} 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);
|
||||
auto undistortedMeasurements = undistortMeasurements<CAMERA>(cameras, measurements);
|
||||
|
||||
point =
|
||||
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
|
||||
point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
|
||||
}
|
||||
|
||||
// Then refine using non-linear optimization
|
||||
|
|
@ -545,7 +540,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
|
|||
template <class CALIBRATION>
|
||||
Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
|
||||
const Point2Vector& measurements,
|
||||
double rank_tol = 1e-9, bool optimize = false,
|
||||
double rank_tol = 1e-9,
|
||||
bool optimize = false,
|
||||
const SharedNoiseModel& model = nullptr,
|
||||
const bool useLOST = false) {
|
||||
return triangulatePoint3<PinholeCamera<CALIBRATION>> //
|
||||
|
|
@ -553,9 +549,9 @@ Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
|
|||
}
|
||||
|
||||
struct GTSAM_EXPORT TriangulationParameters {
|
||||
|
||||
double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
|
||||
///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance)
|
||||
///< (the rank is the number of singular values of the triangulation matrix which are larger than
|
||||
///< rankTolerance)
|
||||
bool enableEPI; ///< if set to true, will refine triangulation using LM
|
||||
|
||||
/**
|
||||
|
|
@ -583,30 +579,27 @@ struct GTSAM_EXPORT TriangulationParameters {
|
|||
*
|
||||
*/
|
||||
TriangulationParameters(const double _rankTolerance = 1.0,
|
||||
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
|
||||
const bool _enableEPI = false,
|
||||
double _landmarkDistanceThreshold = -1,
|
||||
double _dynamicOutlierRejectionThreshold = -1,
|
||||
const SharedNoiseModel& _noiseModel = nullptr) :
|
||||
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
|
||||
const SharedNoiseModel& _noiseModel = nullptr)
|
||||
: rankTolerance(_rankTolerance),
|
||||
enableEPI(_enableEPI), //
|
||||
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
|
||||
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
|
||||
noiseModel(_noiseModel){
|
||||
}
|
||||
noiseModel(_noiseModel) {}
|
||||
|
||||
// stream to output
|
||||
friend std::ostream &operator<<(std::ostream &os,
|
||||
const TriangulationParameters& p) {
|
||||
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 << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold << std::endl;
|
||||
os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl;
|
||||
os << "noise model" << std::endl;
|
||||
return os;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ///
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -642,16 +635,10 @@ class TriangulationResult : public std::optional<Point3> {
|
|||
* Constructor
|
||||
*/
|
||||
TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
|
||||
static TriangulationResult Degenerate() {
|
||||
return TriangulationResult(DEGENERATE);
|
||||
}
|
||||
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);
|
||||
}
|
||||
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; }
|
||||
|
|
@ -662,8 +649,7 @@ class TriangulationResult : public std::optional<Point3> {
|
|||
return value();
|
||||
}
|
||||
// stream to output
|
||||
friend std::ostream& operator<<(std::ostream& os,
|
||||
const TriangulationResult& result) {
|
||||
friend std::ostream& operator<<(std::ostream& os, const TriangulationResult& result) {
|
||||
if (result)
|
||||
os << "point = " << *result << std::endl;
|
||||
else
|
||||
|
|
@ -687,7 +673,6 @@ 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
|
||||
|
|
@ -696,25 +681,22 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
|
|||
else
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
Point3 point =
|
||||
triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
|
||||
params.enableEPI, params.noiseModel);
|
||||
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)
|
||||
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();
|
||||
if (p_local.z() <= 0) return TriangulationResult::BehindCamera();
|
||||
#endif
|
||||
// Check reprojection error
|
||||
if (params.dynamicOutlierRejectionThreshold > 0) {
|
||||
|
|
@ -725,19 +707,21 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
|
|||
i += 1;
|
||||
}
|
||||
// Flag as degenerate if average reprojection error is too large
|
||||
if (params.dynamicOutlierRejectionThreshold > 0
|
||||
&& maxReprojError > params.dynamicOutlierRejectionThreshold)
|
||||
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)
|
||||
// 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
|
||||
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may
|
||||
// depend on outliers
|
||||
return TriangulationResult::BehindCamera();
|
||||
}
|
||||
}
|
||||
|
|
@ -749,5 +733,4 @@ using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
|
|||
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
|
||||
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
|
||||
using CameraSetSpherical = CameraSet<SphericalCamera>;
|
||||
} // \namespace gtsam
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue