Skip to content

Commit

Permalink
Merge pull request #1194 from borglab/feature/saveTriangulation
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert authored May 11, 2022
2 parents 9c88249 + b78a4e6 commit 953aa9f
Show file tree
Hide file tree
Showing 5 changed files with 200 additions and 70 deletions.
117 changes: 104 additions & 13 deletions gtsam/geometry/geometry.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1088,58 +1088,149 @@ class StereoCamera {
};

#include <gtsam/geometry/triangulation.h>
class TriangulationResult {
enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
Status status;
TriangulationResult(const gtsam::Point3& p);
const gtsam::Point3& get() const;
static TriangulationResult Degenerate();
static TriangulationResult Outlier();
static TriangulationResult FarPoint();
static TriangulationResult BehindCamera();
bool valid() const;
bool degenerate() const;
bool outlier() const;
bool farPoint() const;
bool behindCamera() 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
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
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
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

0 comments on commit 953aa9f

Please sign in to comment.