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

Revert "code formatted" 

This reverts commit 8be6890b20.
release/4.3a0
Varun Agrawal 2020-12-05 18:08:45 -05:00
parent 858884f1e7
commit b24f943c36
5 changed files with 3116 additions and 3311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

166
gtsam/geometry/triangulation.h
View File

@ -27,25 +27,22 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam
{
namespace gtsam {
/// 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() : 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
{
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.") {
}
};
@ -85,15 +82,13 @@ namespace gtsam
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, Key landmarkKey,
const Point3 &initialEstimate)
{
const Point3& initialEstimate) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
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];
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
@ -116,15 +111,13 @@ namespace gtsam
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
const Point3 &initialEstimate)
{
const Point3& initialEstimate) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(
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];
graph.emplace_shared<TriangulationFactor<CAMERA> > //
(camera_i, measurements[i], unit, landmarkKey);
@ -153,8 +146,7 @@ namespace gtsam
template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector &measurements, const Point3 &initialEstimate)
{
const Point2Vector& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values
Values values;
@ -175,8 +167,7 @@ namespace gtsam
template<class CAMERA>
Point3 triangulateNonlinear(
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
Values values;
@ -195,19 +186,15 @@ namespace gtsam
* @return Returns a Matrix34
*/
template<class CALIBRATION>
struct CameraProjectionMatrix
{
CameraProjectionMatrix(const CALIBRATION &calibration) : K_(calibration.K())
{
struct CameraProjectionMatrix {
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);
}
private:
const Matrix3 K_;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
@ -228,8 +215,7 @@ namespace gtsam
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false)
{
bool optimize = false) {
assert(poses.size() == measurements.size());
if (poses.size() < 2)
@ -251,8 +237,7 @@ namespace gtsam
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// 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)
throw(TriangulationCheiralityException());
@ -278,8 +263,7 @@ namespace gtsam
Point3 triangulatePoint3(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
bool optimize = false)
{
bool optimize = false) {
size_t m = cameras.size();
assert(measurements.size() == m);
@ -301,8 +285,7 @@ namespace gtsam
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// 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)
throw(TriangulationCheiralityException());
@ -317,14 +300,12 @@ namespace gtsam
Point3 triangulatePoint3(
const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
const Point2Vector& measurements, double rank_tol = 1e-9,
bool optimize = false)
{
bool optimize = false) {
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
///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance)
@ -353,16 +334,15 @@ namespace gtsam
*/
TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
double _dynamicOutlierRejectionThreshold = -1) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
double _dynamicOutlierRejectionThreshold = -1) :
rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
landmarkDistanceThreshold(_landmarkDistanceThreshold), //
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold)
{
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
}
// stream to output
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 << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
@ -373,11 +353,11 @@ namespace gtsam
}
private:
/// Serialization function
friend class boost::serialization::access;
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(enableEPI);
ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
@ -388,22 +368,16 @@ namespace gtsam
/**
* TriangulationResult is an optional point, along with the reasons why it is invalid.
*/
class TriangulationResult : public boost::optional<Point3>
{
enum Status
{
VALID,
DEGENERATE,
BEHIND_CAMERA,
OUTLIER,
FAR_POINT
class TriangulationResult: public boost::optional<Point3> {
enum Status {
VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
};
Status status_;
TriangulationResult(Status s) : status_(s)
{
TriangulationResult(Status s) :
status_(s) {
}
public:
/**
* Default constructor, only for serialization
*/
@ -412,50 +386,40 @@ namespace gtsam
/**
* Constructor
*/
TriangulationResult(const Point3 &p) : status_(VALID)
{
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);
}
static TriangulationResult Degenerate()
{
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
static TriangulationResult Outlier()
{
static TriangulationResult Outlier() {
return TriangulationResult(OUTLIER);
}
static TriangulationResult FarPoint()
{
static TriangulationResult FarPoint() {
return TriangulationResult(FAR_POINT);
}
static TriangulationResult BehindCamera()
{
static TriangulationResult BehindCamera() {
return TriangulationResult(BEHIND_CAMERA);
}
bool valid() const
{
bool valid() const {
return status_ == VALID;
}
bool degenerate() const
{
bool degenerate() const {
return status_ == DEGENERATE;
}
bool outlier() const
{
bool outlier() const {
return status_ == OUTLIER;
}
bool farPoint() const
{
bool farPoint() const {
return status_ == FAR_POINT;
}
bool behindCamera() const
{
bool behindCamera() const {
return status_ == BEHIND_CAMERA;
}
// stream to output
friend std::ostream &operator<<(std::ostream &os,
const TriangulationResult &result)
{
const TriangulationResult& result) {
if (result)
os << "point = " << *result << std::endl;
else
@ -464,11 +428,11 @@ namespace gtsam
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int version)
{
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(status_);
}
};
@ -477,8 +441,7 @@ namespace gtsam
template<class CAMERA>
TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters &params)
{
const TriangulationParameters& params) {
size_t m = cameras.size();
@ -487,18 +450,18 @@ namespace gtsam
return TriangulationResult::Degenerate();
else
// We triangulate the 3D position of the landmark
try
{
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)
{
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
@ -508,8 +471,7 @@ namespace gtsam
return TriangulationResult::BehindCamera();
#endif
// Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0)
{
if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi);
maxReprojError = std::max(maxReprojError, reprojectionError.norm());
@ -517,21 +479,18 @@ namespace gtsam
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 &)
{
} 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 &)
{
} 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();
}
@ -541,4 +500,5 @@ namespace gtsam
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
} // namespace gtsam
} // \namespace gtsam

508
gtsam/gtsam.i
View File

File diff suppressed because it is too large Load Diff

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

@ -14,10 +14,11 @@ import numpy as np
import gtsam as g
from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, CameraSetCal3_S2,\
CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, \
Point3, Pose3, Point2Vector, Pose3Vector, Rot3, triangulatePoint3
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \
Point2Vector, Pose3Vector, triangulatePoint3, \
CameraSetCal3_S2, CameraSetCal3Bundler
from numpy.core.records import array
class TestVisualISAMExample(GtsamTestCase):
""" Tests for triangulation with shared and individual calibrations """