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

adding 2 more status items for triangulation results: outliers (dynamicOutlierRejection triggered) and farPoint (maxLandmarkDistance triggered)

release/4.3a0
lcarlone 2016-06-29 12:26:21 -04:00
parent eb9cda7c92
commit 66d42a336f
1 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/triangulation.h
View File

@ -382,7 +382,7 @@ private:
*/
class TriangulationResult: public boost::optional<Point3> {
enum Status {
VALID, DEGENERATE, BEHIND_CAMERA
VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
};
Status status_;
TriangulationResult(Status s) :
@ -405,12 +405,27 @@ public:
static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE);
}
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;
}
@ -459,7 +474,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
if (params.landmarkDistanceThreshold > 0
&& distance(pose.translation(), point)
> params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate();
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
@ -478,7 +493,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
// Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate();
return TriangulationResult::Outlier();
// all good!
return TriangulationResult(point);