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

Removed a lot of obsolete code

release/4.3a0
dellaert 2015-02-26 15:01:25 +01:00
parent 0a75da9858
commit 3b808e644b
1 changed files with 51 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

263
gtsam/geometry/PinholeSet.h
View File

@ -38,22 +38,19 @@ private:
protected: protected:
// Some triangulation parameters /// @name Triangulation parameters
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ /// @{
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
const bool enableEPI_; ///< if set to true, will refine triangulation using LM const bool enableEPI_; ///< if set to true, will refine triangulation using LM
/// @}
mutable Point3 point_; ///< Current estimate of the 3D point mutable Point3 point_; ///< Current estimate of the 3D point
mutable bool degenerate_; mutable bool degenerate_;
mutable bool cheiralityException_; mutable bool cheiralityException_;
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
double landmarkDistanceThreshold_; // if the landmark is triangulated at a double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate // distance larger than that the factor is considered degenerate
@ -66,9 +63,6 @@ public:
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a set of cameras
typedef CameraSet<CAMERA> Cameras;
/** /**
* Constructor * Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate * @param rankTol tolerance used to check if point triangulation is degenerate
@ -78,10 +72,8 @@ public:
PinholeSet(const double rankTol = 1.0, const bool enableEPI = false, PinholeSet(const double rankTol = 1.0, const bool enableEPI = false,
double landmarkDistanceThreshold = 1e10, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) : double dynamicOutlierRejectionThreshold = -1) :
rankTolerance_(rankTol), retriangulationThreshold_(1e-5), enableEPI_( rankTolerance_(rankTol), enableEPI_(enableEPI), degenerate_(false), cheiralityException_(
enableEPI), degenerate_(false), cheiralityException_(false), throwCheirality_( false), landmarkDistanceThreshold_(landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
false), verboseCheirality_(false), landmarkDistanceThreshold_(
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
dynamicOutlierRejectionThreshold) { dynamicOutlierRejectionThreshold) {
} }
@ -107,111 +99,67 @@ public:
return Base::equals(p, tol); // TODO all flags return Base::equals(p, tol); // TODO all flags
} }
/// Check if the new linearization point_ is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
size_t m = cameras.size();
bool retriangulate = false;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
if (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true;
if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
retriangulationThreshold_)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
}
}
if (retriangulate) { // we store the current poses used for triangulation
cameraPosesTriangulation_.clear();
cameraPosesTriangulation_.reserve(m);
for (size_t i = 0; i < m; i++)
// cameraPosesTriangulation_[i] = cameras[i].pose();
cameraPosesTriangulation_.push_back(cameras[i].pose());
}
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
/// triangulateSafe /// triangulateSafe
size_t triangulateSafe(const Values& values) const { size_t triangulateSafe() const {
return triangulateSafe(this->cameras(values));
}
/// triangulateSafe size_t m = this->size();
size_t triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true; degenerate_ = true;
return m; return m;
} }
bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate) { // We triangulate the 3D position of the landmark
// We triangulate the 3D position of the landmark try {
try { // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; point_ = triangulatePoint3<CAMERA>(*this, this->measured_, rankTolerance_,
point_ = triangulatePoint3<CAMERA>(cameras, this->measured_, enableEPI_);
rankTolerance_, enableEPI_); degenerate_ = false;
degenerate_ = false; cheiralityException_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers // Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0; double totalReprojError = 0.0;
size_t i = 0; size_t i = 0;
BOOST_FOREACH(const CAMERA& camera, cameras) { BOOST_FOREACH(const CAMERA& camera, *this) {
Point3 cameraTranslation = camera.pose().translation(); Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away // we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
degenerate_ = true;
break;
}
const Point2& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
// we discard smart factors that have large reprojection error
if (dynamicOutlierRejectionThreshold_ > 0
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
degenerate_ = true; degenerate_ = true;
break;
} catch (TriangulationUnderconstrainedException&) { }
// if TriangulationUnderconstrainedException can be const Point2& zi = this->measured_.at(i);
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before try {
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) Point2 reprojectionError(camera.project(point_) - zi);
// in the second case we want to use a rotation-only smart factor totalReprojError += reprojectionError.vector().norm();
degenerate_ = true; } catch (CheiralityException) {
cheiralityException_ = false; cheiralityException_ = true;
} catch (TriangulationCheiralityException&) { }
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers i += 1;
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true;
} }
// we discard smart factors that have large reprojection error
if (dynamicOutlierRejectionThreshold_ > 0
&& totalReprojError / m > dynamicOutlierRejectionThreshold_)
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;
} 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;
} }
return m; return m;
} }
/// triangulate /// triangulate
bool triangulateForLinearize(const Cameras& cameras) const { bool triangulateForLinearize() const {
bool isDebug = false; bool isDebug = false;
size_t nrCameras = this->triangulateSafe(cameras); size_t nrCameras = this->triangulateSafe(*this);
if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) { if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) {
if (isDebug) { if (isDebug) {
@ -230,103 +178,6 @@ public:
} }
} }
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& cameras) const {
Values valuesFactor;
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_))
return false;
// 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;
if (this->degenerate_) {
std::cout << "PinholeSet: this is not ready" << std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
}
return true;
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
cameras.project2(point_, boost::none, E);
return nonDegenerate;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
return Base::reprojectionError(cameras, point_);
else
return zero(cameras.size() * 2);
}
/**
* Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
size_t nrCameras;
if (externalPoint) {
nrCameras = this->keys_.size();
point_ = *externalPoint;
degenerate_ = false;
cheiralityException_ = false;
} else {
nrCameras = this->triangulateSafe(cameras);
}
if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) {
// if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
return 0.0;
}
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
std::cout
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
<< std::endl;
this->degenerate_ = true;
}
if (this->degenerate_) {
// return 0.0; // TODO: this maybe should be zero?
std::cout
<< "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
<< std::endl;
// 3D parameterization of point at infinity
const Point2& zi = this->measured_.at(0);
this->point_ = cameras.front().backprojectPointAtInfinity(zi);
return Base::totalReprojectionErrorAtInfinity(cameras, this->point_);
} else {
// Just use version in base class
return Base::totalReprojectionError(cameras, point_);
}
}
/** return the landmark */ /** return the landmark */
boost::optional<Point3> point() const { boost::optional<Point3> point() const {
return point_; return point_;
@ -348,16 +199,6 @@ public:
return cheiralityException_; return cheiralityException_;
} }
/** return cheirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const {
return throwCheirality_;
}
private: private:
/// Serialization function /// Serialization function
@ -365,8 +206,6 @@ private:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
} }
}; };