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

166
gtsam/geometry/triangulation.h
View File

@ -27,22 +27,25 @@
#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: public:
TriangulationUnderconstrainedException() : TriangulationUnderconstrainedException() : std::runtime_error("Triangulation Underconstrained Exception.")
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: public:
TriangulationCheiralityException() : TriangulationCheiralityException() : std::runtime_error(
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.") { {
} }
}; };
@ -82,13 +85,15 @@ 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 values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++)
{
const Pose3 &pose_i = poses[i]; const Pose3 &pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera; typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal); Camera camera_i(pose_i, sharedCal);
@ -111,13 +116,15 @@ 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 values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create( static SharedNoiseModel unit(noiseModel::Unit::Create(
traits<typename CAMERA::Measurement>::dimension)); traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++)
{
const CAMERA &camera_i = cameras[i]; const CAMERA &camera_i = cameras[i];
graph.emplace_shared<TriangulationFactor<CAMERA>> // graph.emplace_shared<TriangulationFactor<CAMERA>> //
(camera_i, measurements[i], unit, landmarkKey); (camera_i, measurements[i], unit, landmarkKey);
@ -146,7 +153,8 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
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;
@ -167,7 +175,8 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
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;
@ -186,15 +195,19 @@ Point3 triangulateNonlinear(
* @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 { Matrix34 operator()(const Pose3 &pose) const
{
return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
} }
private: private:
const Matrix3 K_; const Matrix3 K_;
public: public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
@ -215,7 +228,8 @@ 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)
@ -237,7 +251,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
#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); const Point3 &p_local = pose.transformTo(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
@ -263,7 +278,8 @@ 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);
@ -285,7 +301,8 @@ Point3 triangulatePoint3(
#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); const Point3 &p_local = camera.pose().transformTo(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
@ -300,12 +317,14 @@ 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>> // return triangulatePoint3<PinholeCamera<CALIBRATION>> //
(cameras, measurements, rank_tol, optimize); (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)
@ -334,15 +353,16 @@ struct GTSAM_EXPORT TriangulationParameters {
*/ */
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 << "rankTolerance = " << p.rankTolerance << std::endl;
os << "enableEPI = " << p.enableEPI << std::endl; os << "enableEPI = " << p.enableEPI << std::endl;
os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
@ -353,11 +373,11 @@ struct GTSAM_EXPORT TriangulationParameters {
} }
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE &ar, const unsigned int version)
{
ar &BOOST_SERIALIZATION_NVP(rankTolerance); ar &BOOST_SERIALIZATION_NVP(rankTolerance);
ar &BOOST_SERIALIZATION_NVP(enableEPI); ar &BOOST_SERIALIZATION_NVP(enableEPI);
ar &BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); ar &BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
@ -368,16 +388,22 @@ private:
/** /**
* 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
{
VALID,
DEGENERATE,
BEHIND_CAMERA,
OUTLIER,
FAR_POINT
}; };
Status status_; Status status_;
TriangulationResult(Status s) : TriangulationResult(Status s) : status_(s)
status_(s) { {
} }
public:
public:
/** /**
* Default constructor, only for serialization * Default constructor, only for serialization
*/ */
@ -386,40 +412,50 @@ public:
/** /**
* 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() { static TriangulationResult Outlier()
{
return TriangulationResult(OUTLIER); return TriangulationResult(OUTLIER);
} }
static TriangulationResult FarPoint() { static TriangulationResult FarPoint()
{
return TriangulationResult(FAR_POINT); return TriangulationResult(FAR_POINT);
} }
static TriangulationResult BehindCamera() { static TriangulationResult BehindCamera()
{
return TriangulationResult(BEHIND_CAMERA); return TriangulationResult(BEHIND_CAMERA);
} }
bool valid() const { bool valid() const
{
return status_ == VALID; return status_ == VALID;
} }
bool degenerate() const { bool degenerate() const
{
return status_ == DEGENERATE; return status_ == DEGENERATE;
} }
bool outlier() const { bool outlier() const
{
return status_ == OUTLIER; return status_ == OUTLIER;
} }
bool farPoint() const { bool farPoint() const
{
return status_ == FAR_POINT; return status_ == FAR_POINT;
} }
bool behindCamera() const { bool behindCamera() const
{
return status_ == BEHIND_CAMERA; return status_ == BEHIND_CAMERA;
} }
// stream to output // stream to output
friend std::ostream &operator<<(std::ostream &os, friend std::ostream &operator<<(std::ostream &os,
const TriangulationResult& result) { const TriangulationResult &result)
{
if (result) if (result)
os << "point = " << *result << std::endl; os << "point = " << *result << std::endl;
else else
@ -428,11 +464,11 @@ public:
} }
private: private:
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE &ar, const unsigned int version)
{
ar &BOOST_SERIALIZATION_NVP(status_); ar &BOOST_SERIALIZATION_NVP(status_);
} }
}; };
@ -441,7 +477,8 @@ private:
template <class CAMERA> template <class CAMERA>
TriangulationResult triangulateSafe(const CameraSet<CAMERA> &cameras, TriangulationResult triangulateSafe(const CameraSet<CAMERA> &cameras,
const typename CAMERA::MeasurementVector &measured, const typename CAMERA::MeasurementVector &measured,
const TriangulationParameters& params) { const TriangulationParameters &params)
{
size_t m = cameras.size(); size_t m = cameras.size();
@ -450,18 +487,18 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
return TriangulationResult::Degenerate(); return TriangulationResult::Degenerate();
else else
// We triangulate the 3D position of the landmark // We triangulate the 3D position of the landmark
try { try
{
Point3 point = triangulatePoint3<CAMERA>(cameras, measured, Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
params.rankTolerance, params.enableEPI); params.rankTolerance, params.enableEPI);
// Check landmark distance and re-projection errors to avoid outliers // Check landmark distance and re-projection errors to avoid outliers
size_t i = 0; size_t i = 0;
double maxReprojError = 0.0; double maxReprojError = 0.0;
for(const CAMERA& camera: cameras) { for (const CAMERA &camera : cameras)
{
const Pose3 &pose = camera.pose(); const Pose3 &pose = camera.pose();
if (params.landmarkDistanceThreshold > 0 if (params.landmarkDistanceThreshold > 0 && distance3(pose.translation(), point) > params.landmarkDistanceThreshold)
&& distance3(pose.translation(), point)
> params.landmarkDistanceThreshold)
return TriangulationResult::FarPoint(); return TriangulationResult::FarPoint();
#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
@ -471,7 +508,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
return TriangulationResult::BehindCamera(); return TriangulationResult::BehindCamera();
#endif #endif
// Check reprojection error // Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0) { if (params.dynamicOutlierRejectionThreshold > 0)
{
const Point2 &zi = measured.at(i); const Point2 &zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi); Point2 reprojectionError(camera.project(point) - zi);
maxReprojError = std::max(maxReprojError, reprojectionError.norm()); maxReprojError = std::max(maxReprojError, reprojectionError.norm());
@ -479,18 +517,21 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
i += 1; i += 1;
} }
// Flag as degenerate if average reprojection error is too large // Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0 if (params.dynamicOutlierRejectionThreshold > 0 && maxReprojError > params.dynamicOutlierRejectionThreshold)
&& maxReprojError > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Outlier(); return TriangulationResult::Outlier();
// all good! // all good!
return TriangulationResult(point); return TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) { }
catch (TriangulationUnderconstrainedException &)
{
// This exception is thrown if // 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 // 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) // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
return TriangulationResult::Degenerate(); return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) { }
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(); return TriangulationResult::BehindCamera();
} }
@ -500,5 +541,4 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>; using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>; using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
} // \namespace gtsam } // namespace gtsam

516
gtsam/gtsam.i
View File

File diff suppressed because it is too large Load Diff

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)