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> { class TriangulationResult: public boost::optional<Point3> {
enum Status { enum Status {
VALID, DEGENERATE, BEHIND_CAMERA VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
}; };
Status status_; Status status_;
TriangulationResult(Status s) : TriangulationResult(Status s) :
@ -405,12 +405,27 @@ public:
static TriangulationResult Degenerate() { static TriangulationResult Degenerate() {
return TriangulationResult(DEGENERATE); return TriangulationResult(DEGENERATE);
} }
static TriangulationResult Outlier() {
return TriangulationResult(OUTLIER);
}
static TriangulationResult FarPoint() {
return TriangulationResult(FAR_POINT);
}
static TriangulationResult BehindCamera() { static TriangulationResult BehindCamera() {
return TriangulationResult(BEHIND_CAMERA); return TriangulationResult(BEHIND_CAMERA);
} }
bool valid() const {
return status_ == VALID;
}
bool degenerate() const { bool degenerate() const {
return status_ == DEGENERATE; return status_ == DEGENERATE;
} }
bool outlier() const {
return status_ == OUTLIER;
}
bool farPoint() const {
return status_ == FAR_POINT;
}
bool behindCamera() const { bool behindCamera() const {
return status_ == BEHIND_CAMERA; return status_ == BEHIND_CAMERA;
} }
@ -459,7 +474,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
if (params.landmarkDistanceThreshold > 0 if (params.landmarkDistanceThreshold > 0
&& distance(pose.translation(), point) && distance(pose.translation(), point)
> params.landmarkDistanceThreshold) > params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate(); 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
// Only needed if this was not yet handled by exception // 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 // Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0 if (params.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params.dynamicOutlierRejectionThreshold) && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Degenerate(); return TriangulationResult::Outlier();
// all good! // all good!
return TriangulationResult(point); return TriangulationResult(point);