diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 8b655af46..de6ce8509 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -38,51 +38,114 @@ namespace gtsam { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY + }; + + /* + * Parameters for the smart stereo projection factors + */ + class GTSAM_EXPORT SmartStereoProjectionParams { + + public: + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + mutable TriangulationParameters triangulation; + const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + + /// Constructor + SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + 1e-5), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartStereoProjectionParams() { + } + + void print(const std::string& str) const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } + }; + + /** - * SmartStereoProjectionFactor: triangulates point - */ + * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. + * This factor operates with StereoCamrea. This factor requires that values + * contains the involved camera poses. Calibration is assumed to be fixed. +*/ template class SmartStereoProjectionFactor: public SmartFactorBase { +private: + + typedef SmartFactorBase Base; + typedef SmartStereoProjectionFactor This; + protected: + /// @name Parameters + /// @{ + const SmartStereoProjectionParams params_; + /// @} + /// @name Caching triangulation /// @{ - const TriangulationParameters parameters_; - // TODO: -// mutable TriangulationResult result_; ///< result from triangulateSafe - - const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + mutable TriangulationResult result_; ///< result from triangulateSafe mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses /// @} - const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases - - const double linearizationThreshold_; ///< threshold to decide whether to re-linearize - mutable std::vector cameraPosesLinearization_; ///< current linearization poses - - mutable Point3 point_; ///< Current estimate of the 3D point - - mutable bool degenerate_; - mutable bool cheiralityException_; - - - /// shorthand for base class type - typedef SmartFactorBase Base; - - /// shorthand for this class - typedef SmartStereoProjectionFactor This; - - enum { - ZDim = 3 - }; ///< Dimension trait of measurement type - - /// @name Parameters governing how triangulation result is treated - /// @{ - const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) - const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - public: /// shorthand for a smart pointer to a factor @@ -93,22 +156,12 @@ public: /** * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from body to sensor frame (default identity) + * @param params internal parameters of the smart factors */ - SmartStereoProjectionFactor(const double rankTolerance, - const double linThreshold, const bool manageDegeneracy, - const bool enableEPI, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : - parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, - dynamicOutlierRejectionThreshold), // - retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_( - linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false) { + SmartStereoProjectionFactor(const SmartStereoProjectionParams& params = + SmartStereoProjectionParams()) : + params_(params), // + result_(TriangulationResult::Degenerate()) { } /** Virtual destructor */ @@ -123,14 +176,19 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartStereoProjectionFactor\n"; - std::cout << "triangulationParameters:\n" << parameters_ << std::endl; - std::cout << "degenerate_ = " << degenerate_ << std::endl; - std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; - std::cout << "linearizationThreshold_ = " << linearizationThreshold_ - << std::endl; + std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl; + std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl; + std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); } + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && params_.linearizationMode == e->params_.linearizationMode + && Base::equals(p, tol); + } + /// 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 @@ -149,7 +207,7 @@ public: if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], - retriangulationThreshold_)) { + params_.retriangulationThreshold)) { retriangulate = true; // at least two poses are different, hence we retriangulate break; } @@ -167,124 +225,105 @@ public: 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 Values& values) const { +// return triangulateSafe(this->cameras(values)); +// } /// triangulateSafe - size_t triangulateSafe(const Cameras& cameras) const { + TriangulationResult 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; + return TriangulationResult::Degenerate(); } - bool retriangulate = decideIfTriangulate(cameras); + bool retriangulate = decideIfTriangulate(cameras); if (retriangulate) { // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - - //TODO: Chris will replace this with something else for stereo -// point_ = triangulatePoint3(cameras, this->measured_, -// rankTolerance_, enableEPI_); - - // // // Temporary hack to use monocular triangulation - std::vector mono_measurements; - BOOST_FOREACH(const StereoPoint2& sp, this->measured_) { - mono_measurements.push_back(sp.point2()); - } - - std::vector > mono_cameras; - BOOST_FOREACH(const StereoCamera& camera, cameras) { - const Pose3& pose = camera.pose(); - const Cal3_S2& K = camera.calibration()->calibration(); - mono_cameras.push_back(PinholeCamera(pose, K)); - } - point_ = triangulatePoint3 >(mono_cameras, mono_measurements, - parameters_.rankTolerance, parameters_.enableEPI); - - // // // End temporary hack - - // FIXME: temporary: triangulation using only first camera -// const StereoPoint2& z0 = this->measured_.at(0); -// point_ = cameras[0].backproject(z0); - - degenerate_ = false; - cheiralityException_ = false; - - // Check landmark distance and reprojection errors to avoid outliers - double totalReprojError = 0.0; - size_t i = 0; - BOOST_FOREACH(const StereoCamera& camera, cameras) { - Point3 cameraTranslation = camera.pose().translation(); - // we discard smart factors corresponding to points that are far away - if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) { - degenerate_ = true; - break; - } - const StereoPoint2& zi = this->measured_.at(i); - try { - StereoPoint2 reprojectionError(camera.project(point_) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - cheiralityException_ = true; - } - i += 1; - } - //std::cout << "totalReprojError error: " << totalReprojError << std::endl; - // we discard smart factors that have large reprojection error - if (parameters_.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > parameters_.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; - } +// try { +// // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; +// +// //TODO: Chris will replace this with something else for stereo +//// point_ = triangulatePoint3(cameras, this->measured_, +//// rankTolerance_, enableEPI_); +// +// // // // Temporary hack to use monocular triangulation +// std::vector mono_measurements; +// BOOST_FOREACH(const StereoPoint2& sp, this->measured_) { +// mono_measurements.push_back(sp.point2()); +// } +// +// std::vector > mono_cameras; +// BOOST_FOREACH(const StereoCamera& camera, cameras) { +// const Pose3& pose = camera.pose(); +// const Cal3_S2& K = camera.calibration()->calibration(); +// mono_cameras.push_back(PinholeCamera(pose, K)); +// } +// point_ = triangulatePoint3 >(mono_cameras, mono_measurements, +// parameters_.rankTolerance, parameters_.enableEPI); +// +// // // // End temporary hack +// +// // FIXME: temporary: triangulation using only first camera +//// const StereoPoint2& z0 = this->measured_.at(0); +//// point_ = cameras[0].backproject(z0); +// +// degenerate_ = false; +// cheiralityException_ = false; +// +// // Check landmark distance and reprojection errors to avoid outliers +// double totalReprojError = 0.0; +// size_t i = 0; +// BOOST_FOREACH(const StereoCamera& camera, cameras) { +// Point3 cameraTranslation = camera.pose().translation(); +// // we discard smart factors corresponding to points that are far away +// if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) { +// degenerate_ = true; +// break; +// } +// const StereoPoint2& zi = this->measured_.at(i); +// try { +// StereoPoint2 reprojectionError(camera.project(point_) - zi); +// totalReprojError += reprojectionError.vector().norm(); +// } catch (CheiralityException) { +// cheiralityException_ = true; +// } +// i += 1; +// } +// //std::cout << "totalReprojError error: " << totalReprojError << std::endl; +// // we discard smart factors that have large reprojection error +// if (parameters_.dynamicOutlierRejectionThreshold > 0 +// && totalReprojError / m > parameters_.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; + return TriangulationResult(Point3()); } /// triangulate bool triangulateForLinearize(const Cameras& cameras) const { - - bool isDebug = false; - size_t nrCameras = this->triangulateSafe(cameras); - - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) { - std::cout << "createImplicitSchurFactor: 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; - } + triangulateSafe(cameras); // imperative, might reset result_ + return (result_); } /// linearize returns a Hessianfactor that is an approximation of error(p) boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0) const { + const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = + false) const { - bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors std::vector js; @@ -293,146 +332,142 @@ public: if (this->measured_.size() != cameras.size()) { std::cout - << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" + << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input" << std::endl; exit(1); } triangulateSafe(cameras); - if (isDebug) - std::cout << "point_ = " << point_ << std::endl; - if (numKeys < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { - if (isDebug) - std::cout << "In linearize: exception" << std::endl; + if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { + // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) v = zero(Base::Dim); - return boost::make_shared >(this->keys_, Gs, gs, - 0.0); + return boost::make_shared >(this->keys_, + Gs, gs, 0.0); } - // 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 (isDebug) - std::cout << "degenerate_ = true" << std::endl; - } - - if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize - for (size_t i = 0; i < cameras.size(); i++) - this->cameraPosesLinearization_[i] = cameras[i].pose(); - - // ================================================================== + // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). std::vector Fblocks; Matrix F, E; Vector b; - computeJacobians(Fblocks, E, b, cameras); - Base::FillDiagonalF(Fblocks, F); // expensive !!! + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); - // Schur complement trick - // Frank says: should be possible to do this more efficiently? - // And we care, as in grouped factors this is called repeatedly - Matrix H(Base::Dim * numKeys, Base::Dim * numKeys); - Vector gs_vector; + // Whiten using noise model + Base::whitenJacobians(Fblocks, E, b); - Matrix3 P = Cameras::PointCov(E, lambda); - H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); + // build augmented hessian + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); - if (isDebug) - std::cout << "gs_vector size " << gs_vector.size() << std::endl; - if (isDebug) - std::cout << "H:\n" << H << std::endl; - - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * Base::Dim; - gs.at(i1) = gs_vector.segment(i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * Base::Dim); - GsCount2++; - } - } - } - // ================================================================== - double f = b.squaredNorm(); - return boost::make_shared >(this->keys_, Gs, gs, f); + return boost::make_shared >(this->keys_, + augmentedHessian); } -// // create factor -// boost::shared_ptr > createImplicitSchurFactor( + // create factor +// boost::shared_ptr > createRegularImplicitSchurFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) -// return Base::createImplicitSchurFactor(cameras, point_, lambda); +// return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); // else -// return boost::shared_ptr >(); +// // failed: return empty +// return boost::shared_ptr >(); // } // // /// create factor -// boost::shared_ptr > createJacobianQFactor( +// boost::shared_ptr > createJacobianQFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) -// return Base::createJacobianQFactor(cameras, point_, lambda); +// return Base::createJacobianQFactor(cameras, *result_, lambda); // else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// // failed: return empty +// return boost::make_shared >(this->keys_); // } // // /// Create a factor, takes values -// boost::shared_ptr > createJacobianQFactor( +// boost::shared_ptr > createJacobianQFactor( // const Values& values, double lambda) const { -// Cameras cameras; -// // TODO triangulate twice ?? -// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); -// if (nonDegenerate) -// return createJacobianQFactor(cameras, lambda); -// else -// return boost::make_shared< JacobianFactorQ >(this->keys_); +// return createJacobianQFactor(this->cameras(values), lambda); // } -// + /// different (faster) way to compute Jacobian factor boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) - return Base::createJacobianSVDFactor(cameras, point_, lambda); + return Base::createJacobianSVDFactor(cameras, *result_, lambda); else return boost::make_shared >(this->keys_); } - /// Returns true if nonDegenerate - bool computeCamerasAndTriangulate(const Values& values, - Cameras& cameras) const { - Values valuesFactor; + /// linearize to a Hessianfactor + virtual boost::shared_ptr > linearizeToHessian( + const Values& values, double lambda = 0.0) const { + return createHessianFactor(this->cameras(values), lambda); + } - // Select only the cameras - BOOST_FOREACH(const Key key, this->keys_) - valuesFactor.insert(key, values.at(key)); +// /// linearize to an Implicit Schur factor +// virtual boost::shared_ptr > linearizeToImplicit( +// const Values& values, double lambda = 0.0) const { +// return createRegularImplicitSchurFactor(this->cameras(values), lambda); +// } +// +// /// linearize to a JacobianfactorQ +// virtual boost::shared_ptr > linearizeToJacobian( +// const Values& values, double lambda = 0.0) const { +// return createJacobianQFactor(this->cameras(values), lambda); +// } - cameras = this->cameras(valuesFactor); - size_t nrCameras = this->triangulateSafe(cameras); - - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (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 << "SmartStereoProjectionFactor: this is not ready" - << std::endl; - std::cout << "this->cheiralityException_ " << this->cheiralityException_ - << std::endl; - std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Cameras& cameras, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(cameras, lambda); +// case IMPLICIT_SCHUR: +// return createRegularImplicitSchurFactor(cameras, lambda); +// case JACOBIAN_SVD: +// return createJacobianSVDFactor(cameras, lambda); +// case JACOBIAN_Q: +// return createJacobianQFactor(cameras, lambda); + default: + throw std::runtime_error("SmartStereoFactorlinearize: unknown mode"); } - return true; + } + + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped(const Values& values, + const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + Cameras cameras = this->cameras(values); + return linearizeDamped(cameras, lambda); + } + + /// linearize + virtual boost::shared_ptr linearize( + const Values& values) const { + return linearizeDamped(values); + } + + /** + * Triangulate and compute derivative of error with respect to point + * @return whether triangulation worked + */ + bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const { + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + cameras.project2(*result_, boost::none, E); + return nonDegenerate; } /** @@ -440,87 +475,62 @@ public: * @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; + Cameras cameras = this->cameras(values); + return triangulateAndComputeE(E, cameras); } - /// Version that takes values, and creates the point - bool computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Values& values) const { - Cameras cameras; - bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); - if (nonDegenerate) - computeJacobians(Fblocks, E, b, cameras, 0.0); - return nonDegenerate; - } /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - void computeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Cameras& cameras) const { - if (this->degenerate_) { - throw("FIXME: computeJacobians degenerate case commented out!"); -// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; -// std::cout << "point " << point_ << std::endl; -// std::cout -// << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used" -// << std::endl; -// if (D > 6) { -// std::cout -// << "Management of degeneracy is not yet ready when one also optimizes for the calibration " -// << std::endl; -// } + void computeJacobiansWithTriangulatedPoint( + std::vector& Fblocks, Matrix& E, Vector& b, + const Cameras& cameras) const { + + if (!result_) { + throw ("computeJacobiansWithTriangulatedPoint"); +// // Handle degeneracy +// // TODO check flag whether we should do this +// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity( +// this->measured_.at(0)); */ // -// int numKeys = this->keys_.size(); -// E = zeros(2 * numKeys, 2); -// b = zero(2 * numKeys); -// double f = 0; -// for (size_t i = 0; i < this->measured_.size(); i++) { -// if (i == 0) { // first pose -// this->point_ = cameras[i].backprojectPointAtInfinity( -// this->measured_.at(i)); -// // 3D parametrization of point at infinity: [px py 1] -// } -// Matrix Fi, Ei; -// Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) -// - this->measured_.at(i)).vector(); -// -// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); -// f += bi.squaredNorm(); -// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi)); -// E.block < 2, 2 > (2 * i, 0) = Ei; -// subInsert(b, bi, 2 * i); -// } -// return f; +// Base::computeJacobians(Fblocks, E, b, cameras, backProjected); } else { - // nondegenerate: just return Base version - Base::computeJacobians(Fblocks, E, b, cameras, point_); - } // end else + // valid result: just return Base version + Base::computeJacobians(Fblocks, E, b, cameras, *result_); + } + } + + /// Version that takes values, and creates the point + bool triangulateAndComputeJacobians( + std::vector& Fblocks, Matrix& E, Vector& b, + const Values& values) const { + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + return nonDegenerate; } /// takes values bool triangulateAndComputeJacobiansSVD( std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { - typename Base::Cameras cameras; - double good = computeCamerasAndTriangulate(values, cameras); - if (good) - return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); - return true; + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); + if (nonDegenerate) + Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + 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); + Cameras cameras = this->cameras(values); + bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - return Base::unwhitenedError(cameras, point_); + return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * 3); + return zero(cameras.size() * Base::ZDim); } /** @@ -532,84 +542,60 @@ public: 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 (externalPoint) + result_ = TriangulationResult(*externalPoint); + else + result_ = triangulateSafe(cameras); - if (nrCameras < 2 - || (!this->manageDegeneracy_ - && (this->cheiralityException_ || this->degenerate_))) { + if (result_) + // All good, just use version in base class + return Base::totalReprojectionError(cameras, *result_); + else if (params_.degeneracyMode == HANDLE_INFINITY) { + throw("Backproject at infinity"); +// // Otherwise, manage the exceptions with rotation-only factors +// const StereoPoint2& z0 = this->measured_.at(0); +// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0); +// +// return Base::totalReprojectionError(cameras, backprojected); + } else // 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; -// size_t i = 0; -// double overallError = 0; -// BOOST_FOREACH(const Camera& camera, cameras) { -// const StereoPoint2& zi = this->measured_.at(i); -// if (i == 0) // first pose -// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity -// StereoPoint2 reprojectionError( -// camera.projectPointAtInfinity(this->point_) - zi); -// overallError += 0.5 -// * this->noise_.at(i)->distance(reprojectionError.vector()); -// i += 1; -// } -// return overallError; - } else { - // Just use version in base class - return Base::totalReprojectionError(cameras, point_); + /// Calculate total reprojection error + virtual double error(const Values& values) const { + if (this->active(values)) { + return totalReprojectionError(Base::cameras(values)); + } else { // else of active flag + return 0.0; } } /** return the landmark */ - boost::optional point() const { - return point_; - } + TriangulationResult point() const { + return result_; + } - /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; - } + /** COMPUTE the landmark */ + TriangulationResult point(const Values& values) const { + Cameras cameras = this->cameras(values); + return triangulateSafe(cameras); + } - /** return the degenerate state */ - inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); - } + /// Is result valid? + bool isValid() const { + return result_; + } - /** return the cheirality status flag */ - inline bool isPointBehindCamera() const { - return cheiralityException_; - } - /** return chirality verbosity */ - inline bool verboseCheirality() const { - return verboseCheirality_; - } + /** return the degenerate state */ + bool isDegenerate() const { + return result_.degenerate(); + } - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { - return throwCheirality_; - } + /** return the cheirality status flag */ + bool isPointBehindCamera() const { + return result_.behindCamera(); + } private: @@ -618,8 +604,8 @@ private: 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_); + ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); + ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); } }; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index ac269335e..f9ed686c6 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -46,8 +46,6 @@ public: protected: - LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) public: @@ -71,14 +69,9 @@ public: * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations */ - SmartStereoProjectionPoseFactor(const double rankTol = 1, - const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_( - linearizeTo) { + SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params = + SmartStereoProjectionParams()) : + Base(params) { } /** Virtual destructor */ @@ -149,6 +142,22 @@ public: return e && Base::equals(p, tol); } + /** + * error calculates the error of the factor. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** return the calibration object */ + inline const std::vector > calibration() const { + return K_all_; + } + /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -166,44 +175,6 @@ public: return cameras; } - /** - * Linearize to Gaussian Factor - * @param values Values structure which must contain camera poses for this factor - * @return - */ - virtual boost::shared_ptr linearize( - const Values& values) const { - // depending on flag set on construction we may linearize to different linear factors - switch(linearizeTo_){ - case JACOBIAN_SVD : - return this->createJacobianSVDFactor(cameras(values), 0.0); - break; - case JACOBIAN_Q : - throw("JacobianQ not working yet!"); -// return this->createJacobianQFactor(cameras(values), 0.0); - break; - default: - return this->createHessianFactor(cameras(values)); - break; - } - } - - /** - * error calculates the error of the factor. - */ - virtual double error(const Values& values) const { - if (this->active(values)) { - return this->totalReprojectionError(cameras(values)); - } else { // else of active flag - return 0.0; - } - } - - /** return the calibration object */ - inline const std::vector > calibration() const { - return K_all_; - } - private: /// Serialization function diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 90e88c666..a4a8c9269 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -43,8 +43,11 @@ static Cal3_S2Stereo::shared_ptr K2( static boost::shared_ptr Kbundler( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); -static double rankTol = 1.0; -static double linThreshold = -1.0; +//static double rankTol = 1.0; +//static double linThreshold = -1.0; + +static SmartStereoProjectionParams params; + // static bool manageDegeneracy = true; // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); @@ -80,7 +83,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, return measurements_cam; } -LevenbergMarquardtParams params; +LevenbergMarquardtParams lm_params; /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { @@ -89,7 +92,7 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) { /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor2) { - SmartFactor factor1(rankTol, linThreshold); + SmartFactor factor1(params); } /* ************************************************************************* */ @@ -100,7 +103,7 @@ TEST( SmartStereoProjectionPoseFactor, Constructor3) { /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor4) { - SmartFactor factor1(rankTol, linThreshold); + SmartFactor factor1(params); factor1.add(measurement1, poseKey1, model, K); } @@ -278,7 +281,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Values result; gttic_(SmartStereoProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); @@ -325,16 +328,16 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + + SmartFactor::shared_ptr smartFactor1( new SmartFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -355,7 +358,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { values.insert(x3, pose3 * noise_pose); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); EXPECT(assert_equal(pose3, result.at(x3))); } @@ -363,7 +366,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { - double excludeLandmarksFutherThanDist = 2; +// double excludeLandmarksFutherThanDist = 2; vector views; views.push_back(x1); @@ -393,19 +396,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist)); + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(2); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -427,7 +428,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { // All factors are disabled and pose should remain where it is Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); EXPECT(assert_equal(values.at(x3), result.at(x3))); } @@ -471,24 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(params)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -510,7 +509,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // All factors are disabled and pose should remain where it is Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); EXPECT(assert_equal(pose3, result.at(x3))); } @@ -571,7 +570,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // values.insert(x3, pose3*noise_pose); // //// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); // result = optimizer.optimize(); // EXPECT(assert_equal(pose3,result.at(x3))); //} @@ -630,7 +629,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // values.insert(L(2), landmark2); // values.insert(L(3), landmark3); // -// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); // Values result = optimizer.optimize(); // // EXPECT(assert_equal(pose3,result.at(x3))); @@ -672,13 +671,16 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // Create smartfactors double rankTol = 10; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); + SmartStereoProjectionParams params; + params.setRankTolerance(rankTol); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); // Create graph to optimize @@ -781,7 +783,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // // Values result; // gttic_(SmartStereoProjectionPoseFactor); -// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); // result = optimizer.optimize(); // gttoc_(SmartStereoProjectionPoseFactor); // tictoc_finishedIteration_(); @@ -855,7 +857,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // // Values result; // gttic_(SmartStereoProjectionPoseFactor); -// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); // result = optimizer.optimize(); // gttoc_(SmartStereoProjectionPoseFactor); // tictoc_finishedIteration_();