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

Now complete functional triangulateSafe

release/4.3a0
dellaert 2015-02-26 15:33:43 +01:00
parent 61ba2f080c
commit 262b42e829
2 changed files with 64 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
gtsam/geometry/PinholeSet.h
View File

@ -41,39 +41,44 @@ protected:
/// @name Triangulation parameters
/// @{
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
const double rankTolerance_; ///< threshold to decide whether triangulation is result.degenerate
const bool enableEPI_; ///< if set to true, will refine triangulation using LM
/**
* if the landmark is triangulated at distance larger than this,
* result is flagged as degenerate.
*/
const double landmarkDistanceThreshold_; //
/**
* If this is nonnegative the we will check if the average reprojection error
* is smaller than this threshold after triangulation, otherwise result is
* flagged as degenerate.
*/
const double dynamicOutlierRejectionThreshold_;
/// @}
mutable Point3 point_; ///< Current estimate of the 3D point
mutable bool degenerate_;
mutable bool cheiralityException_;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// @name Triangulation result
/// @{
struct Result {
Point3 point;
bool degenerate;
bool cheiralityException;
};
/// @}
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
*/
PinholeSet(const double rankTol = 1.0, const bool enableEPI = false,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
rankTolerance_(rankTol), enableEPI_(enableEPI), degenerate_(false), cheiralityException_(
false), landmarkDistanceThreshold_(landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
rankTolerance_(rankTol), enableEPI_(enableEPI), landmarkDistanceThreshold_(
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
dynamicOutlierRejectionThreshold) {
}
@ -81,17 +86,14 @@ public:
virtual ~PinholeSet() {
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "") const {
std::cout << s << "PinholeSet, z = \n";
std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
Base::print("");
/// @name Testable
/// @{
/// print
virtual void print(const std::string& s = "") const {
Base::print(s);
std::cout << s << "PinholeSet\n";
std::cout << "rankTolerance = " << rankTolerance_ << std::endl;
}
/// equals
@ -99,22 +101,28 @@ public:
return Base::equals(p, tol); // TODO all flags
}
/// @}
/// triangulateSafe
size_t triangulateSafe() const {
Result triangulateSafe(const std::vector<typename Base::Z>& measured) const {
Result result;
size_t m = this->size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true;
return m;
// if we have a single pose the corresponding factor is uninformative
if (m < 2) {
result.degenerate = true;
return result;
}
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
point_ = triangulatePoint3<CAMERA>(*this, this->measured_, rankTolerance_,
result.point = triangulatePoint3<CAMERA>(*this, measured, rankTolerance_,
enableEPI_);
degenerate_ = false;
cheiralityException_ = false;
result.degenerate = false;
result.cheiralityException = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
@ -122,81 +130,38 @@ public:
BOOST_FOREACH(const CAMERA& camera, *this) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
degenerate_ = true;
if (cameraTranslation.distance(result.point)
> landmarkDistanceThreshold_) {
result.degenerate = true;
break;
}
const Point2& zi = this->measured_.at(i);
const Point2& zi = measured.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
Point2 reprojectionError(camera.project(result.point) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
result.cheiralityException = true;
}
i += 1;
}
// we discard smart factors that have large reprojection error
if (dynamicOutlierRejectionThreshold_ > 0
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
degenerate_ = true;
result.degenerate = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel *this (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
degenerate_ = true;
cheiralityException_ = false;
result.degenerate = true;
result.cheiralityException = false;
} catch (TriangulationCheiralityException&) {
// point is behind one of the *this: can be the case of close-to-parallel *this or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true;
result.cheiralityException = true;
}
return m;
}
/// triangulate
bool triangulateForLinearize() const {
bool isDebug = false;
size_t nrCameras = this->triangulateSafe(*this);
if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) {
if (isDebug) {
std::cout
<< "createRegularImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
} else {
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
return true;
}
}
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
}
/** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const {
triangulateSafe(values);
return point_;
}
/** return the degenerate state */
inline bool isDegenerate() const {
return (cheiralityException_ || degenerate_);
}
/** return the cheirality status flag */
inline bool isPointBehindCamera() const {
return cheiralityException_;
return result;
}
private:

9
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -33,6 +33,7 @@ TEST(PinholeSet, Stereo) {
CalibratedCamera camera;
set.push_back(camera);
set.push_back(camera);
// set.print("set: ");
Point3 p(0, 0, 1);
EXPECT_LONGS_EQUAL(6, traits<CalibratedCamera>::dimension);
@ -59,6 +60,10 @@ TEST(PinholeSet, Stereo) {
EXPECT(assert_equal(F1, F[0]));
EXPECT(assert_equal(F1, F[1]));
EXPECT(assert_equal(actualE, E));
// Instantiate triangulateSafe
// TODO triangulation does not work yet for CalibratedCamera
// PinholeSet<CalibratedCamera>::Result actual = set.triangulateSafe(z);
}
/* ************************************************************************* */
@ -120,6 +125,10 @@ TEST(PinholeSet, Pinhole) {
camera.backprojectPointAtInfinity(Point2())));
actualV = set.reprojectionErrorAtInfinity(p, measured);
EXPECT(assert_equal(expectedV, actualV));
// Instantiate triangulateSafe
PinholeSet<Camera>::Result actual = set.triangulateSafe(z);
CHECK(actual.degenerate);
}
/* ************************************************************************* */