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

Handled both degeneracies same way

release/4.3a0
dellaert 2015-03-04 23:34:26 -08:00
parent e6a90db2d5
commit 91f3cd9e63
1 changed files with 8 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/slam/SmartProjectionFactor.h
View File

@ -449,27 +449,15 @@ public:
if (result_) if (result_)
// All good, just use version in base class // All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_); return Base::totalReprojectionError(cameras, *result_);
else { else if (manageDegeneracy_) {
// if we don't want to manage the exceptions we discard the factor // Otherwise, manage the exceptions with rotation-only factors
if (!manageDegeneracy_)
return 0.0;
if (isPointBehindCamera()) { // if we want to manage the exceptions with rotation-only factors
throw std::runtime_error(
"SmartProjectionFactor::totalReprojectionError does not handle point behind camera yet");
}
if (isDegenerate()) {
// 3D parameterization of point at infinity
const Point2& z0 = this->measured_.at(0); const Point2& z0 = this->measured_.at(0);
result_ = TriangulationResult( result_ = TriangulationResult(
cameras.front().backprojectPointAtInfinity(z0)); cameras.front().backprojectPointAtInfinity(z0));
return Base::totalReprojectionErrorAtInfinity(cameras, *result_); return Base::totalReprojectionErrorAtInfinity(cameras, *result_);
} } else
// should not reach here. TODO use switch // if we don't want to manage the exceptions we discard the factor
throw std::runtime_error( return 0.0;
"SmartProjectionFactor::totalReprojectionError internal error");
}
} }
/** return the landmark */ /** return the landmark */