12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

code formatted

release/4.3a0
Sushmita 2020-12-03 21:10:10 -05:00
parent adf3ce5574
commit 8be6890b20
5 changed files with 3155 additions and 2961 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

702
gtsam/geometry/triangulation.h
View File

@ -27,49 +27,52 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
namespace gtsam { namespace gtsam
{
/// Exception thrown by triangulateDLT when SVD returns rank < 3 /// Exception thrown by triangulateDLT when SVD returns rank < 3
class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error
public: {
TriangulationUnderconstrainedException() : public:
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 /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error
public: {
TriangulationCheiralityException() : public:
std::runtime_error( TriangulationCheiralityException() : std::runtime_error(
"Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.")
} {
}; }
};
/** /**
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
* @param projection_matrices Projection matrices (K*P^-1) * @param projection_matrices Projection matrices (K*P^-1)
* @param measurements 2D measurements * @param measurements 2D measurements
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
* @return Triangulated point, in homogeneous coordinates * @return Triangulated point, in homogeneous coordinates
*/ */
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, 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);
/** /**
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
* @param projection_matrices Projection matrices (K*P^-1) * @param projection_matrices Projection matrices (K*P^-1)
* @param measurements 2D measurements * @param measurements 2D measurements
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
* @return Triangulated Point3 * @return Triangulated Point3
*/ */
GTSAM_EXPORT Point3 triangulateDLT( GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> &projection_matrices,
const Point2Vector& measurements, const Point2Vector &measurements,
double rank_tol = 1e-9); double rank_tol = 1e-9);
/** /**
* Create a factor graph with projection factors from poses and one calibration * Create a factor graph with projection factors from poses and one calibration
* @param poses Camera poses * @param poses Camera poses
* @param sharedCal shared pointer to single calibration object (monocular only!) * @param sharedCal shared pointer to single calibration object (monocular only!)
@ -78,27 +81,29 @@ GTSAM_EXPORT Point3 triangulateDLT(
* @param initialEstimate * @param initialEstimate
* @return graph and initial values * @return graph and initial values
*/ */
template<class CALIBRATION> template <class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal, const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, Key landmarkKey, const Point2Vector &measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3 &initialEstimate)
Values values; {
values.insert(landmarkKey, initialEstimate); // Initial landmark value Values values;
NonlinearFactorGraph graph; values.insert(landmarkKey, initialEstimate); // Initial landmark value
static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); NonlinearFactorGraph graph;
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
for (size_t i = 0; i < measurements.size(); i++) { static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
const Pose3& pose_i = poses[i]; for (size_t i = 0; i < measurements.size(); i++)
typedef PinholePose<CALIBRATION> Camera; {
Camera camera_i(pose_i, sharedCal); const Pose3 &pose_i = poses[i];
graph.emplace_shared<TriangulationFactor<Camera> > // typedef PinholePose<CALIBRATION> Camera;
(camera_i, measurements[i], unit2, landmarkKey); Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera>> //
(camera_i, measurements[i], unit2, landmarkKey);
}
return std::make_pair(graph, values);
} }
return std::make_pair(graph, values);
}
/** /**
* Create a factor graph with projection factors from pinhole cameras * Create a factor graph with projection factors from pinhole cameras
* (each camera has a pose and calibration) * (each camera has a pose and calibration)
* @param cameras pinhole cameras (monocular or stereo) * @param cameras pinhole cameras (monocular or stereo)
@ -107,35 +112,37 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
* @param initialEstimate * @param initialEstimate
* @return graph and initial values * @return graph and initial values
*/ */
template<class CAMERA> template <class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA> &cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, const typename CAMERA::MeasurementVector &measurements, Key landmarkKey,
const Point3& initialEstimate) { const Point3 &initialEstimate)
Values values; {
values.insert(landmarkKey, initialEstimate); // Initial landmark value Values values;
NonlinearFactorGraph graph; values.insert(landmarkKey, initialEstimate); // Initial landmark value
static SharedNoiseModel unit(noiseModel::Unit::Create( NonlinearFactorGraph graph;
traits<typename CAMERA::Measurement>::dimension)); static SharedNoiseModel unit(noiseModel::Unit::Create(
for (size_t i = 0; i < measurements.size(); i++) { traits<typename CAMERA::Measurement>::dimension));
const CAMERA& camera_i = cameras[i]; for (size_t i = 0; i < measurements.size(); i++)
graph.emplace_shared<TriangulationFactor<CAMERA> > // {
(camera_i, measurements[i], unit, landmarkKey); const CAMERA &camera_i = cameras[i];
graph.emplace_shared<TriangulationFactor<CAMERA>> //
(camera_i, measurements[i], unit, landmarkKey);
}
return std::make_pair(graph, values);
} }
return std::make_pair(graph, values);
}
/** /**
* Optimize for triangulation * Optimize for triangulation
* @param graph nonlinear factors for projection * @param graph nonlinear factors for projection
* @param values initial values * @param values initial values
* @param landmarkKey to refer to landmark * @param landmarkKey to refer to landmark
* @return refined Point3 * @return refined Point3
*/ */
GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, 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 * Given an initial estimate , refine a point using measurements in several cameras
* @param poses Camera poses * @param poses Camera poses
* @param sharedCal shared pointer to single calibration object * @param sharedCal shared pointer to single calibration object
@ -143,63 +150,69 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
* @param initialEstimate * @param initialEstimate
* @return refined Point3 * @return refined Point3
*/ */
template<class CALIBRATION> template <class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses, Point3 triangulateNonlinear(const std::vector<Pose3> &poses,
boost::shared_ptr<CALIBRATION> sharedCal, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, const Point3& initialEstimate) { const Point2Vector &measurements, const Point3 &initialEstimate)
{
// Create a factor graph and initial values // Create a factor graph and initial values
Values values; Values values;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CALIBRATION> // boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
/** /**
* Given an initial estimate , refine a point using measurements in several cameras * Given an initial estimate , refine a point using measurements in several cameras
* @param cameras pinhole cameras (monocular or stereo) * @param cameras pinhole cameras (monocular or stereo)
* @param measurements 2D measurements * @param measurements 2D measurements
* @param initialEstimate * @param initialEstimate
* @return refined Point3 * @return refined Point3
*/ */
template<class CAMERA> template <class CAMERA>
Point3 triangulateNonlinear( Point3 triangulateNonlinear(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA> &cameras,
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) { const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate)
{
// Create a factor graph and initial values // Create a factor graph and initial values
Values values; Values values;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph<CAMERA> // boost::tie(graph, values) = triangulationGraph<CAMERA> //
(cameras, measurements, Symbol('p', 0), initialEstimate); (cameras, measurements, Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
/** /**
* Create a 3*4 camera projection matrix from calibration and pose. * Create a 3*4 camera projection matrix from calibration and pose.
* Functor for partial application on calibration * Functor for partial application on calibration
* @param pose The camera pose * @param pose The camera pose
* @param cal The calibration * @param cal The calibration
* @return Returns a Matrix34 * @return Returns a Matrix34
*/ */
template<class CALIBRATION> template <class CALIBRATION>
struct CameraProjectionMatrix { struct CameraProjectionMatrix
CameraProjectionMatrix(const CALIBRATION& calibration) : {
K_(calibration.K()) { CameraProjectionMatrix(const CALIBRATION &calibration) : K_(calibration.K())
} {
Matrix34 operator()(const Pose3& pose) const { }
return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); Matrix34 operator()(const Pose3 &pose) const
} {
private: return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
const Matrix3 K_; }
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/** private:
const Matrix3 K_;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* Function to triangulate 3D landmark point from an arbitrary number * Function to triangulate 3D landmark point from an arbitrary number
* of poses (at least 2) using the DLT. The function checks that the * 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 * resulting point lies in front of all cameras, but has no other checks
@ -211,43 +224,45 @@ public:
* @param optimize Flag to turn on nonlinear refinement of triangulation * @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3 * @return Returns a Point3
*/ */
template<class CALIBRATION> template <class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, Point3 triangulatePoint3(const std::vector<Pose3> &poses,
boost::shared_ptr<CALIBRATION> sharedCal, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, double rank_tol = 1e-9, const Point2Vector &measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false)
{
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
if (poses.size() < 2) if (poses.size() < 2)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
for(const Pose3& pose: poses) for (const Pose3 &pose : poses)
projection_matrices.push_back(createP(pose)); projection_matrices.push_back(createP(pose));
// Triangulate linearly // Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// Then refine using non-linear optimization // Then refine using non-linear optimization
if (optimize) if (optimize)
point = triangulateNonlinear<CALIBRATION> // point = triangulateNonlinear<CALIBRATION> //
(poses, sharedCal, measurements, point); (poses, sharedCal, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras // verify that the triangulated point lies in front of all cameras
for(const Pose3& pose: poses) { for (const Pose3 &pose : poses)
const Point3& p_local = pose.transformTo(point); {
if (p_local.z() <= 0) const Point3 &p_local = pose.transformTo(point);
throw(TriangulationCheiralityException()); if (p_local.z() <= 0)
} throw(TriangulationCheiralityException());
}
#endif #endif
return point; return point;
} }
/** /**
* Function to triangulate 3D landmark point from an arbitrary number * 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 * 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 * above, except that each camera has its own calibration. The function
@ -259,72 +274,76 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
* @param optimize Flag to turn on nonlinear refinement of triangulation * @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3 * @return Returns a Point3
*/ */
template<class CAMERA> template <class CAMERA>
Point3 triangulatePoint3( Point3 triangulatePoint3(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA> &cameras,
const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, const typename CAMERA::MeasurementVector &measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false)
{
size_t m = cameras.size(); size_t m = cameras.size();
assert(measurements.size() == m); assert(measurements.size() == m);
if (m < 2) if (m < 2)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for(const CAMERA& camera: cameras) for (const CAMERA &camera : cameras)
projection_matrices.push_back( projection_matrices.push_back(
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())( CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
camera.pose())); camera.pose()));
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization
if (optimize) if (optimize)
point = triangulateNonlinear<CAMERA>(cameras, measurements, point); point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras // verify that the triangulated point lies in front of all cameras
for(const CAMERA& camera: cameras) { for (const CAMERA &camera : cameras)
const Point3& p_local = camera.pose().transformTo(point); {
if (p_local.z() <= 0) const Point3 &p_local = camera.pose().transformTo(point);
throw(TriangulationCheiralityException()); if (p_local.z() <= 0)
} throw(TriangulationCheiralityException());
}
#endif #endif
return point; return point;
} }
/// Pinhole-specific version /// Pinhole-specific version
template<class CALIBRATION> template <class CALIBRATION>
Point3 triangulatePoint3( Point3 triangulatePoint3(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras, const CameraSet<PinholeCamera<CALIBRATION>> &cameras,
const Point2Vector& measurements, double rank_tol = 1e-9, const Point2Vector &measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false)
return triangulatePoint3<PinholeCamera<CALIBRATION> > // {
(cameras, measurements, rank_tol, optimize); return triangulatePoint3<PinholeCamera<CALIBRATION>> //
} (cameras, measurements, rank_tol, optimize);
}
struct GTSAM_EXPORT TriangulationParameters { struct GTSAM_EXPORT TriangulationParameters
{
double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate 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 bool enableEPI; ///< if set to true, will refine triangulation using LM
/** /**
* if the landmark is triangulated at distance larger than this, * if the landmark is triangulated at distance larger than this,
* result is flagged as degenerate. * result is flagged as degenerate.
*/ */
double landmarkDistanceThreshold; // double landmarkDistanceThreshold; //
/** /**
* If this is nonnegative the we will check if the average reprojection error * If this is nonnegative the we will check if the average reprojection error
* is smaller than this threshold after triangulation, otherwise result is * is smaller than this threshold after triangulation, otherwise result is
* flagged as degenerate. * flagged as degenerate.
*/ */
double dynamicOutlierRejectionThreshold; double dynamicOutlierRejectionThreshold;
/** /**
* Constructor * Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate * @param rankTol tolerance used to check if point triangulation is degenerate
* @param enableEPI if true refine triangulation with embedded LM iterations * @param enableEPI if true refine triangulation with embedded LM iterations
@ -332,173 +351,194 @@ struct GTSAM_EXPORT TriangulationParameters {
* @param dynamicOutlierRejectionThreshold or if average error larger than this * @param dynamicOutlierRejectionThreshold or if average error larger than this
* *
*/ */
TriangulationParameters(const double _rankTolerance = 1.0, TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1) : double _dynamicOutlierRejectionThreshold = -1) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
rankTolerance(_rankTolerance), enableEPI(_enableEPI), // landmarkDistanceThreshold(_landmarkDistanceThreshold), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), // dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold)
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { {
} }
// stream to output // stream to output
friend std::ostream &operator<<(std::ostream &os, friend std::ostream &operator<<(std::ostream &os,
const TriangulationParameters& p) { const TriangulationParameters &p)
os << "rankTolerance = " << p.rankTolerance << std::endl; {
os << "enableEPI = " << p.enableEPI << std::endl; os << "rankTolerance = " << p.rankTolerance << std::endl;
os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold os << "enableEPI = " << p.enableEPI << std::endl;
<< std::endl; os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
os << "dynamicOutlierRejectionThreshold = " << std::endl;
<< p.dynamicOutlierRejectionThreshold << std::endl; os << "dynamicOutlierRejectionThreshold = "
return os; << p.dynamicOutlierRejectionThreshold << std::endl;
} return os;
}
private: private:
/// Serialization function
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);
}
};
/// Serialization function /**
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);
}
};
/**
* TriangulationResult is an optional point, along with the reasons why it is invalid. * TriangulationResult is an optional point, along with the reasons why it is invalid.
*/ */
class TriangulationResult: public boost::optional<Point3> { class TriangulationResult : public boost::optional<Point3>
enum Status { {
VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT enum Status
}; {
Status status_; VALID,
TriangulationResult(Status s) : DEGENERATE,
status_(s) { BEHIND_CAMERA,
} OUTLIER,
public: FAR_POINT
};
Status status_;
TriangulationResult(Status s) : status_(s)
{
}
/** public:
/**
* Default constructor, only for serialization * Default constructor, only for serialization
*/ */
TriangulationResult() {} TriangulationResult() {}
/** /**
* Constructor * Constructor
*/ */
TriangulationResult(const Point3& p) : TriangulationResult(const Point3 &p) : status_(VALID)
status_(VALID) { {
reset(p); reset(p);
} }
static TriangulationResult Degenerate() { static TriangulationResult Degenerate()
return TriangulationResult(DEGENERATE); {
} return TriangulationResult(DEGENERATE);
static TriangulationResult Outlier() { }
return TriangulationResult(OUTLIER); static TriangulationResult Outlier()
} {
static TriangulationResult FarPoint() { return TriangulationResult(OUTLIER);
return TriangulationResult(FAR_POINT); }
} static TriangulationResult FarPoint()
static TriangulationResult BehindCamera() { {
return TriangulationResult(BEHIND_CAMERA); return TriangulationResult(FAR_POINT);
} }
bool valid() const { static TriangulationResult BehindCamera()
return status_ == VALID; {
} return TriangulationResult(BEHIND_CAMERA);
bool degenerate() const { }
return status_ == DEGENERATE; bool valid() const
} {
bool outlier() const { return status_ == VALID;
return status_ == OUTLIER; }
} bool degenerate() const
bool farPoint() const { {
return status_ == FAR_POINT; return status_ == DEGENERATE;
} }
bool behindCamera() const { bool outlier() const
return status_ == BEHIND_CAMERA; {
} return status_ == OUTLIER;
// stream to output }
friend std::ostream &operator<<(std::ostream &os, bool farPoint() const
const TriangulationResult& result) { {
if (result) return status_ == FAR_POINT;
os << "point = " << *result << std::endl; }
else bool behindCamera() const
os << "no point, status = " << result.status_ << std::endl; {
return os; return status_ == BEHIND_CAMERA;
} }
// stream to output
private: friend std::ostream &operator<<(std::ostream &os,
const TriangulationResult &result)
/// Serialization function {
friend class boost::serialization::access; if (result)
template<class ARCHIVE> os << "point = " << *result << std::endl;
void serialize(ARCHIVE & ar, const unsigned int version) { else
ar & BOOST_SERIALIZATION_NVP(status_); os << "no point, status = " << result.status_ << std::endl;
} return os;
};
/// triangulateSafe: extensive checking of the outcome
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);
// 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 Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(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 private:
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>; /// Serialization function
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>; friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int version)
{
ar &BOOST_SERIALIZATION_NVP(status_);
}
};
} // \namespace gtsam /// triangulateSafe: extensive checking of the outcome
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);
// 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 Point2 &zi = measured.at(i);
Point2 reprojectionError(camera.project(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>>;
} // namespace gtsam

5372
gtsam/gtsam.i
View File

File diff suppressed because it is too large Load Diff

10
python/gtsam/preamble.h
View File

@ -5,10 +5,10 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
#else #else
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
#endif #endif
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >); PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >); PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);

18
python/gtsam/specializations.h
View File

@ -1,17 +1,17 @@
// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html // Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
// These are required to save one copy operation on Python calls // These are required to save one copy operation on Python calls
#ifdef GTSAM_ALLOCATOR_TBB #ifdef GTSAM_ALLOCATOR_TBB
py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector"); py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>>(m_, "KeyVector");
#else #else
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector"); py::bind_vector<std::vector<gtsam::Key>>(m_, "KeyVector");
#endif #endif
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector"); py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>>(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector"); py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s"); py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>(m_, "BetweenFactorPose3s");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s"); py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>(m_, "BetweenFactorPose2s");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3"); py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>>(m_, "BinaryMeasurementsUnit3");
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap"); py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector"); py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap"); py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2"); py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler"); py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(m_, "CameraSetCal3Bundler");

14
python/gtsam/tests/test_Triangulation.py
View File

@ -14,11 +14,10 @@ import numpy as np
import gtsam as g import gtsam as g
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ from gtsam import Cal3_S2, Cal3Bundler, CameraSetCal3_S2,\
PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, \
Point2Vector, Pose3Vector, triangulatePoint3, \ Point3, Pose3, Point2Vector, Pose3Vector, Rot3, triangulatePoint3
CameraSetCal3_S2, CameraSetCal3Bundler
from numpy.core.records import array
class TestVisualISAMExample(GtsamTestCase): class TestVisualISAMExample(GtsamTestCase):
""" Tests for triangulation with shared and individual calibrations """ """ Tests for triangulation with shared and individual calibrations """
@ -48,7 +47,6 @@ class TestVisualISAMExample(GtsamTestCase):
Returns: Returns:
vector of measurements and cameras vector of measurements and cameras
""" """
cameras = [] cameras = []
measurements = Point2Vector() measurements = Point2Vector()
for k, pose in zip(cal_params, self.poses): for k, pose in zip(cal_params, self.poses):
@ -85,6 +83,8 @@ class TestVisualISAMExample(GtsamTestCase):
K2 = (1600, 1300, 0, 650, 440) K2 = (1600, 1300, 0, 650, 440)
measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2) measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2)
# convert list to CameraSet object
cameras = CameraSetCal3_S2() cameras = CameraSetCal3_S2()
for camera in camera_list: for camera in camera_list:
cameras.append(camera) cameras.append(camera)
@ -99,6 +99,8 @@ class TestVisualISAMExample(GtsamTestCase):
K2 = (1600, 0, 0, 650, 440) K2 = (1600, 0, 0, 650, 440)
measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2) measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2)
# convert list to CameraSet object
cameras = CameraSetCal3Bundler() cameras = CameraSetCal3Bundler()
for camera in camera_list: for camera in camera_list:
cameras.append(camera) cameras.append(camera)