From fb4dd81c4de30b9e44c87cc74df68439875fc54c Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 15 Jul 2015 12:58:03 -0400 Subject: [PATCH 01/28] refactoring: step 1 --- .../slam/SmartStereoProjectionFactor.h | 18 ++++++++--------- .../slam/SmartStereoProjectionPoseFactor.h | 7 ++----- .../testSmartStereoProjectionPoseFactor.cpp | 20 +++++++++---------- 3 files changed, 21 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index f4e82d98d..8b655af46 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -33,6 +33,12 @@ namespace gtsam { +/// Linearization mode: what factor to linearize to + enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD + }; + + /** * SmartStereoProjectionFactor: triangulates point */ @@ -82,11 +88,8 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a StereoCamera // TODO: Get rid of this? - typedef StereoCamera Camera; - /// Vector of cameras - typedef CameraSet Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -195,7 +198,7 @@ public: } std::vector > mono_cameras; - BOOST_FOREACH(const Camera& camera, 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)); @@ -215,7 +218,7 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { + 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) { @@ -578,9 +581,6 @@ public: } } - /// Cameras are computed in derived class - virtual Cameras cameras(const Values& values) const = 0; - /** return the landmark */ boost::optional point() const { return point_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index bc4d3ccfb..ac269335e 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -42,10 +42,7 @@ template class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { public: - /// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; + protected: @@ -163,7 +160,7 @@ public: size_t i=0; BOOST_FOREACH(const Key& k, this->keys_) { Pose3 pose = values.at(k); - typename Base::Camera camera(pose, K_all_[i++]); + StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } return cameras; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 258c8d0eb..90e88c666 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -326,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -394,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); @@ -472,22 +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, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); From 9c2ab0ce3b4b346587d696fa1751edd1b5cbac87 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 15 Jul 2015 16:52:06 -0400 Subject: [PATCH 02/28] change 2 to ZDim --- gtsam/geometry/CameraSet.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index a60528909..1e0150768 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -161,7 +161,7 @@ public: for (size_t i = 0; i < m; i++) { // for each camera const MatrixZD& Fi = Fs[i]; - const Eigen::Matrix Ei_P = // + const Eigen::Matrix Ei_P = // E.block(ZDim * i, 0, ZDim, N) * P; // D = (Dx2) * ZDim From bd4dd8493380edd5f4877faada369fcba11b3056 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 15 Jul 2015 16:53:04 -0400 Subject: [PATCH 03/28] huge refactor. Compiles again, but triangulation still broken, SmartStereo test fails --- .../slam/SmartStereoProjectionFactor.h | 742 +++++++++--------- .../slam/SmartStereoProjectionPoseFactor.h | 67 +- .../testSmartStereoProjectionPoseFactor.cpp | 90 +-- 3 files changed, 429 insertions(+), 470 deletions(-) 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_(); From 93f7eafaa86a50aa5d0cceb0df820bb5df115ee1 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 15 Jul 2015 23:16:45 -0400 Subject: [PATCH 04/28] re-enable triangulation hack --- .../slam/SmartStereoProjectionFactor.h | 136 +++++++++--------- 1 file changed, 67 insertions(+), 69 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index de6ce8509..6c38b5f0b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -241,76 +241,74 @@ public: 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)); + } + Point3 point = triangulatePoint3 >(mono_cameras, mono_measurements, + params_.triangulation.rankTolerance, params_.triangulation.enableEPI); + + // // // End temporary hack + + // FIXME: temporary: triangulation using only first camera +// const StereoPoint2& z0 = this->measured_.at(0); +// point_ = cameras[0].backproject(z0); + + // 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) > params_.triangulation.landmarkDistanceThreshold) { + return TriangulationResult::Degenerate(); + } + const StereoPoint2& zi = this->measured_.at(i); + try { + StereoPoint2 reprojectionError(camera.project(point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + return TriangulationResult::BehindCamera(); + } + i += 1; + } + //std::cout << "totalReprojError error: " << totalReprojError << std::endl; + // we discard smart factors that have large reprojection error + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); + + result_ = TriangulationResult(point); + + } 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 + return TriangulationResult::Degenerate(); + } 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 + return TriangulationResult::BehindCamera(); + } } - return TriangulationResult(Point3()); + return result_; + } /// triangulate From 3f0e695cc93ba87253590b63f3d3c0566d96ff59 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 16 Jul 2015 11:26:07 -0400 Subject: [PATCH 05/28] some tests pass again --- .../slam/SmartStereoProjectionFactor.h | 92 +++++++++---------- 1 file changed, 43 insertions(+), 49 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 6c38b5f0b..8c546b9b0 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -241,27 +241,28 @@ public: bool retriangulate = decideIfTriangulate(cameras); if (retriangulate) { // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + std::cout << "triangulateSafe i \n" << std::endl; - //TODO: Chris will replace this with something else for stereo + //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()); - } + // // // 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)); - } - Point3 point = triangulatePoint3 >(mono_cameras, mono_measurements, - params_.triangulation.rankTolerance, params_.triangulation.enableEPI); + 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)); + } +// Point3 point = triangulatePoint3 >(mono_cameras, mono_measurements, +// params_.triangulation.rankTolerance, params_.triangulation.enableEPI); + result_ = gtsam::triangulateSafe(mono_cameras, mono_measurements, + params_.triangulation); // // // End temporary hack @@ -270,42 +271,31 @@ public: // point_ = cameras[0].backproject(z0); // 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) > params_.triangulation.landmarkDistanceThreshold) { - return TriangulationResult::Degenerate(); - } - const StereoPoint2& zi = this->measured_.at(i); - try { - StereoPoint2 reprojectionError(camera.project(point) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - return TriangulationResult::BehindCamera(); - } - i += 1; - } +// 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(*result_) > params_.triangulation.landmarkDistanceThreshold) { +// return TriangulationResult::Degenerate(); +// } +// const StereoPoint2& zi = this->measured_.at(i); +// try { +// StereoPoint2 reprojectionError(camera.project(*result_) - zi); +// totalReprojError += reprojectionError.vector().norm(); +// } catch (CheiralityException) { +// return TriangulationResult::BehindCamera(); +// } +// i += 1; +// } //std::cout << "totalReprojError error: " << totalReprojError << std::endl; // we discard smart factors that have large reprojection error - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) - return TriangulationResult::Degenerate(); +// if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 +// && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) +// return TriangulationResult::Degenerate(); - result_ = TriangulationResult(point); +// result_ = TriangulationResult(point); - } 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 - return TriangulationResult::Degenerate(); - } 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 - return TriangulationResult::BehindCamera(); - } } return result_; @@ -545,6 +535,9 @@ public: else result_ = triangulateSafe(cameras); + std::cout << "Triangulation result in totalReprojectionError" << std::endl; + std::cout << result_ << std::endl; + if (result_) // All good, just use version in base class return Base::totalReprojectionError(cameras, *result_); @@ -555,9 +548,10 @@ public: // Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0); // // return Base::totalReprojectionError(cameras, backprojected); - } else + } else { // if we don't want to manage the exceptions we discard the factor return 0.0; + } } /// Calculate total reprojection error From 36c652ac402c99c1c06648ae80e5aff49a2f7bfe Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 19 Jul 2015 20:37:16 -0400 Subject: [PATCH 06/28] remove monocular triangulation hack and make tests pass again --- .../slam/SmartStereoProjectionFactor.h | 110 ++++++++---------- .../testSmartStereoProjectionPoseFactor.cpp | 18 ++- 2 files changed, 56 insertions(+), 72 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 8c546b9b0..d57d1ca25 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -234,67 +234,58 @@ public: 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 - return TriangulationResult::Degenerate(); - } +// if (m < 2) { // if we have a single pose the corresponding factor is uninformative +// return TriangulationResult::Degenerate(); +// } bool retriangulate = decideIfTriangulate(cameras); if (retriangulate) { - // We triangulate the 3D position of the landmark - std::cout << "triangulateSafe i \n" << 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 reprojections; + reprojections.reserve(m); + for(size_t i = 0; i < m; i++) { + reprojections.push_back(cameras[i].backproject(measured_[i])); } - 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)); + Point3 pw_sum; + BOOST_FOREACH(const Point3& pw, reprojections) { + pw_sum = pw_sum + pw; } -// Point3 point = triangulatePoint3 >(mono_cameras, mono_measurements, -// params_.triangulation.rankTolerance, params_.triangulation.enableEPI); - result_ = gtsam::triangulateSafe(mono_cameras, mono_measurements, - params_.triangulation); + // average reprojected landmark + Point3 pw_avg = pw_sum / double(m); - // // // End temporary hack + // check if it lies in front of all cameras + bool cheirality_ok = true; + double totalReprojError = 0; + for(size_t i = 0; i < m; i++) { + const Pose3& pose = cameras[i].pose(); + const Point3& pl = pose.transform_to(pw_avg); + if (pl.z() <= 0) { + cheirality_ok = false; + break; + } - // FIXME: temporary: triangulation using only first camera -// const StereoPoint2& z0 = this->measured_.at(0); -// point_ = cameras[0].backproject(z0); + // check landmark distance + if (params_.triangulation.landmarkDistanceThreshold > 0 && + pl.norm() > params_.triangulation.landmarkDistanceThreshold) { + return TriangulationResult::Degenerate(); + } - // 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(*result_) > params_.triangulation.landmarkDistanceThreshold) { -// return TriangulationResult::Degenerate(); -// } -// const StereoPoint2& zi = this->measured_.at(i); -// try { -// StereoPoint2 reprojectionError(camera.project(*result_) - zi); -// totalReprojError += reprojectionError.vector().norm(); -// } catch (CheiralityException) { -// return TriangulationResult::BehindCamera(); -// } -// i += 1; -// } - //std::cout << "totalReprojError error: " << totalReprojError << std::endl; - // we discard smart factors that have large reprojection error -// if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 -// && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) -// return TriangulationResult::Degenerate(); + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { + const StereoPoint2& zi = measured_[i]; + StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); + totalReprojError += reprojectionError.vector().norm(); + } + } // for -// result_ = TriangulationResult(point); + if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); + + if(cheirality_ok == false) { + result_ = TriangulationResult::BehindCamera(); + } + result_ = TriangulationResult(pw_avg); } return result_; @@ -389,11 +380,11 @@ public: return boost::make_shared >(this->keys_); } - /// linearize to a Hessianfactor - virtual boost::shared_ptr > linearizeToHessian( - const Values& values, double lambda = 0.0) const { - return createHessianFactor(this->cameras(values), lambda); - } +// /// linearize to a Hessianfactor +// virtual boost::shared_ptr > linearizeToHessian( +// const Values& values, double lambda = 0.0) const { +// return createHessianFactor(this->cameras(values), lambda); +// } // /// linearize to an Implicit Schur factor // virtual boost::shared_ptr > linearizeToImplicit( @@ -420,8 +411,8 @@ public: return createHessianFactor(cameras, lambda); // case IMPLICIT_SCHUR: // return createRegularImplicitSchurFactor(cameras, lambda); -// case JACOBIAN_SVD: -// return createJacobianSVDFactor(cameras, lambda); + case JACOBIAN_SVD: + return createJacobianSVDFactor(cameras, lambda); // case JACOBIAN_Q: // return createJacobianQFactor(cameras, lambda); default: @@ -535,14 +526,11 @@ public: else result_ = triangulateSafe(cameras); - std::cout << "Triangulation result in totalReprojectionError" << std::endl; - std::cout << result_ << std::endl; - 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"); + throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo.")); // // Otherwise, manage the exceptions with rotation-only factors // const StereoPoint2& z0 = this->measured_.at(0); // Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index a4a8c9269..f35c82539 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -124,7 +124,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); - + cout << "Test 122 STARTS HERE ----------------------------------------- 122 " << endl; // create second camera 1 meter to the right of first camera Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); @@ -277,7 +277,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1); + EXPECT_DOUBLES_EQUAL(991819.94, graph.error(values), 1); Values result; gttic_(SmartStereoProjectionPoseFactor); @@ -286,7 +286,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); VectorValues delta = GFG->optimize(); @@ -436,7 +436,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { - double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error vector views; @@ -474,7 +473,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); @@ -668,11 +666,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - // Create smartfactors - double rankTol = 10; - SmartStereoProjectionParams params; - params.setRankTolerance(rankTol); + params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); @@ -979,6 +974,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { + vector views; views.push_back(x1); views.push_back(x2); @@ -1023,7 +1019,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), - hessianFactorRot->information(), 1e-8)); + hessianFactorRot->information(), 1e-7)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); @@ -1039,7 +1035,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations and translations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), - hessianFactorRotTran->information(), 1e-8)); + hessianFactorRotTran->information(), 1e-6)); } /* ************************************************************************* */ From fcc9ac26691ed3c66ef6021e4803469aeb25f0f1 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 19 Jul 2015 20:39:34 -0400 Subject: [PATCH 07/28] remove extra cout --- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index f35c82539..d0139d850 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -124,7 +124,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); - cout << "Test 122 STARTS HERE ----------------------------------------- 122 " << endl; + // create second camera 1 meter to the right of first camera Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); From b5a978c534319f6030ef30f8545eae4dd88d125f Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 19 Jul 2015 21:01:14 -0400 Subject: [PATCH 08/28] improve dynamic outlier rejection test --- .../testSmartStereoProjectionPoseFactor.cpp | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index d0139d850..4a3985842 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -436,8 +436,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - vector views; views.push_back(x1); views.push_back(x2); @@ -473,7 +471,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setDynamicOutlierRejectionThreshold(1); SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); @@ -488,6 +486,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { SmartFactor::shared_ptr smartFactor4(new SmartFactor(params)); smartFactor4->add(measurements_cam4, views, model, K); + // same as factor 4, but dynamic outlier rejection is off + SmartFactor::shared_ptr smartFactor4b(new SmartFactor()); + smartFactor4b->add(measurements_cam4, views, model, K); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graph; @@ -505,7 +507,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { values.insert(x2, pose2); values.insert(x3, pose3); - // All factors are disabled and pose should remain where it is + EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9); + // zero error due to dynamic outlier rejection + EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); + + // dynamic outlier rejection is off + EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + + // Factors 1-3 should have valid point, factor 4 should not + EXPECT(smartFactor1->point()); + EXPECT(smartFactor2->point()); + EXPECT(smartFactor3->point()); + EXPECT(smartFactor4->point().degenerate()); + EXPECT(smartFactor4b->point()); + + // Factor 4 is disabled, pose 3 stays put Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); From 7ae31bd8e755ccdbd49c6d46413515fb310a2230 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 20 Jul 2015 11:43:57 -0400 Subject: [PATCH 09/28] delete some unused stuff --- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4a3985842..d9207dc21 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -40,11 +40,7 @@ static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); static Cal3_S2Stereo::shared_ptr K2( new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); -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 SmartStereoProjectionParams params; From 5e660198b6775da8189d566347cc7821ca17249c Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 20 Jul 2015 11:45:54 -0400 Subject: [PATCH 10/28] make more generic so it can be used with StereoCamera, too --- gtsam/slam/TriangulationFactor.h | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index b94eafba7..967cc78cd 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -18,6 +18,7 @@ #include #include #include +#include namespace gtsam { @@ -42,9 +43,12 @@ protected: /// shorthand for this class typedef TriangulationFactor This; + /// shorthand for measurement type, e.g. Point2 or StereoPoint2 + typedef typename CAMERA::Measurement Measurement; + // Keep a copy of measurement and calibration for I/O const CAMERA camera_; ///< CAMERA in which this landmark was seen - const Point2 measured_; ///< 2D measurement + const Measurement measured_; ///< 2D measurement // verbosity handling for Cheirality Exceptions const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) @@ -69,14 +73,17 @@ public: * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality */ - TriangulationFactor(const CAMERA& camera, const Point2& measured, + TriangulationFactor(const CAMERA& camera, const Measurement& measured, const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false, bool verboseCheirality = false) : Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_( throwCheirality), verboseCheirality_(verboseCheirality) { - if (model && model->dim() != 2) + if (model && model->dim() != Measurement::dimension) throw std::invalid_argument( - "TriangulationFactor must be created with 2-dimensional noise model."); + "TriangulationFactor must be created with " + + boost::lexical_cast((int) Measurement::dimension) + + "-dimensional noise model."); + } /** Virtual destructor */ @@ -113,18 +120,18 @@ public: Vector evaluateError(const Point3& point, boost::optional H2 = boost::none) const { try { - Point2 error(camera_.project2(point, boost::none, H2) - measured_); + Measurement error(camera_.project2(point, boost::none, H2) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(2, 3); + *H2 = zeros(Measurement::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return ones(2) * 2.0 * camera_.calibration().fx(); + return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); } } @@ -147,8 +154,8 @@ public: if (Ab.rows() == 0) { std::vector dimensions(1, 3); Ab = VerticalBlockMatrix(dimensions, 2, true); - A.resize(2,3); - b.resize(2); + A.resize(Measurement::dimension,3); + b.resize(Measurement::dimension); } // Would be even better if we could pass blocks to project @@ -164,7 +171,7 @@ public: } /** return the measurement */ - const Point2& measured() const { + const Measurement& measured() const { return measured_; } From 02c7b2b81de29286333dcffa7ed517744fc0860e Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 20 Jul 2015 11:46:18 -0400 Subject: [PATCH 11/28] made more generic, and comments --- gtsam/geometry/triangulation.h | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 4ac634f03..c3ab56200 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 triangulateDLT( /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses - * @param sharedCal shared pointer to single calibration object + * @param sharedCal shared pointer to single calibration object (monocular only!) * @param measurements 2D measurements * @param landmarkKey to refer to landmark * @param initialEstimate @@ -97,7 +97,7 @@ std::pair triangulationGraph( /** * Create a factor graph with projection factors from pinhole cameras * (each camera has a pose and calibration) - * @param cameras pinhole cameras + * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param landmarkKey to refer to landmark * @param initialEstimate @@ -106,22 +106,21 @@ std::pair triangulationGraph( template std::pair triangulationGraph( const std::vector& cameras, - const std::vector& measurements, Key landmarkKey, + const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + (camera_i, measurements[i], unit, landmarkKey)); } return std::make_pair(graph, values); } -/// PinholeCamera specific version +/// PinholeCamera specific version // TODO: (chris) why does this exist? template std::pair triangulationGraph( const std::vector >& cameras, @@ -165,7 +164,7 @@ Point3 triangulateNonlinear(const std::vector& poses, /** * Given an initial estimate , refine a point using measurements in several cameras - * @param cameras pinhole cameras + * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param initialEstimate * @return refined Point3 @@ -173,7 +172,7 @@ Point3 triangulateNonlinear(const std::vector& poses, template Point3 triangulateNonlinear( const std::vector& cameras, - const std::vector& measurements, const Point3& initialEstimate) { + const std::vector& measurements, const Point3& initialEstimate) { // Create a factor graph and initial values Values values; @@ -184,7 +183,7 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } -/// PinholeCamera specific version +/// PinholeCamera specific version // TODO: (chris) why does this exist? template Point3 triangulateNonlinear( const std::vector >& cameras, From b5aa7fb7f0d1879fae4303d85915530ea773ace1 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 27 Jul 2015 23:25:05 -0400 Subject: [PATCH 12/28] change interface to match PinholePose. --- gtsam/geometry/StereoCamera.h | 4 ++-- gtsam/slam/tests/testTriangulationFactor.cpp | 25 ++++++++++++++++++++ 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index f09626c9d..ac32be7ae 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -66,8 +66,8 @@ public: StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); /// Return shared pointer to calibration - const Cal3_S2Stereo::shared_ptr calibration() const { - return K_; + const Cal3_S2Stereo& calibration() const { + return *K_; } /// @} diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 2c3a64495..bfd63ab56 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,30 @@ TEST( triangulation, TriangulationFactor ) { CHECK(assert_equal(HExpected, HActual, 1e-3)); } +//****************************************************************************** +TEST( triangulation, TriangulationFactorStereo ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5)); + StereoCamera camera2(pose1, stereoCal); + + StereoPoint2 z2 = camera2.project(landmark); + + typedef TriangulationFactor Factor; + Factor factor(camera2, z2, model, pointKey); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + //****************************************************************************** int main() { TestResult tr; From da9078cf3bb75b3477f568032e427ea88c2e78e5 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 27 Jul 2015 23:27:20 -0400 Subject: [PATCH 13/28] add nonlinear triangulation back. Some unit tests fail again --- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index d57d1ca25..f0a4dd4f5 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -285,6 +285,10 @@ public: if(cheirality_ok == false) { result_ = TriangulationResult::BehindCamera(); } + + pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); + + result_ = TriangulationResult(pw_avg); } @@ -537,6 +541,7 @@ public: // // return Base::totalReprojectionError(cameras, backprojected); } else { + std::cout << "Degenerate factor" << std::endl; // if we don't want to manage the exceptions we discard the factor return 0.0; } From db64b48fda2827df4fc625b5559c053492693400 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 28 Jul 2015 14:48:53 -0400 Subject: [PATCH 14/28] tests pass --- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index d9207dc21..097a9bddd 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -273,7 +273,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - EXPECT_DOUBLES_EQUAL(991819.94, graph.error(values), 1); + EXPECT_DOUBLES_EQUAL(979345.4, graph.error(values), 1); Values result; gttic_(SmartStereoProjectionPoseFactor); @@ -282,7 +282,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { gttoc_(SmartStereoProjectionPoseFactor); tictoc_finishedIteration_(); - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); VectorValues delta = GFG->optimize(); @@ -510,7 +510,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); // dynamic outlier rejection is off - EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(6272.613220592455, smartFactor4b->error(values), 1e-9); // Factors 1-3 should have valid point, factor 4 should not EXPECT(smartFactor1->point()); From 748877ff7e6496bfca0330dfbac661f47d5d269e Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 28 Jul 2015 14:56:45 -0400 Subject: [PATCH 15/28] remove calibration template from SmartStereoProjectionFactor --- .../slam/SmartStereoProjectionFactor.h | 19 +++++++++---------- .../slam/SmartStereoProjectionPoseFactor.h | 7 ++----- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index f0a4dd4f5..e9a18e9ec 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -126,12 +126,10 @@ enum DegeneracyMode { * 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: @@ -149,7 +147,7 @@ protected: public: /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /// Vector of cameras typedef CameraSet Cameras; @@ -184,7 +182,8 @@ public: /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { - const This *e = dynamic_cast(&p); + const SmartStereoProjectionFactor *e = + dynamic_cast(&p); return e && params_.linearizationMode == e->params_.linearizationMode && Base::equals(p, tol); } @@ -467,7 +466,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - std::vector& Fblocks, Matrix& E, Vector& b, + std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras) const { if (!result_) { @@ -486,7 +485,7 @@ public: /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( - std::vector& Fblocks, Matrix& E, Vector& b, + std::vector& Fblocks, Matrix& E, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); @@ -497,7 +496,7 @@ public: /// takes values bool triangulateAndComputeJacobiansSVD( - std::vector& Fblocks, Matrix& Enull, Vector& b, + std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); @@ -595,9 +594,9 @@ private: }; /// traits -template -struct traits > : public Testable< - SmartStereoProjectionFactor > { +template<> +struct traits : public Testable< + SmartStereoProjectionFactor> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index f9ed686c6..50d9bb8fb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -39,10 +39,7 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { - -public: - +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: @@ -53,7 +50,7 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class typedef SmartStereoProjectionPoseFactor This; From fd1e41a9e6efa6a4d1e38b45320aa1429fa5ffdd Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 28 Jul 2015 15:12:02 -0400 Subject: [PATCH 16/28] remove calibration template from SmartStereoProjectionPoseFactor --- .../slam/SmartStereoProjectionFactor.h | 2 +- .../slam/SmartStereoProjectionPoseFactor.h | 30 +++---- .../testSmartStereoProjectionPoseFactor.cpp | 78 +++++++++---------- 3 files changed, 54 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e9a18e9ec..3981e9a84 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -332,7 +332,7 @@ public: } // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). - std::vector Fblocks; + std::vector Fblocks; Matrix F, E; Vector b; computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 50d9bb8fb..061c67a08 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -38,12 +38,11 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) public: @@ -53,7 +52,7 @@ public: typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -83,7 +82,7 @@ public: */ void add(const StereoPoint2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, - const boost::shared_ptr K_i) { + const boost::shared_ptr K_i) { Base::add(measured_i, poseKey_i, noise_i); K_all_.push_back(K_i); } @@ -97,7 +96,7 @@ public: */ void add(std::vector measurements, std::vector poseKeys, std::vector noises, - std::vector > Ks) { + std::vector > Ks) { Base::add(measurements, poseKeys, noises); for (size_t i = 0; i < measurements.size(); i++) { K_all_.push_back(Ks.at(i)); @@ -112,7 +111,7 @@ public: * @param K the (known) camera calibration (same for all measurements) */ void add(std::vector measurements, std::vector poseKeys, - const SharedNoiseModel noise, const boost::shared_ptr K) { + const SharedNoiseModel noise, const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { Base::add(measurements.at(i), poseKeys.at(i), noise); K_all_.push_back(K); @@ -127,14 +126,15 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; - BOOST_FOREACH(const boost::shared_ptr& K, K_all_) + BOOST_FOREACH(const boost::shared_ptr& K, K_all_) K->print("calibration = "); Base::print("", keyFormatter); } /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { - const This *e = dynamic_cast(&p); + const SmartStereoProjectionPoseFactor *e = + dynamic_cast(&p); return e && Base::equals(p, tol); } @@ -151,7 +151,7 @@ public: } /** return the calibration object */ - inline const std::vector > calibration() const { + inline const std::vector > calibration() const { return K_all_; } @@ -161,11 +161,11 @@ public: * to keys involved in this factor * @return vector of Values */ - typename Base::Cameras cameras(const Values& values) const { - typename Base::Cameras cameras; + Base::Cameras cameras(const Values& values) const { + Base::Cameras cameras; size_t i=0; BOOST_FOREACH(const Key& k, this->keys_) { - Pose3 pose = values.at(k); + const Pose3& pose = values.at(k); StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } @@ -185,9 +185,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : public Testable< - SmartStereoProjectionPoseFactor > { +template<> +struct traits : public Testable< + SmartStereoProjectionPoseFactor> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 097a9bddd..bef8d4048 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -62,8 +62,6 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartStereoProjectionPoseFactor SmartFactor; - vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -83,32 +81,32 @@ LevenbergMarquardtParams lm_params; /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor2) { - SmartFactor factor1(params); + SmartStereoProjectionPoseFactor factor1(params); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor3) { - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); factor1->add(measurement1, poseKey1, model, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor4) { - SmartFactor factor1(params); + SmartStereoProjectionPoseFactor factor1(params); factor1.add(measurement1, poseKey1, model, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); factor1->add(measurement1, poseKey1, model, K); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); factor2->add(measurement1, poseKey1, model, K); CHECK(assert_equal(*factor1, *factor2)); @@ -136,7 +134,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartFactor factor1; + SmartStereoProjectionPoseFactor factor1; factor1.add(level_uv, x1, model, K2); factor1.add(level_uv_right, x2, model, K2); @@ -144,7 +142,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); + SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values); double actualError2 = factor1.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); @@ -178,13 +176,13 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); factor1->add(level_uv, x1, model, K); factor1->add(level_uv_right, x2, model, K); double actualError1 = factor1->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); @@ -240,13 +238,13 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); smartFactor1->add(measurements_cam1, views, model, K2); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor()); smartFactor2->add(measurements_cam2, views, model, K2); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor()); smartFactor3->add(measurements_cam3, views, model, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -327,13 +325,13 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - SmartFactor::shared_ptr smartFactor1( new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -396,13 +394,13 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { params.setLinearizationMode(JACOBIAN_SVD); params.setLandmarkDistanceThreshold(2); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -470,20 +468,20 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(1); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(params)); smartFactor4->add(measurements_cam4, views, model, K); // same as factor 4, but dynamic outlier rejection is off - SmartFactor::shared_ptr smartFactor4b(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor()); smartFactor4b->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -556,13 +554,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor3->add(measurements_cam3, views, model, K); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -683,13 +681,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { SmartStereoProjectionParams params; params.setRankTolerance(10); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(params)); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); smartFactor3->add(measurements_cam3, views, model, K); // Create graph to optimize @@ -766,10 +764,10 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // // double rankTol = 50; -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor1->add(measurements_cam1, views, model, K2); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor2->add(measurements_cam2, views, model, K2); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -835,13 +833,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // // double rankTol = 10; // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy)); // smartFactor3->add(measurements_cam3, views, model, K); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -900,7 +898,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // measurements_cam1.push_back(cam1_uv1); // measurements_cam1.push_back(cam2_uv1); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); @@ -941,7 +939,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor()); smartFactorInstance->add(measurements_cam1, views, model, K); Values values; @@ -1009,7 +1007,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - SmartFactor::shared_ptr smartFactor(new SmartFactor()); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor()); smartFactor->add(measurements_cam1, views, model, K2); Values values; From 07bd7fa2bd3bac8c6dae59f14d7903e90334bf92 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 28 Jul 2015 15:14:39 -0400 Subject: [PATCH 17/28] fix examples and wrapper --- gtsam.h | 2 +- gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index ebd554549..4de397d7c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -915,7 +915,7 @@ class StereoCamera { // Standard Interface gtsam::Pose3 pose() const; double baseline() const; - gtsam::Cal3_S2Stereo* calibration() const; + gtsam::Cal3_S2Stereo calibration() const; // Manifold gtsam::StereoCamera retract(const Vector& d) const; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index ac2c31077..5ba8ec913 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartStereoProjectionPoseFactor SmartFactor; + typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); From 123c55f0d7b42c110cb506123128b5d8da2be4a4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 29 Jul 2015 14:05:48 -0400 Subject: [PATCH 18/28] respect triangulation.enableEPI flag in SmartStereo factor --- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 3981e9a84..75444d62a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -238,6 +238,12 @@ public: // } bool retriangulate = decideIfTriangulate(cameras); + +// if(!retriangulate) +// std::cout << "retriangulate = false" << std::endl; +// +// bool retriangulate = true; + if (retriangulate) { std::vector reprojections; @@ -285,7 +291,8 @@ public: result_ = TriangulationResult::BehindCamera(); } - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); + if(params_.triangulation.enableEPI) + pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); result_ = TriangulationResult(pw_avg); From 321a7dbb111594a92acdc627606fc753d53f3688 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 16 Aug 2015 16:34:10 -0400 Subject: [PATCH 19/28] call print in Base --- gtsam/slam/SmartFactorBase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b147c2721..f4ef51d9a 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -198,7 +198,7 @@ public: } if(body_P_sensor_) body_P_sensor_->print("body_P_sensor_:\n"); - print("", keyFormatter); + Base::print("", keyFormatter); } /// equals From d9ae4021680abd651506a9ebf0bba2bfdc86ea9b Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 16 Aug 2015 18:14:19 -0400 Subject: [PATCH 20/28] fix TriangulationFactor bug (for stereo camera), and add new unit tests and comparisons --- gtsam/geometry/tests/testTriangulation.cpp | 106 +++++++++++++++++++++ gtsam/slam/TriangulationFactor.h | 2 +- 2 files changed, 107 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 352493683..bd18143cb 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -18,9 +18,15 @@ #include #include +#include +#include #include +#include +#include +#include #include + #include #include @@ -273,6 +279,106 @@ TEST( triangulation, onePose) { TriangulationUnderconstrainedException); } +//****************************************************************************** +TEST( triangulation, StereotriangulateNonlinear ) { + + Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + + // two camera poses m1, m2 + Matrix4 m1, m2; + m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, + 0.592783835, -0.77156583, 0.230856632, 66.2186159, + 0.116517574, -0.201470143, -0.9725393, -4.28382528, + 0, 0, 0, 1; + + m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, + -0.29277519, 0.947083213, 0.131587097, 65.843136, + -0.0206094928, 0.131334858, -0.991123524, -4.3525033, + 0, 0, 0, 1; + + typedef CameraSet Cameras; + Cameras cameras; + cameras.push_back(StereoCamera(Pose3(m1), stereoK)); + cameras.push_back(StereoCamera(Pose3(m2), stereoK)); + + vector measurements; + measurements += StereoPoint2(226.936, 175.212, 424.469); + measurements += StereoPoint2(339.571, 285.547, 669.973); + + Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 + + Point3 actual = triangulateNonlinear(cameras, measurements, initial); + + Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187 + + EXPECT(assert_equal(expected, actual, 1e-4)); + + + // regular stereo factor comparison - expect very similar result as above + { + typedef GenericStereoFactor StereoFactor; + + Values values; + values.insert(Symbol('x', 1), Pose3(m1)); + values.insert(Symbol('x', 2), Pose3(m2)); + values.insert(Symbol('l', 1), initial); + + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(3)); + graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK))); + graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK))); + + const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); + graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); + graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + } + + // use Triangulation Factor directly - expect same result as above + { + Values values; + values.insert(Symbol('l', 1), initial); + + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(3)); + + graph.push_back(TriangulationFactor(cameras[0], measurements[0], unit, Symbol('l',1))); + graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + } + + // use ExpressionFactor - expect same result as above + { + Values values; + values.insert(Symbol('l', 1), initial); + + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create(3)); + + Expression point_(Symbol('l',1)); + Expression camera0_(cameras[0]); + Expression camera1_(cameras[1]); + Expression project0_(camera0_, &StereoCamera::project2, point_); + Expression project1_(camera1_, &StereoCamera::project2, point_); + + graph.addExpressionFactor(unit, measurements[0], project0_); + graph.addExpressionFactor(unit, measurements[1], project1_); + + LevenbergMarquardtOptimizer optimizer(graph, values); + Values result = optimizer.optimize(); + + EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + } +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 967cc78cd..aa50929a5 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -153,7 +153,7 @@ public: // Allocate memory for Jacobian factor, do only once if (Ab.rows() == 0) { std::vector dimensions(1, 3); - Ab = VerticalBlockMatrix(dimensions, 2, true); + Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true); A.resize(Measurement::dimension,3); b.resize(Measurement::dimension); } From 1279038c77569afb53ba32d51bdba4fcd32b5db8 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Sun, 16 Aug 2015 20:29:07 -0400 Subject: [PATCH 21/28] Add Stereo triangulation comparison with expression factor --- gtsam/slam/tests/testTriangulationFactor.cpp | 24 +++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index bfd63ab56..5ac92b4a9 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -47,7 +49,7 @@ Point2 z1 = camera1.project(landmark); //****************************************************************************** TEST( triangulation, TriangulationFactor ) { - // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); SharedNoiseModel model; typedef TriangulationFactor Factor; @@ -66,9 +68,9 @@ TEST( triangulation, TriangulationFactor ) { //****************************************************************************** TEST( triangulation, TriangulationFactorStereo ) { - // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); - SharedNoiseModel model; + SharedNoiseModel model=noiseModel::Isotropic::Sigma(3,0.5); Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5)); StereoCamera camera2(pose1, stereoCal); @@ -86,6 +88,22 @@ TEST( triangulation, TriangulationFactorStereo ) { // Verify the Jacobians are correct CHECK(assert_equal(HExpected, HActual, 1e-3)); + + // compare same problem against expression factor + Expression::UnaryFunction::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2); + Expression point_(pointKey); + Expression project2_(f, point_); + + ExpressionFactor eFactor(model, z2, project2_); + + Values values; + values.insert(pointKey, landmark); + + vector HActual1(1), HActual2(1); + Vector error1 = factor.unwhitenedError(values, HActual1); + Vector error2 = eFactor.unwhitenedError(values, HActual2); + EXPECT(assert_equal(error1, error2)); + EXPECT(assert_equal(HActual1[0], HActual2[0])); } //****************************************************************************** From 1727b607288cdd5b2a2b038d200342ec7ff55b20 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 19 Aug 2015 00:11:35 -0400 Subject: [PATCH 22/28] Fixed and cleaned up unit test --- .../slam/SmartStereoProjectionFactor.h | 44 +++++----- .../testSmartStereoProjectionPoseFactor.cpp | 80 ++++++++++++++++--- 2 files changed, 93 insertions(+), 31 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 75444d62a..c0532e073 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -233,10 +234,6 @@ public: 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 -// return TriangulationResult::Degenerate(); -// } - bool retriangulate = decideIfTriangulate(cameras); // if(!retriangulate) @@ -245,7 +242,7 @@ public: // bool retriangulate = true; if (retriangulate) { - +// std::cout << "Retriangulate " << std::endl; std::vector reprojections; reprojections.reserve(m); for(size_t i = 0; i < m; i++) { @@ -259,21 +256,22 @@ public: // average reprojected landmark Point3 pw_avg = pw_sum / double(m); - // check if it lies in front of all cameras - bool cheirality_ok = true; double totalReprojError = 0; + + // check if it lies in front of all cameras for(size_t i = 0; i < m; i++) { const Pose3& pose = cameras[i].pose(); const Point3& pl = pose.transform_to(pw_avg); if (pl.z() <= 0) { - cheirality_ok = false; - break; + result_ = TriangulationResult::BehindCamera(); + return result_; } // check landmark distance if (params_.triangulation.landmarkDistanceThreshold > 0 && pl.norm() > params_.triangulation.landmarkDistanceThreshold) { - return TriangulationResult::Degenerate(); + result_ = TriangulationResult::Degenerate(); + return result_; } if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { @@ -284,20 +282,27 @@ public: } // for if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) - return TriangulationResult::Degenerate(); - - if(cheirality_ok == false) { - result_ = TriangulationResult::BehindCamera(); + && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { + result_ = TriangulationResult::Degenerate(); + return result_; } - if(params_.triangulation.enableEPI) - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); - + if(params_.triangulation.enableEPI) { + try { + pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); + } catch(StereoCheiralityException& e) { + if(params_.verboseCheirality) + std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl; + if(params_.throwCheirality) + throw; + result_ = TriangulationResult::BehindCamera(); + return TriangulationResult::BehindCamera(); + } + } result_ = TriangulationResult(pw_avg); - } + } // if retriangulate return result_; } @@ -547,7 +552,6 @@ public: // // return Base::totalReprojectionError(cameras, backprojected); } else { - std::cout << "Degenerate factor" << std::endl; // if we don't want to manage the exceptions we discard the factor return 0.0; } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index bef8d4048..dc35a782b 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -208,6 +208,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); @@ -226,11 +227,11 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + vector measurements_l1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + vector measurements_l2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + vector measurements_l3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); vector views; @@ -238,17 +239,21 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); + SmartStereoProjectionParams smart_params; + smart_params.triangulation.enableEPI = true; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(smart_params)); + smartFactor1->add(measurements_l1, views, model, K2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor()); - smartFactor2->add(measurements_cam2, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(smart_params)); + smartFactor2->add(measurements_l2, views, model, K2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor()); - smartFactor3->add(measurements_cam3, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(smart_params)); + smartFactor3->add(measurements_l3, views, model, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -271,7 +276,13 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); - EXPECT_DOUBLES_EQUAL(979345.4, graph.error(values), 1); + // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; + EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-9); + + // get triangulated landmarks from smart factors + Point3 landmark1_smart = *smartFactor1->point(); + Point3 landmark2_smart = *smartFactor2->point(); + Point3 landmark3_smart = *smartFactor3->point(); Values result; gttic_(SmartStereoProjectionPoseFactor); @@ -282,6 +293,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); +// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl; + GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); VectorValues delta = GFG->optimize(); VectorValues expected = VectorValues::Zero(delta); @@ -289,6 +302,51 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // result.print("results of 3 camera, 3 landmark optimization \n"); EXPECT(assert_equal(pose3, result.at(x3))); + + /* *************************************************************** + * Same problem with regular Stereo factors, expect same error! + * ****************************************************************/ + +// landmark1_smart.print("landmark1_smart"); +// landmark2_smart.print("landmark2_smart"); +// landmark3_smart.print("landmark3_smart"); + + // add landmarks to values + values.insert(L(1), landmark1_smart); + values.insert(L(2), landmark2_smart); + values.insert(L(3), landmark3_smart); + + // add factors + NonlinearFactorGraph graph2; + + graph2.push_back(PriorFactor(x1, pose1, noisePrior)); + graph2.push_back(PriorFactor(x2, pose2, noisePrior)); + + typedef GenericStereoFactor ProjectionFactor; + + bool verboseCheirality = true; + + graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality)); + + graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality)); + + graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality)); + graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); + +// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; + EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-9); + + LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); + Values result2 = optimizer2.optimize(); + EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5); + +// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl; + } /* *************************************************************************/ @@ -508,7 +566,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); // dynamic outlier rejection is off - EXPECT_DOUBLES_EQUAL(6272.613220592455, smartFactor4b->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); // Factors 1-3 should have valid point, factor 4 should not EXPECT(smartFactor1->point()); From c8df985e2f3994bea7362bdf6009b1f91a67f086 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 19 Aug 2015 07:30:14 -0400 Subject: [PATCH 23/28] Relax test tolerance a bit for quaternion mode --- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index dc35a782b..89c9c020a 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -277,7 +277,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7); // get triangulated landmarks from smart factors Point3 landmark1_smart = *smartFactor1->point(); @@ -339,7 +339,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7); LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); Values result2 = optimizer2.optimize(); From 8b4228fa5656790e6a9b3aab1555af90286a96a9 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 19 Aug 2015 10:58:19 -0400 Subject: [PATCH 24/28] Make smart parameters public. Now easier to set, and it's sufficient for this to be const within the smart factor itself! --- gtsam/slam/SmartProjectionFactor.h | 12 +++++------- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 13 ++++++------- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 8fc8880e1..120b0bbce 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -44,23 +44,21 @@ enum DegeneracyMode { /* * Parameters for the smart projection factors */ -class GTSAM_EXPORT SmartProjectionParams { - -public: +struct GTSAM_EXPORT SmartProjectionParams { 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 + TriangulationParameters triangulation; + 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) + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) /// @} // Constructor diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index c0532e073..d3dca11e0 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert + * @author Chris Beall */ #pragma once @@ -47,23 +48,21 @@ enum DegeneracyMode { /* * Parameters for the smart stereo projection factors */ - class GTSAM_EXPORT SmartStereoProjectionParams { - - public: + struct GTSAM_EXPORT SmartStereoProjectionParams { 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 + TriangulationParameters triangulation; + 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) + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) /// @} From 0503df31fa8a3a5facaa745bbfea4916e0530135 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 25 Aug 2015 12:14:52 -0400 Subject: [PATCH 25/28] Relax tolerance to 1e-6 for MKL/quaternion test, and fix up documentation a bit. --- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 9 ++++++--- gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h | 7 +++++-- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 2 +- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index d3dca11e0..c12bf8d94 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -11,7 +11,7 @@ /** * @file SmartStereoProjectionFactor.h - * @brief Base class to create smart factors on poses or cameras + * @brief Smart stereo factor on StereoCameras (pose + calibration) * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert @@ -123,8 +123,11 @@ enum DegeneracyMode { /** * 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. + * This factor operates with StereoCamera. This factor requires that values + * contains the involved StereoCameras. Calibration is assumed to be fixed, as this + * is also assumed in StereoCamera. + * If you'd like to store poses in values instead of cameras, use + * SmartStereoProjectionPoseFactor instead */ class SmartStereoProjectionFactor: public SmartFactorBase { private: diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 061c67a08..c9d8ec285 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -11,7 +11,7 @@ /** * @file SmartStereoProjectionPoseFactor.h - * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @brief Smart stereo factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira @@ -35,7 +35,10 @@ namespace gtsam { */ /** - * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * This factor assumes that camera calibration is fixed, but each camera + * has its own calibration. + * The factor only constrains poses (variable dimension is 6). + * This factor requires that values contains the involved poses (Pose3). * @addtogroup SLAM */ class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 89c9c020a..3b16a9db8 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -1089,7 +1089,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), - hessianFactorRot->information(), 1e-7)); + hessianFactorRot->information(), 1e-6)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); From 29ad9478f7543a875c2dcd5af1f81843d8c6a483 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 26 Aug 2015 13:02:39 -0400 Subject: [PATCH 26/28] Move noise model initialization from add function into constructor. Can only have one noise model per factor anyway --- gtsam/slam/SmartFactorBase.h | 59 ++--- gtsam/slam/SmartProjectionFactor.h | 4 +- gtsam/slam/SmartProjectionPoseFactor.h | 6 +- gtsam/slam/tests/testSmartFactorBase.cpp | 25 ++- .../tests/testSmartProjectionCameraFactor.cpp | 160 +++++++------- .../tests/testSmartProjectionPoseFactor.cpp | 206 +++++++++--------- .../SmartStereoProjectionFactorExample.cpp | 6 +- .../slam/SmartStereoProjectionFactor.h | 4 +- .../slam/SmartStereoProjectionPoseFactor.h | 33 ++- .../testSmartStereoProjectionPoseFactor.cpp | 116 +++++----- 10 files changed, 298 insertions(+), 321 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f4ef51d9a..3f043a469 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert + * @author Chris Beall */ #pragma once @@ -79,22 +80,6 @@ protected: typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; - // check that noise model is isotropic and the same - // TODO, this is hacky, we should just do this via constructor, not add - void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { - if (!sharedNoiseModel) - return; - SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< - noiseModel::Isotropic>(sharedNoiseModel); - if (!sharedIsotropic) - throw std::runtime_error("SmartFactorBase: needs isotropic"); - if (!noiseModel_) - noiseModel_ = sharedIsotropic; - else if (!sharedIsotropic->equals(*noiseModel_)) - throw std::runtime_error( - "SmartFactorBase: cannot add measurements with different noise model"); - } - public: // Definitions for blocks of F, externally visible @@ -109,8 +94,21 @@ public: typedef CameraSet Cameras; /// Constructor - SmartFactorBase(boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor){} + SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor){ + + if (!sharedNoiseModel) + throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); + + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + noiseModel::Isotropic>(sharedNoiseModel); + + if (!sharedIsotropic) + throw std::runtime_error("SmartFactorBase: needs isotropic"); + + noiseModel_ = sharedIsotropic; + } /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { @@ -122,34 +120,18 @@ public: * @param poseKey is the index corresponding to the camera observing the landmark * @param sharedNoiseModel is the measurement noise */ - void add(const Z& measured_i, const Key& cameraKey_i, - const SharedNoiseModel& sharedNoiseModel) { + void add(const Z& measured_i, const Key& cameraKey_i) { this->measured_.push_back(measured_i); this->keys_.push_back(cameraKey_i); - maybeSetNoiseModel(sharedNoiseModel); } /** - * Add a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys */ - void add(std::vector& measurements, std::vector& cameraKeys, - std::vector& noises) { + void add(std::vector& measurements, std::vector& cameraKeys) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); this->keys_.push_back(cameraKeys.at(i)); - maybeSetNoiseModel(noises.at(i)); - } - } - - /** - * Add a bunch of measurements and use the same noise model for all of them - */ - void add(std::vector& measurements, std::vector& cameraKeys, - const SharedNoiseModel& noise) { - for (size_t i = 0; i < measurements.size(); i++) { - this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(cameraKeys.at(i)); - maybeSetNoiseModel(noise); } } @@ -158,11 +140,10 @@ public: * The noise is assumed to be the same for all measurements */ template - void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { + void add(const SFM_TRACK& trackToAdd) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); - maybeSetNoiseModel(noise); } } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 120b0bbce..5146c5a32 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -159,10 +159,10 @@ public: * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ - SmartProjectionFactor( + SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel, const boost::optional body_P_sensor = boost::none, const SmartProjectionParams& params = SmartProjectionParams()) : - Base(body_P_sensor), params_(params), // + Base(sharedNoiseModel, body_P_sensor), params_(params), // result_(TriangulationResult::Degenerate()) { } diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 834c9c9b6..c091ff79d 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -61,14 +61,16 @@ public: /** * Constructor + * @param Isotropic measurement noise * @param K (fixed) calibration, assumed to be the same for all cameras * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ - SmartProjectionPoseFactor(const boost::shared_ptr K, + SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr K, const boost::optional body_P_sensor = boost::none, const SmartProjectionParams& params = SmartProjectionParams()) : - Base(body_P_sensor, params), K_(K) { + Base(sharedNoiseModel, body_P_sensor, params), K_(K) { } /** Virtual destructor */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 373d482fe..bf5969d4d 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -16,17 +16,24 @@ * @date Feb 2015 */ -#include "../SmartFactorBase.h" +#include #include #include using namespace std; using namespace gtsam; +static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); +static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); + /* ************************************************************************* */ #include #include class PinholeFactor: public SmartFactorBase > { +public: + typedef SmartFactorBase > Base; + PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { + } virtual double error(const Values& values) const { return 0.0; } @@ -37,15 +44,19 @@ class PinholeFactor: public SmartFactorBase > { }; TEST(SmartFactorBase, Pinhole) { - PinholeFactor f; - f.add(Point2(), 1, SharedNoiseModel()); - f.add(Point2(), 2, SharedNoiseModel()); + PinholeFactor f= PinholeFactor(unit2); + f.add(Point2(), 1); + f.add(Point2(), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } /* ************************************************************************* */ #include class StereoFactor: public SmartFactorBase { +public: + typedef SmartFactorBase Base; + StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { + } virtual double error(const Values& values) const { return 0.0; } @@ -56,9 +67,9 @@ class StereoFactor: public SmartFactorBase { }; TEST(SmartFactorBase, Stereo) { - StereoFactor f; - f.add(StereoPoint2(), 1, SharedNoiseModel()); - f.add(StereoPoint2(), 2, SharedNoiseModel()); + StereoFactor f(unit3); + f.add(StereoPoint2(), 1); + f.add(StereoPoint2(), 2); EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 9838d65e5..d4f60b085 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -72,39 +72,39 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor) { using namespace vanilla; - SmartFactor::shared_ptr factor1(new SmartFactor()); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor2) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(boost::none, params); + SmartFactor factor1(unit2, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor3) { using namespace vanilla; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, x1, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor4) { using namespace vanilla; params.setRankTolerance(rankTol); - SmartFactor factor1(boost::none, params); - factor1.add(measurement1, x1, unit2); + SmartFactor factor1(unit2, boost::none, params); + factor1.add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Equals ) { using namespace vanilla; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, x1, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(measurement1, x1); - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, x1, unit2); + SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); + factor2->add(measurement1, x1); } /* *************************************************************************/ @@ -115,9 +115,9 @@ TEST( SmartProjectionCameraFactor, noiseless ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); double expectedError = 0.0; DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); @@ -140,9 +140,9 @@ TEST( SmartProjectionCameraFactor, noisy ) { Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right); values.insert(c2, perturbed_level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); // Point is now uninitialized before a triangulation event EXPECT(!factor1->point()); @@ -164,20 +164,16 @@ TEST( SmartProjectionCameraFactor, noisy ) { Vector actual = factor1->whitenedError(cameras1, point1); EXPECT(assert_equal(expected, actual, 1)); - SmartFactor::shared_ptr factor2(new SmartFactor()); + SmartFactor::shared_ptr factor2(new SmartFactor(unit2)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector noises; - noises.push_back(unit2); - noises.push_back(unit2); - vector views; views.push_back(c1); views.push_back(c2); - factor2->add(measurements, views, noises); + factor2->add(measurements, views); double actualError2 = factor2->error(values); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1); @@ -195,16 +191,16 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create and fill smartfactors - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); vector views; views.push_back(c1); views.push_back(c2); views.push_back(c3); - smartFactor1->add(measurements_cam1, views, unit2); - smartFactor2->add(measurements_cam2, views, unit2); - smartFactor3->add(measurements_cam3, views, unit2); + smartFactor1->add(measurements_cam1, views); + smartFactor2->add(measurements_cam2, views); + smartFactor3->add(measurements_cam3, views); // Create factor graph and add priors on two cameras NonlinearFactorGraph graph; @@ -308,8 +304,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { measures.second = measurements_cam1.at(i); track1.measurements.push_back(measures); } - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(track1, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(track1); SfM_Track track2; for (size_t i = 0; i < 3; ++i) { @@ -318,11 +314,11 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { measures.second = measurements_cam2.at(i); track2.measurements.push_back(measures); } - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(track2, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(track2); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); @@ -384,20 +380,20 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { views.push_back(c2); views.push_back(c3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor()); - smartFactor4->add(measurements_cam4, views, unit2); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); - SmartFactor::shared_ptr smartFactor5(new SmartFactor()); - smartFactor5->add(measurements_cam5, views, unit2); + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); @@ -464,20 +460,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { views.push_back(c2); views.push_back(c3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor()); - smartFactor4->add(measurements_cam4, views, unit2); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); - SmartFactor::shared_ptr smartFactor5(new SmartFactor()); - smartFactor5->add(measurements_cam5, views, unit2); + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); @@ -540,20 +536,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { views.push_back(c2); views.push_back(c3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, unit2); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, unit2); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, unit2); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); + smartFactor3->add(measurements_cam3, views); - SmartFactor::shared_ptr smartFactor4(new SmartFactor()); - smartFactor4->add(measurements_cam4, views, unit2); + SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2)); + smartFactor4->add(measurements_cam4, views); - SmartFactor::shared_ptr smartFactor5(new SmartFactor()); - smartFactor5->add(measurements_cam5, views, unit2); + SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2)); + smartFactor5->add(measurements_cam5, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6); @@ -604,9 +600,9 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); double actualError = factor1->error(values); @@ -633,9 +629,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { values.insert(c2, level_camera_right); NonlinearFactorGraph smartGraph; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); smartGraph.push_back(factor1); double expectedError = factor1->error(values); double expectedErrorGraph = smartGraph.error(values); @@ -674,9 +670,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { values.insert(c2, level_camera_right); NonlinearFactorGraph smartGraph; - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); smartGraph.push_back(factor1); Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; @@ -765,9 +761,9 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, c1, unit2); - factor1->add(level_uv_right, c2, unit2); + SmartFactor::shared_ptr factor1(new SmartFactor(unit2)); + factor1->add(level_uv, c1); + factor1->add(level_uv_right, c2); Matrix expectedE; Vector expectedb; @@ -814,9 +810,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { params.setEnableEPI(useEPI); SmartFactor::shared_ptr explicitFactor( - new SmartFactor(boost::none, params)); - explicitFactor->add(level_uv, c1, unit2); - explicitFactor->add(level_uv_right, c2, unit2); + new SmartFactor(unit2, boost::none, params)); + explicitFactor->add(level_uv, c1); + explicitFactor->add(level_uv_right, c2); GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize( values); @@ -826,9 +822,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { // Implicit Schur version params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); SmartFactor::shared_ptr implicitFactor( - new SmartFactor(boost::none, params)); - implicitFactor->add(level_uv, c1, unit2); - implicitFactor->add(level_uv_right, c2, unit2); + new SmartFactor(unit2, boost::none, params)); + implicitFactor->add(level_uv, c1); + implicitFactor->add(level_uv_right, c2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = implicitFactor->linearize(values); CHECK(gaussianImplicitSchurFactor); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 72147ff35..8796dfe65 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -53,7 +53,7 @@ LevenbergMarquardtParams lmParams; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); } /* ************************************************************************* */ @@ -61,14 +61,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, boost::none, params); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor3) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); - factor1->add(measurement1, x1, model); + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); } /* ************************************************************************* */ @@ -76,18 +76,18 @@ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(sharedK, boost::none, params); - factor1.add(measurement1, x1, model); + SmartFactor factor1(model, sharedK, boost::none, params); + factor1.add(measurement1, x1); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(sharedK)); - factor1->add(measurement1, x1, model); + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); - SmartFactor::shared_ptr factor2(new SmartFactor(sharedK)); - factor2->add(measurement1, x1, model); + SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); + factor2->add(measurement1, x1); CHECK(assert_equal(*factor1, *factor2)); } @@ -101,9 +101,9 @@ TEST( SmartProjectionPoseFactor, noiseless ) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactor factor(sharedK); - factor.add(level_uv, x1, model); - factor.add(level_uv_right, x2, model); + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses values.insert(x1, cam1.pose()); @@ -166,26 +166,22 @@ TEST( SmartProjectionPoseFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor(new SmartFactor((sharedK))); - factor->add(level_uv, x1, model); - factor->add(level_uv_right, x2, model); + SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); double actualError1 = factor->error(values); - SmartFactor::shared_ptr factor2(new SmartFactor((sharedK))); + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector noises; - noises.push_back(model); - noises.push_back(model); - vector views; views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises); + factor2->add(measurements, views); double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } @@ -238,14 +234,14 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartProjectionPoseFactor smartFactor1(K, sensor_to_body, params); - smartFactor1.add(measurements_cam1, views, model); + SmartProjectionPoseFactor smartFactor1(model, K, sensor_to_body, params); + smartFactor1.add(measurements_cam1, views); - SmartProjectionPoseFactor smartFactor2(K, sensor_to_body, params); - smartFactor2.add(measurements_cam2, views, model); + SmartProjectionPoseFactor smartFactor2(model, K, sensor_to_body, params); + smartFactor2.add(measurements_cam2, views); - SmartProjectionPoseFactor smartFactor3(K, sensor_to_body, params); - smartFactor3.add(measurements_cam3, views, model); + SmartProjectionPoseFactor smartFactor3(model, K, sensor_to_body, params); + smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -296,14 +292,14 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2)); - smartFactor2->add(measurements_cam2, views, model); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2)); - smartFactor3->add(measurements_cam3, views, model); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -366,8 +362,8 @@ TEST( SmartProjectionPoseFactor, Factors ) { views.push_back(x1); views.push_back(x2); - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(sharedK); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); SmartFactor::Cameras cameras; cameras.push_back(cam1); @@ -524,14 +520,14 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK)); - smartFactor2->add(measurements_cam2, views, model); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK)); - smartFactor3->add(measurements_cam3, views, model); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -586,19 +582,19 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { params.setLinearizationMode(gtsam::JACOBIAN_SVD); params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactor factor1(sharedK, boost::none, params); + SmartFactor factor1(model, sharedK, boost::none, params); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -650,16 +646,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { params.setEnableEPI(false); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -717,20 +713,20 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(sharedK, boost::none, params)); - smartFactor4->add(measurements_cam4, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor4->add(measurements_cam4, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -775,16 +771,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { params.setLinearizationMode(gtsam::JACOBIAN_Q); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -895,16 +891,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); NonlinearFactorGraph graph; graph.push_back(smartFactor1); @@ -978,12 +974,12 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac params.setDegeneracyMode(gtsam::HANDLE_INFINITY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK2, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK2, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK2, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK2, boost::none, params)); + smartFactor2->add(measurements_cam2, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); @@ -1040,16 +1036,16 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1108,8 +1104,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -1140,8 +1136,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK)); - smartFactorInstance->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); + smartFactorInstance->add(measurements_cam1, views); Values values; values.insert(x1, cam1.pose()); @@ -1196,8 +1192,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { vector measurements_cam1; projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2)); - smartFactor->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); + smartFactor->add(measurements_cam1, views); Values values; values.insert(x1, cam1.pose()); @@ -1239,8 +1235,8 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(sharedBundlerK, boost::none, params); - factor.add(measurement1, x1, model); + SmartFactor factor(model, sharedBundlerK, boost::none, params); + factor.add(measurement1, x1); } /* *************************************************************************/ @@ -1263,14 +1259,14 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { views.push_back(x2); views.push_back(x3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK)); - smartFactor1->add(measurements_cam1, views, model); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); + smartFactor1->add(measurements_cam1, views); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK)); - smartFactor2->add(measurements_cam2, views, model); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); + smartFactor2->add(measurements_cam2, views); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK)); - smartFactor3->add(measurements_cam3, views, model); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1334,16 +1330,16 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedBundlerK, boost::none, params)); - smartFactor1->add(measurements_cam1, views, model); + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor1->add(measurements_cam1, views); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedBundlerK, boost::none, params)); - smartFactor2->add(measurements_cam2, views, model); + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor2->add(measurements_cam2, views); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedBundlerK, boost::none, params)); - smartFactor3->add(measurements_cam3, views, model); + new SmartFactor(model, sharedBundlerK, boost::none, params)); + smartFactor3->add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 5ba8ec913..d3680460f 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -109,17 +109,17 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor()); + SmartFactor::shared_ptr factor(new SmartFactor(model)); size_t current_l = 3; // hardcoded landmark ID from first measurement while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor()); + factor = SmartFactor::shared_ptr(new SmartFactor(model)); current_l = l; } - factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K); + factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K); } Pose3 first_pose = initial_estimate.at(Symbol('x',1)); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index c12bf8d94..e7118a36c 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -159,8 +159,10 @@ public: * Constructor * @param params internal parameters of the smart factors */ - SmartStereoProjectionFactor(const SmartStereoProjectionParams& params = + SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = SmartStereoProjectionParams()) : + Base(sharedNoiseModel), // params_(params), // result_(TriangulationResult::Degenerate()) { } diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index c9d8ec285..c2526fd74 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -62,15 +62,13 @@ 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 Isotropic measurement noise + * @param params internal parameters of the smart factors */ - SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params = + SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = SmartStereoProjectionParams()) : - Base(params) { + Base(sharedNoiseModel, params) { } /** Virtual destructor */ @@ -80,27 +78,23 @@ public: * add a new measurement and pose key * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKey is key corresponding to the camera observing the same landmark - * @param noise_i is the measurement noise - * @param K_i is the (known) camera calibration + * @param K is the (fixed) camera calibration */ - void add(const StereoPoint2 measured_i, const Key poseKey_i, - const SharedNoiseModel noise_i, - const boost::shared_ptr K_i) { - Base::add(measured_i, poseKey_i, noise_i); - K_all_.push_back(K_i); + void add(const StereoPoint2 measured, const Key poseKey, + const boost::shared_ptr K) { + Base::add(measured, poseKey); + K_all_.push_back(K); } /** * Variant of the previous one in which we include a set of measurements * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noises vector of measurement noises * @param Ks vector of calibration objects */ void add(std::vector measurements, std::vector poseKeys, - std::vector noises, std::vector > Ks) { - Base::add(measurements, poseKeys, noises); + Base::add(measurements, poseKeys); for (size_t i = 0; i < measurements.size(); i++) { K_all_.push_back(Ks.at(i)); } @@ -110,13 +104,12 @@ public: * Variant of the previous one in which we include a set of measurements with the same noise and calibration * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noise measurement noise (same for all measurements) * @param K the (known) camera calibration (same for all measurements) */ void add(std::vector measurements, std::vector poseKeys, - const SharedNoiseModel noise, const boost::shared_ptr K) { + const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i), noise); + Base::add(measurements.at(i), poseKeys.at(i)); K_all_.push_back(K); } } diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 3b16a9db8..2981f675d 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -81,33 +81,33 @@ LevenbergMarquardtParams lm_params; /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor2) { - SmartStereoProjectionPoseFactor factor1(params); + SmartStereoProjectionPoseFactor factor1(model, params); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor3) { - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); - factor1->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(measurement1, poseKey1, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor4) { - SmartStereoProjectionPoseFactor factor1(params); - factor1.add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor factor1(model, params); + factor1.add(measurement1, poseKey1, K); } /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); - factor1->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(measurement1, poseKey1, K); - SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); - factor2->add(measurement1, poseKey1, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model)); + factor2->add(measurement1, poseKey1, K); CHECK(assert_equal(*factor1, *factor2)); } @@ -134,9 +134,9 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartStereoProjectionPoseFactor factor1; - factor1.add(level_uv, x1, model, K2); - factor1.add(level_uv_right, x2, model, K2); + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right, x2, K2); double actualError = factor1.error(values); double expectedError = 0.0; @@ -176,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); - SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); + factor1->add(level_uv, x1, K); + factor1->add(level_uv_right, x2, K); double actualError1 = factor1->error(values); - SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor()); + SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model)); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector noises; - noises.push_back(model); - noises.push_back(model); - vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); @@ -199,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises, Ks); + factor2->add(measurements, views, Ks); double actualError2 = factor2->error(values); @@ -241,14 +237,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { SmartStereoProjectionParams smart_params; smart_params.triangulation.enableEPI = true; - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(smart_params)); - smartFactor1->add(measurements_l1, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor1->add(measurements_l1, views, K2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(smart_params)); - smartFactor2->add(measurements_l2, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor2->add(measurements_l2, views, K2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(smart_params)); - smartFactor3->add(measurements_l3, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params)); + smartFactor3->add(measurements_l3, views, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -383,14 +379,14 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { SmartStereoProjectionParams params; params.setLinearizationMode(JACOBIAN_SVD); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -452,14 +448,14 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { params.setLinearizationMode(JACOBIAN_SVD); params.setLandmarkDistanceThreshold(2); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -526,21 +522,21 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { params.setDynamicOutlierRejectionThreshold(1); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(params)); - smartFactor4->add(measurements_cam4, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor4->add(measurements_cam4, views, K); // same as factor 4, but dynamic outlier rejection is off - SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor()); - smartFactor4b->add(measurements_cam4, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model)); + smartFactor4b->add(measurements_cam4, views, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -739,14 +735,14 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { SmartStereoProjectionParams params; params.setRankTolerance(10); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params)); - smartFactor1->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params)); - smartFactor2->add(measurements_cam2, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params)); - smartFactor3->add(measurements_cam3, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); // Create graph to optimize NonlinearFactorGraph graph; @@ -997,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); + SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model)); + smartFactorInstance->add(measurements_cam1, views, K); Values values; values.insert(x1, pose1); @@ -1065,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor()); - smartFactor->add(measurements_cam1, views, model, K2); + SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model)); + smartFactor->add(measurements_cam1, views, K2); Values values; values.insert(x1, pose1); From 92e210b8939ad5687a1f5a44a5594352ff183364 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 26 Aug 2015 13:25:12 -0400 Subject: [PATCH 27/28] Fix examples and Matlab wrapper --- examples/SFMExample_SmartFactor.cpp | 4 ++-- examples/SFMExample_SmartFactorPCG.cpp | 4 ++-- gtsam.h | 12 +++++++----- .../examples/SmartProjectionFactorExample.cpp | 6 +++--- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index bb8935439..e7c0aa696 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -58,7 +58,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { @@ -71,7 +71,7 @@ int main(int argc, char* argv[]) { // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, i, measurementNoise); + smartfactor->add(measurement, i); } // insert the smart factor in the graph diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 7ec5f8268..743934c7c 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor(K)); + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); for (size_t i = 0; i < poses.size(); ++i) { @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: - smartfactor->add(measurement, i, measurementNoise); + smartfactor->add(measurement, i); } // insert the smart factor in the graph diff --git a/gtsam.h b/gtsam.h index 6fb73e0ca..a75dbf056 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2364,15 +2364,17 @@ class SmartProjectionParams { template virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(const CALIBRATION* K); - SmartProjectionPoseFactor(const CALIBRATION* K, + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const CALIBRATION* K, + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, const gtsam::Pose3& body_P_sensor, const gtsam::SmartProjectionParams& params); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, - const gtsam::noiseModel::Base* noise_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i); // enabling serialization functionality //void serialize() const; diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index e00dcee57..1423ef113 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -87,17 +87,17 @@ int main(int argc, char** argv){ //read stereo measurements and construct smart factors - SmartFactor::shared_ptr factor(new SmartFactor(K)); + SmartFactor::shared_ptr factor(new SmartFactor(model, K)); size_t current_l = 3; // hardcoded landmark ID from first measurement while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); - factor = SmartFactor::shared_ptr(new SmartFactor(K)); + factor = SmartFactor::shared_ptr(new SmartFactor(model, K)); current_l = l; } - factor->add(Point2(uL,v), i, model); + factor->add(Point2(uL,v), i); } Pose3 firstPose = initial_estimate.at(1); From 8e2915c4f3b958a2d7b57f9da97ea2ad93de6fc6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 4 Sep 2015 18:14:13 +0000 Subject: [PATCH 28/28] README.md edited online with Bitbucket --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 679af5a2f..ccdc7f07e 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,7 @@ Prerequisites: - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- A modern compiler, i.e., at least gcc 4.7.3 on Linux. Optional prerequisites - used automatically if findable by CMake: @@ -46,6 +47,6 @@ See the [`INSTALL`](INSTALL) file for more detailed installation instructions. GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. -Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. - +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. + GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file