Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Wrap triangulateSafe #1194

Merged
merged 9 commits into from
May 11, 2022
Merged

Wrap triangulateSafe #1194

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
b5e9dc0
Make status a public field
dellaert May 11, 2022
58cbdcd
Wrap TriangulationResult
dellaert May 11, 2022
e60f4ac
Be consistent in wrapping templated triangulation versions
dellaert May 11, 2022
02828e2
Fix unrelated warning
dellaert May 11, 2022
8333c8f
Wrapped safe triangulation versions
dellaert May 11, 2022
287ea28
Camera sets
dellaert May 11, 2022
db371c2
Added safe triangulation tests
dellaert May 11, 2022
7bd0ae7
Fix pybind11 issue
dellaert May 11, 2022
b78a4e6
Include all constructors, methods
dellaert May 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
113 changes: 99 additions & 14 deletions gtsam/geometry/geometry.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1040,7 +1040,11 @@ class Similarity3 {
double scale() const;
};

template <T>
template <T = {gtsam::PinholeCameraCal3_S2,
gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified}>
class CameraSet {
CameraSet();

Expand Down Expand Up @@ -1088,58 +1092,139 @@ class StereoCamera {
};

#include <gtsam/geometry/triangulation.h>
class TriangulationResult {
enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
Status status;
const gtsam::Point3& get() const;
};

class TriangulationParameters {
double rankTolerance;
bool enableEPI;
double landmarkDistanceThreshold;
double dynamicOutlierRejectionThreshold;
SharedNoiseModel noiseModel;
TriangulationParameters(const double rankTolerance = 1.0,
const bool enableEPI = false,
double landmarkDistanceThreshold = -1,
double dynamicOutlierRejectionThreshold = -1,
const gtsam::SharedNoiseModel& noiseModel = nullptr);
};

// Templates appear not yet supported for free functions - issue raised at
// borglab/wrap#14 to add support

// Cal3_S2 versions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::TriangulationResult triangulateSafe(
const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::TriangulationParameters& params);

// Cal3DS2 versions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3DS2* sharedCal,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal,
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3DS2* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::TriangulationResult triangulateSafe(
const gtsam::CameraSetCal3DS2& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::TriangulationParameters& params);

// Cal3Bundler versions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::TriangulationResult triangulateSafe(
const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::TriangulationParameters& params);

// Cal3Fisheye versions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Fisheye* sharedCal,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal,
gtsam::Cal3Fisheye* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3DS2* sharedCal,
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::TriangulationResult triangulateSafe(
const gtsam::CameraSetCal3Fisheye& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::TriangulationParameters& params);

// Cal3Unified versions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Unified* sharedCal,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal,
gtsam::Cal3Unified* sharedCal,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::Point3& initialEstimate);
gtsam::TriangulationResult triangulateSafe(
const gtsam::CameraSetCal3Unified& cameras,
const gtsam::Point2Vector& measurements,
const gtsam::TriangulationParameters& params);



#include <gtsam/geometry/BearingRange.h>
template <POSE, POINT, BEARING, RANGE>
Expand Down
66 changes: 26 additions & 40 deletions gtsam/geometry/triangulation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#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>
Expand Down Expand Up @@ -510,18 +511,18 @@ struct GTSAM_EXPORT TriangulationParameters {
};

/**
* 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> {
enum Status {
VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
};
Status status_;
TriangulationResult(Status s) :
status_(s) {
}
public:
class TriangulationResult : public boost::optional<Point3> {
public:
enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
Status status;

private:
TriangulationResult(Status s) : status(s) {}

public:
/**
* Default constructor, only for serialization
*/
Expand All @@ -530,54 +531,38 @@ class TriangulationResult: public boost::optional<Point3> {
/**
* Constructor
*/
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);
}
TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
static TriangulationResult Outlier() {
return TriangulationResult(OUTLIER);
}
static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
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;
}
bool farPoint() const {
return status_ == FAR_POINT;
}
bool behindCamera() const {
return status_ == BEHIND_CAMERA;
}
bool valid() const { return status == VALID; }
bool degenerate() const { return status == DEGENERATE; }
bool outlier() const { return status == OUTLIER; }
bool farPoint() const { return status == FAR_POINT; }
bool behindCamera() const { return status == BEHIND_CAMERA; }
// 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
os << "no point, status = " << result.status_ << std::endl;
os << "no point, status = " << result.status << 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(status_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int version) {
ar& BOOST_SERIALIZATION_NVP(status);
}
};

Expand Down Expand Up @@ -644,6 +629,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
Expand Down
2 changes: 0 additions & 2 deletions gtsam/linear/SubgraphBuilder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -337,7 +337,6 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
DSFVector dsf(n);

size_t count = 0;
double sum = 0.0;
for (const size_t index : sortedIndices) {
const GaussianFactor &gf = *gfg[index];
const auto keys = gf.keys();
Expand All @@ -347,7 +346,6 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
if (dsf.find(u) != dsf.find(v)) {
dsf.merge(u, v);
treeIndices.push_back(index);
sum += weights[index];
if (++count == n - 1) break;
}
}
Expand Down
2 changes: 2 additions & 0 deletions python/gtsam/specializations/geometry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@ py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3DS2>>>(
m_, "CameraSetCal3DS2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(
m_, "CameraSetCal3Bundler");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>>(
Expand Down
Loading