From 0a75da98584bf07c07ebe50c41097212649cc577 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 26 Feb 2015 14:50:52 +0100 Subject: [PATCH] PinholeSet first draft: a CameraSet that assumes PinholeBase-derived CAMERA: knows how to triangulate. First draft is still imperative, with mutable point_, and that might change. --- .cproject | 8 + gtsam/geometry/CameraSet.h | 4 +- gtsam/geometry/PinholeSet.h | 381 ++++++++++++++++++++++++ gtsam/geometry/tests/testPinholeSet.cpp | 131 ++++++++ 4 files changed, 522 insertions(+), 2 deletions(-) create mode 100644 gtsam/geometry/PinholeSet.h create mode 100644 gtsam/geometry/tests/testPinholeSet.cpp diff --git a/.cproject b/.cproject index e190d8f65..94e8c3a50 100644 --- a/.cproject +++ b/.cproject @@ -1043,6 +1043,14 @@ true true + + make + -j4 + testPinholeSet.run + true + true + true + make -j2 diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 2192c38b9..179ec9c50 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -78,7 +78,7 @@ public: } /// equals - virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + bool equals(const CameraSet& p, double tol = 1e-9) const { if (this->size() != p.size()) return false; bool camerasAreEqual = true; @@ -143,7 +143,7 @@ public: Vector reprojectionError(const Point3& point, const std::vector& measured, boost::optional F = boost::none, // boost::optional E = boost::none) const { - return ErrorVector(project2(point,F,E), measured); + return ErrorVector(project2(point, F, E), measured); } /// Calculate vector of re-projection errors, from point at infinity diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h new file mode 100644 index 000000000..c950a9150 --- /dev/null +++ b/gtsam/geometry/PinholeSet.h @@ -0,0 +1,381 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PinholeSet.h + * @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera + * @author Frank Dellaert + * @author Luca Carlone + * @author Zsolt Kira + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * PinholeSet: triangulates point and keeps an estimate of it around. + */ +template +class PinholeSet: public CameraSet { + +private: + typedef CameraSet Base; + typedef PinholeSet This; + +protected: + + // Some 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 cameraPosesTriangulation_; ///< current triangulation poses + + const bool enableEPI_; ///< if set to true, will refine triangulation using LM + + mutable Point3 point_; ///< Current estimate of the 3D point + + mutable bool degenerate_; + 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 + // 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 shared_ptr; + + /// shorthand for a set of cameras + typedef CameraSet Cameras; + + /** + * 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), retriangulationThreshold_(1e-5), enableEPI_( + enableEPI), degenerate_(false), cheiralityException_(false), throwCheirality_( + false), verboseCheirality_(false), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + dynamicOutlierRejectionThreshold) { + } + + /** Virtual destructor */ + 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(""); + } + + /// equals + bool equals(const PinholeSet& p, double tol = 1e-9) const { + 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 + size_t triangulateSafe(const Values& values) const { + return triangulateSafe(this->cameras(values)); + } + + /// triangulateSafe + 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 + degenerate_ = true; + return m; + } + bool retriangulate = decideIfTriangulate(cameras); + + if (retriangulate) { + // We triangulate the 3D position of the landmark + try { + // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + point_ = triangulatePoint3(cameras, this->measured_, + rankTolerance_, enableEPI_); + degenerate_ = false; + cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i = 0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + 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; + + } 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 cameras (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 cameras: can be the case of close-to-parallel cameras 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; + } + + /// triangulate + bool triangulateForLinearize(const Cameras& cameras) const { + + bool isDebug = false; + size_t nrCameras = this->triangulateSafe(cameras); + + 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; + } + } + + /// 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 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 */ + boost::optional point() const { + return point_; + } + + /** COMPUTE the landmark */ + boost::optional 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 cheirality verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; + +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + +} // \ namespace gtsam diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp new file mode 100644 index 000000000..079833ec4 --- /dev/null +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -0,0 +1,131 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +TEST(PinholeSet, Stereo) { + typedef vector ZZ; + PinholeSet set; + CalibratedCamera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6, traits::dimension); + + // Check measurements + Point2 expected(0, 0); + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix43 actualE; + Matrix F1; + { + Matrix23 E1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + PinholeSet::FBlocks F; + Matrix E; + set.project2(p, F, E); + LONGS_EQUAL(2, F.size()); + EXPECT(assert_equal(F1, F[0])); + EXPECT(assert_equal(F1, F[1])); + EXPECT(assert_equal(actualE, E)); +} + +/* ************************************************************************* */ +// Cal3Bundler test +#include +#include +TEST(PinholeSet, Pinhole) { + typedef PinholeCamera Camera; + typedef vector ZZ; + PinholeSet set; + Camera camera; + set.push_back(camera); + set.push_back(camera); + Point3 p(0, 0, 1); + EXPECT(assert_equal(set, set)); + PinholeSet set2 = set; + set2.push_back(camera); + EXPECT(!set.equals(set2)); + + // Check measurements + Point2 expected; + ZZ z = set.project2(p); + EXPECT(assert_equal(expected, z[0])); + EXPECT(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix43 actualE; + Matrix F1; + { + Matrix23 E1; + Matrix23 H1; + camera.project2(p, F1, E1); + actualE << E1, E1; + } + + // Check computed derivatives + PinholeSet::FBlocks F; + Matrix E, H; + set.project2(p, F, E); + LONGS_EQUAL(2, F.size()); + EXPECT(assert_equal(F1, F[0])); + EXPECT(assert_equal(F1, F[1])); + EXPECT(assert_equal(actualE, E)); + + // Check errors + ZZ measured; + measured.push_back(Point2(1, 2)); + measured.push_back(Point2(3, 4)); + Vector4 expectedV; + + // reprojectionError + expectedV << -1, -2, -3, -4; + Vector actualV = set.reprojectionError(p, measured); + EXPECT(assert_equal(expectedV, actualV)); + + // reprojectionErrorAtInfinity + EXPECT( + assert_equal(Point3(0, 0, 1), + camera.backprojectPointAtInfinity(Point2()))); + actualV = set.reprojectionErrorAtInfinity(p, measured); + EXPECT(assert_equal(expectedV, actualV)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +