From 7a2af9999ae4f1a54d7af645411e0f6aff0ef1d8 Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Tue, 3 Jun 2014 11:21:11 -0400 Subject: [PATCH] Created stereo Smart factor classes, test class for SmartStereoProjectionPoseFactor --- gtsam/slam/SmartStereoProjectionFactor.h | 710 +++++++++ gtsam/slam/SmartStereoProjectionPoseFactor.h | 215 +++ .../testSmartStereoProjectionPoseFactor.cpp | 1318 +++++++++++++++++ 3 files changed, 2243 insertions(+) create mode 100644 gtsam/slam/SmartStereoProjectionFactor.h create mode 100644 gtsam/slam/SmartStereoProjectionPoseFactor.h create mode 100644 gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h new file mode 100644 index 000000000..4c8403a99 --- /dev/null +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -0,0 +1,710 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartStereoProjectionFactor.h + * @brief Base class to create smart factors on poses or cameras + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include "SmartFactorBase.h" + +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/** + * Structure for storing some state memory, used to speed up optimization + * @addtogroup SLAM + */ +class SmartStereoProjectionFactorState { + +protected: + +public: + + SmartStereoProjectionFactorState() { + } + // Hessian representation (after Schur complement) + bool calculatedHessian; + Matrix H; + Vector gs_vector; + std::vector Gs; + std::vector gs; + double f; +}; + +enum LinearizationMode { + HESSIAN, JACOBIAN_SVD, JACOBIAN_Q +}; + +/** + * SmartStereoProjectionFactor: triangulates point + * TODO: why LANDMARK parameter? + */ +template +class SmartStereoProjectionFactor: public SmartFactorBase { +protected: + + // Some triangulation parameters + const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate + mutable std::vector cameraPosesTriangulation_; ///< current triangulation poses + + const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases + + const bool enableEPI_; ///< if set to true, will refine triangulation using LM + + 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_; + + // verbosity handling for Cheirality Exceptions + const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + boost::shared_ptr state_; + + /// shorthand for smart projection factor state variable + typedef boost::shared_ptr SmartFactorStatePtr; + + /// shorthand for base class type + typedef SmartFactorBase Base; + + double landmarkDistanceThreshold_; // if the landmark is triangulated at a + // distance larger than that the factor is considered degenerate + + double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the + // average reprojection error is smaller than this threshold after triangulation, + // and the factor is disregarded if the error is large + + /// shorthand for this class + typedef SmartStereoProjectionFactor This; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a pinhole camera + typedef PinholeCamera Camera; + typedef std::vector Cameras; + + /** + * 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) + */ + SmartStereoProjectionFactor(const double rankTol, const double linThreshold, + const bool manageDegeneracy, const bool enableEPI, + boost::optional body_P_sensor = boost::none, + double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : + Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( + 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( + false), verboseCheirality_(false), state_(state), + landmarkDistanceThreshold_(landmarkDistanceThreshold), + dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + } + + /** Virtual destructor */ + virtual ~SmartStereoProjectionFactor() { + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartStereoProjectionFactor, z = \n"; + std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; + std::cout << "degenerate_ = " << degenerate_ << std::endl; + std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + Base::print("", keyFormatter); + } + + /// Check if the new linearization point_ is the same as the one used for previous triangulation + bool decideIfTriangulate(const Cameras& cameras) const { + // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // Note that this is not yet "selecting linearization", that will come later, and we only check if the + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ + + size_t m = cameras.size(); + + bool retriangulate = false; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesTriangulation_.empty() + || cameras.size() != cameraPosesTriangulation_.size()) + retriangulate = true; + + if (!retriangulate) { + for (size_t i = 0; i < cameras.size(); i++) { + if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], + retriangulationThreshold_)) { + retriangulate = true; // at least two poses are different, hence we retriangulate + break; + } + } + } + + if (retriangulate) { // we store the current poses used for triangulation + cameraPosesTriangulation_.clear(); + cameraPosesTriangulation_.reserve(m); + for (size_t i = 0; i < m; i++) + // cameraPosesTriangulation_[i] = cameras[i].pose(); + cameraPosesTriangulation_.push_back(cameras[i].pose()); + } + + return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation + } + + /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization + bool decideIfLinearize(const Cameras& cameras) const { + // "selective linearization" + // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose + // (we only care about the "rigidity" of the poses, not about their absolute pose) + + if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize + return true; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesLinearization_.empty() + || (cameras.size() != cameraPosesLinearization_.size())) + return true; + + Pose3 firstCameraPose, firstCameraPoseOld; + for (size_t i = 0; i < cameras.size(); i++) { + + if (i == 0) { // we store the initial pose, this is useful for selective re-linearization + firstCameraPose = cameras[i].pose(); + firstCameraPoseOld = cameraPosesLinearization_[i]; + continue; + } + + // we compare the poses in the frame of the first pose + Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); + Pose3 localCameraPoseOld = firstCameraPoseOld.between( + cameraPosesLinearization_[i]); + if (!localCameraPose.equals(localCameraPoseOld, + this->linearizationThreshold_)) + return true; // at least two "relative" poses are different, hence we re-linearize + } + return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + } + + /// triangulateSafe + size_t triangulateSafe(const Values& values) const { + return triangulateSafe(this->cameras(values)); + } + + /// triangulateSafe + size_t triangulateSafe(const Cameras& cameras) const { + + size_t m = cameras.size(); + if (m < 2) { // if we have a single pose the corresponding factor is uninformative + degenerate_ = true; + return m; + } + bool retriangulate = decideIfTriangulate(cameras); + + if (retriangulate) { + // We triangulate the 3D position of the landmark + try { + // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + point_ = triangulatePoint3(cameras, this->measured_, + rankTolerance_, enableEPI_); + degenerate_ = false; + cheiralityException_ = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i=0; + BOOST_FOREACH(const Camera& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + degenerate_ = true; + break; + } + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + cheiralityException_ = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if(dynamicOutlierRejectionThreshold_ > 0 && + totalReprojError/m > dynamicOutlierRejectionThreshold_) + degenerate_ = true; + + } catch (TriangulationUnderconstrainedException&) { + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + degenerate_ = true; + cheiralityException_ = false; + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint + cheiralityException_ = true; + } + } + return m; + } + + /// triangulate + bool triangulateForLinearize(const Cameras& cameras) const { + + bool isDebug = false; + size_t nrCameras = this->triangulateSafe(cameras); + + if (nrCameras < 2 + || (!this->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; + } + } + + /// linearize returns a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const double lambda = 0.0) const { + + bool isDebug = false; + size_t numKeys = this->keys_.size(); + // Create structures for Hessian Factors + std::vector < Key > js; + std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); + std::vector < Vector > gs(numKeys); + + if (this->measured_.size() != cameras.size()) { + std::cout + << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" + << std::endl; + exit(1); + } + + this->triangulateSafe(cameras); + + if (numKeys < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // std::cout << "In linearize: exception" << std::endl; + BOOST_FOREACH(gtsam::Matrix& m, Gs) + m = zeros(D, D); + BOOST_FOREACH(Vector& v, gs) + v = zero(D); + 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; + } + + bool doLinearize = this->decideIfLinearize(cameras); + + if (this->linearizationThreshold_ >= 0 && doLinearize) // 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(); + + if (!doLinearize) { // return the previous Hessian factor + std::cout << "=============================" << std::endl; + std::cout << "doLinearize " << doLinearize << std::endl; + std::cout << "this->linearizationThreshold_ " + << this->linearizationThreshold_ << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + std::cout + << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" + << std::endl; + exit(1); + return boost::make_shared >(this->keys_, + this->state_->Gs, this->state_->gs, this->state_->f); + } + + // ================================================================== + Matrix F, E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + + // 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(D * numKeys, D * numKeys); + Vector gs_vector; + + H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() + * (b - (E * (PointCov * (E.transpose() * b)))); + if (isDebug) + std::cout << "gs_vector size " << gs_vector.size() << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * D; + gs.at(i1) = gs_vector.segment < D > (i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + if (i2 >= i1) { + Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + GsCount2++; + } + } + } + // ================================================================== + if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables + this->state_->Gs = Gs; + this->state_->gs = gs; + this->state_->f = f; + } + return boost::make_shared >(this->keys_, Gs, gs, f); + } + + // create factor + boost::shared_ptr > createImplicitSchurFactor( + const Cameras& cameras, double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createImplicitSchurFactor(cameras, point_, lambda); + else + return boost::shared_ptr >(); + } + + /// create factor + boost::shared_ptr > createJacobianQFactor( + const Cameras& cameras, double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianQFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorQ >(this->keys_); + } + + /// Create a factor, takes values + boost::shared_ptr > createJacobianQFactor( + const Values& values, double lambda) const { + Cameras myCameras; + // TODO triangulate twice ?? + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + return createJacobianQFactor(myCameras, lambda); + else + return boost::make_shared< JacobianFactorQ >(this->keys_); + } + + /// different (faster) way to compute Jacobian factor + boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianSVDFactor(cameras, point_, lambda); + else + return boost::make_shared< JacobianFactorSVD >(this->keys_); + } + + /// Returns true if nonDegenerate + bool computeCamerasAndTriangulate(const Values& values, + Cameras& myCameras) const { + Values valuesFactor; + + // Select only the cameras + BOOST_FOREACH(const Key key, this->keys_) + valuesFactor.insert(key, values.at(key)); + + myCameras = this->cameras(valuesFactor); + size_t nrCameras = this->triangulateSafe(myCameras); + + 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; + } + return true; + } + + /// Takes values + bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeEP(E, PointCov, myCameras); + return nonDegenerate; + } + + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { + return Base::computeEP(E, PointCov, cameras, point_); + } + + /// Version that takes values, and creates the point + bool computeJacobians(std::vector& Fblocks, + Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeJacobians(Fblocks, E, PointCov, b, myCameras, 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 + double computeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Cameras& cameras) const { + + if (this->degenerate_) { + 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; + } + + 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::KeyMatrix2D(this->keys_[i], Fi)); + E.block < 2, 2 > (2 * i, 0) = Ei; + subInsert(b, bi, 2 * i); + } + return f; + } else { + // nondegenerate: just return Base version + return Base::computeJacobians(Fblocks, E, b, cameras, point_); + } // end else + } + + /// Version that computes PointCov, with optional lambda parameter + double computeJacobians(std::vector& Fblocks, + Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, + const double lambda = 0.0) const { + + double f = computeJacobians(Fblocks, E, b, cameras); + + // Point covariance inv(E'*E) + PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); + + return f; + } + + /// takes values + bool computeJacobiansSVD(std::vector& Fblocks, + Matrix& Enull, Vector& b, const Values& values) const { + typename Base::Cameras myCameras; + double good = computeCamerasAndTriangulate(values, myCameras); + if (good) + computeJacobiansSVD(Fblocks, Enull, b, myCameras); + return true; + } + + /// SVD version + double computeJacobiansSVD(std::vector& Fblocks, + Matrix& Enull, Vector& b, const Cameras& cameras) const { + return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); + } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + // TODO should there be a lambda? + double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); + } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, + const Cameras& cameras, const double lambda) const { + return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + } + + /// Calculate vector of re-projection errors, before applying noise model + /// Assumes triangulation was done and degeneracy handled + Vector reprojectionError(const Cameras& cameras) const { + return Base::reprojectionError(cameras, point_); + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionError(const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + return reprojectionError(myCameras); + else + return zero(myCameras.size() * 2); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + */ + double totalReprojectionError(const Cameras& cameras, + boost::optional externalPoint = boost::none) const { + + size_t nrCameras; + if (externalPoint) { + nrCameras = this->keys_.size(); + point_ = *externalPoint; + degenerate_ = false; + cheiralityException_ = false; + } else { + nrCameras = this->triangulateSafe(cameras); + } + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // if we don't want to manage the exceptions we discard the factor + // std::cout << "In error evaluation: exception" << std::endl; + return 0.0; + } + + if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors + std::cout + << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" + << std::endl; + this->degenerate_ = true; + } + + if (this->degenerate_) { + // return 0.0; // TODO: this maybe should be zero? + std::cout + << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" + << std::endl; + size_t i = 0; + double overallError = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + if (i == 0) // first pose + this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity + Point2 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_); + } + } + + /// Cameras are computed in derived class + virtual Cameras cameras(const Values& values) const = 0; + + /** return the landmark */ + boost::optional point() const { + return point_; + } + + /** COMPUTE the landmark */ + boost::optional point(const Values& values) const { + triangulateSafe(values); + return point_; + } + + /** return the degenerate state */ + inline bool isDegenerate() const { + return (cheiralityException_ || degenerate_); + } + + /** return the cheirality status flag */ + inline bool isPointBehindCamera() const { + return cheiralityException_; + } + /** return chirality verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h new file mode 100644 index 000000000..06e82b2a7 --- /dev/null +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -0,0 +1,215 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartStereoProjectionPoseFactor.h + * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @author Luca Carlone + * @author Chris Beall + * @author Zsolt Kira + */ + +#pragma once + +#include "SmartStereoProjectionFactor.h" + +namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + */ + +/** + * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * @addtogroup SLAM + */ +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +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: + + /// shorthand for base class type + typedef SmartStereoProjectionFactor Base; + + /// shorthand for this class + typedef SmartStereoProjectionPoseFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * 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) + */ + SmartStereoProjectionPoseFactor(const double rankTol = 1, + const double linThreshold = -1, const bool manageDegeneracy = false, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + + /** Virtual destructor */ + virtual ~SmartStereoProjectionPoseFactor() {} + + /** + * 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 + */ + void add(const Point2 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); + } + + /** + * 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); + for (size_t i = 0; i < measurements.size(); i++) { + K_all_.push_back(Ks.at(i)); + } + } + + /** + * 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) { + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements.at(i), poseKeys.at(i), noise); + K_all_.push_back(K); + } + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + 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_) + K->print("calibration = "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + return e && Base::equals(p, tol); + } + + /// get the dimension of the factor + virtual size_t dim() const { + return 6 * this->keys_.size(); + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses corresponding + * to keys involved in this factor + * @return vector of Values + */ + typename Base::Cameras cameras(const Values& values) const { + typename Base::Cameras cameras; + size_t i=0; + BOOST_FOREACH(const Key& k, this->keys_) { + Pose3 pose = values.at(k); + typename Base::Camera camera(pose, *K_all_[i++]); + cameras.push_back(camera); + } + 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 : + 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 + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(K_all_); + } + +}; // end of class declaration + +} // \ namespace gtsam diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp new file mode 100644 index 000000000..c6192b1bd --- /dev/null +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -0,0 +1,1318 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TestSmartStereoProjectionPoseFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @date Sept 2013 + */ + +#include "../SmartStereoProjectionPoseFactor.h" + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +static bool isDebugTest = false; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; + +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); +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 bool manageDegeneracy = true; +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); + +static Key poseKey1(x1); +static Point2 measurement1(323.0, 240.0); +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; +typedef SmartStereoProjectionPoseFactor SmartFactorBundler; + +void stereo_projectToMultipleCameras( + SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, + vector& measurements_cam){ + + Point2 cam1_uv1 = cam1.project(landmark); + Point2 cam2_uv1 = cam2.project(landmark); + Point2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Constructor) { + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +//TEST( SmartStereoProjectionPoseFactor, Constructor2) { +// SmartFactor factor1(rankTol, linThreshold); +//} +// +///* ************************************************************************* */ +//TEST( SmartStereoProjectionPoseFactor, Constructor3) { +// SmartFactor::shared_ptr factor1(new SmartFactor()); +// factor1->add(measurement1, poseKey1, model, K); +//} +// +///* ************************************************************************* */ +//TEST( SmartStereoProjectionPoseFactor, Constructor4) { +// SmartFactor factor1(rankTol, linThreshold); +// factor1.add(measurement1, poseKey1, model, K); +//} +// +///* ************************************************************************* */ +//TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { +// bool manageDegeneracy = true; +// bool enableEPI = false; +// SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); +// factor1.add(measurement1, poseKey1, model, K); +//} +// +///* ************************************************************************* */ +//TEST( SmartStereoProjectionPoseFactor, Equals ) { +// SmartFactor::shared_ptr factor1(new SmartFactor()); +// factor1->add(measurement1, poseKey1, model, K); +// +// SmartFactor::shared_ptr factor2(new SmartFactor()); +// factor2->add(measurement1, poseKey1, model, K); +// +// CHECK(assert_equal(*factor1, *factor2)); +//} +// +///* *************************************************************************/ +//TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// SimpleCamera level_camera(level_pose, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera level_camera_right(level_pose_right, *K2); +// +// // landmark ~5 meters infront of camera +// Point3 landmark(5, 0.5, 1.2); +// +// // 1. Project two landmarks into two cameras and triangulate +// Point2 level_uv = level_camera.project(landmark); +// Point2 level_uv_right = level_camera_right.project(landmark); +// +// Values values; +// values.insert(x1, level_pose); +// values.insert(x2, level_pose_right); +// +// SmartFactor factor1; +// factor1.add(level_uv, x1, model, K); +// factor1.add(level_uv_right, x2, model, K); +// +// double actualError = factor1.error(values); +// double expectedError = 0.0; +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +// +// SmartFactor::Cameras cameras = factor1.cameras(values); +// double actualError2 = factor1.totalReprojectionError(cameras); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); +// +// // test vector of errors +// //Vector actual = factor1.unwhitenedError(values); +// //EXPECT(assert_equal(zero(4),actual,1e-8)); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, noisy ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// SimpleCamera level_camera(level_pose, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera level_camera_right(level_pose_right, *K2); +// +// // landmark ~5 meters infront of camera +// Point3 landmark(5, 0.5, 1.2); +// +// // 1. Project two landmarks into two cameras and triangulate +// Point2 pixelError(0.2,0.2); +// Point2 level_uv = level_camera.project(landmark) + pixelError; +// Point2 level_uv_right = level_camera_right.project(landmark); +// +// Values values; +// values.insert(x1, level_pose); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// values.insert(x2, level_pose_right.compose(noise_pose)); +// +// SmartFactor::shared_ptr factor1(new SmartFactor()); +// 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()); +// vector measurements; +// measurements.push_back(level_uv); +// measurements.push_back(level_uv_right); +// +// std::vector< SharedNoiseModel > noises; +// noises.push_back(model); +// noises.push_back(model); +// +// std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) +// Ks.push_back(K); +// Ks.push_back(K); +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// +// factor2->add(measurements, views, noises, Ks); +// +// double actualError2= factor2->error(values); +// +// DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +//} +// +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// smartFactor1->add(measurements_cam1, views, model, K2); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor()); +// smartFactor2->add(measurements_cam2, views, model, K2); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor()); +// smartFactor3->add(measurements_cam3, views, model, K2); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +//// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); +//// VectorValues delta = GFG->optimize(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor()); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor()); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// 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, boost::none, JACOBIAN_SVD)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +// +// double excludeLandmarksFutherThanDist = 2; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// 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, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// // All factors are disabled and pose should remain where it is +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(values.at(x3),result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +// +// double excludeLandmarksFutherThanDist = 1e10; +// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// Point3 landmark4(5, -0.5, 1); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); +// measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, +// JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, +// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, +// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, +// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor4->add(measurements_cam4, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(smartFactor4); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3); +// +// // All factors are disabled and pose should remain where it is +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// 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, boost::none, JACOBIAN_Q)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ +// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// typedef GenericProjectionFactor ProjectionFactor; +// NonlinearFactorGraph graph; +// +// // 1. Project three landmarks into three cameras and triangulate +// graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); +// +// graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); +// +// graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3* noise_pose); +// values.insert(L(1), landmark1); +// values.insert(L(2), landmark2); +// values.insert(L(3), landmark3); +// if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// Values result = optimizer.optimize(); +// +// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// SimpleCamera cam2(pose2, *K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// double rankTol = 10; +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); +// boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); +// boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); +// +// Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); +// +// boost::shared_ptr GaussianGraph = graph.linearize(values); +// Matrix GraphInformation = GaussianGraph->hessian().first; +// +// // Check Hessian +// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); +// +// Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + +// hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); +// +// // Check Information vector +// // cout << AugInformationMatrix.size() << endl; +// Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector +// +// // Check Hessian +// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// SimpleCamera cam2(pose2, *K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// SimpleCamera cam3(pose3, *K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// +// double rankTol = 50; +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, K2); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, K2); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = gtsam::Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2*noise_pose); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// SimpleCamera cam2(pose2, *K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// SimpleCamera cam3(pose3, *K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// double rankTol = 10; +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = gtsam::Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, Hessian ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// +// // 1. Project three landmarks into three cameras and triangulate +// Point2 cam1_uv1 = cam1.project(landmark1); +// Point2 cam2_uv1 = cam2.project(landmark1); +// vector measurements_cam1; +// measurements_cam1.push_back(cam1_uv1); +// measurements_cam1.push_back(cam2_uv1); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// smartFactor1->add(measurements_cam1,views, model, K2); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// +// boost::shared_ptr hessianFactor = smartFactor1->linearize(values); +// if(isDebugTest) hessianFactor->print("Hessian factor \n"); +// +// // compute triangulation from linearization point +// // compute reprojection errors (sum squared) +// // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) +// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +//} +// +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// SimpleCamera cam2(pose2, *K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// SimpleCamera cam3(pose3, *K); +// +// Point3 landmark1(5, 0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// +// SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); +// smartFactorInstance->add(measurements_cam1, views, model, K); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3); +// +// boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); +// // hessianFactor->print("Hessian factor \n"); +// +// Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); +// +// Values rotValues; +// rotValues.insert(x1, poseDrift.compose(pose1)); +// rotValues.insert(x2, poseDrift.compose(pose2)); +// rotValues.insert(x3, poseDrift.compose(pose3)); +// +// boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); +// // hessianFactorRot->print("Hessian factor \n"); +// +// // Hessian is invariant to rotations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); +// +// Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); +// +// Values tranValues; +// tranValues.insert(x1, poseDrift2.compose(pose1)); +// tranValues.insert(x2, poseDrift2.compose(pose2)); +// tranValues.insert(x3, poseDrift2.compose(pose3)); +// +// boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); +// +// // Hessian is invariant to rotations and translations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// SimpleCamera cam1(pose1, *K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); +// SimpleCamera cam2(pose2, *K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); +// SimpleCamera cam3(pose3, *K2); +// +// Point3 landmark1(5, 0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// +// SmartFactor::shared_ptr smartFactor(new SmartFactor()); +// smartFactor->add(measurements_cam1, views, model, K2); +// +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3); +// +// boost::shared_ptr hessianFactor = smartFactor->linearize(values); +// if(isDebugTest) hessianFactor->print("Hessian factor \n"); +// +// Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); +// +// Values rotValues; +// rotValues.insert(x1, poseDrift.compose(pose1)); +// rotValues.insert(x2, poseDrift.compose(pose2)); +// rotValues.insert(x3, poseDrift.compose(pose3)); +// +// boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); +// if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); +// +// // Hessian is invariant to rotations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); +// +// Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); +// +// Values tranValues; +// tranValues.insert(x1, poseDrift2.compose(pose1)); +// tranValues.insert(x2, poseDrift2.compose(pose2)); +// tranValues.insert(x3, poseDrift2.compose(pose3)); +// +// boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); +// +// // Hessian is invariant to rotations and translations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +//} +// +///* ************************************************************************* */ +//TEST( SmartStereoProjectionPoseFactor, ConstructorWithCal3Bundler) { +// SmartStereoProjectionPoseFactor factor1(rankTol, linThreshold); +// boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +// factor1.add(measurement1, poseKey1, model, Kbundler); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, Cal3Bundler ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: Cal3Bundler **********************" << endl; +// +// // 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), gtsam::Point3(0,0,1)); +// PinholeCamera cam1(pose1, *Kbundler); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// PinholeCamera cam2(pose2, *Kbundler); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// PinholeCamera cam3(pose3, *Kbundler); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// Point2 cam1_uv1 = cam1.project(landmark1); +// Point2 cam2_uv1 = cam2.project(landmark1); +// Point2 cam3_uv1 = cam3.project(landmark1); +// measurements_cam1.push_back(cam1_uv1); +// measurements_cam1.push_back(cam2_uv1); +// measurements_cam1.push_back(cam3_uv1); +// +// Point2 cam1_uv2 = cam1.project(landmark2); +// Point2 cam2_uv2 = cam2.project(landmark2); +// Point2 cam3_uv2 = cam3.project(landmark2); +// measurements_cam2.push_back(cam1_uv2); +// measurements_cam2.push_back(cam2_uv2); +// measurements_cam2.push_back(cam3_uv2); +// +// Point2 cam1_uv3 = cam1.project(landmark3); +// Point2 cam2_uv3 = cam2.project(landmark3); +// Point2 cam3_uv3 = cam3.project(landmark3); +// measurements_cam3.push_back(cam1_uv3); +// measurements_cam3.push_back(cam2_uv3); +// measurements_cam3.push_back(cam3_uv3); +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); +// smartFactor1->add(measurements_cam1, views, model, Kbundler); +// +// SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); +// smartFactor2->add(measurements_cam2, views, model, Kbundler); +// +// SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); +// smartFactor3->add(measurements_cam3, views, model, Kbundler); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); +// if(isDebugTest) tictoc_print_(); +// } +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, Cal3BundlerRotationOnly ){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // 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), gtsam::Point3(0,0,1)); +// PinholeCamera cam1(pose1, *Kbundler); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// PinholeCamera cam2(pose2, *Kbundler); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// PinholeCamera cam3(pose3, *Kbundler); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// Point2 cam1_uv1 = cam1.project(landmark1); +// Point2 cam2_uv1 = cam2.project(landmark1); +// Point2 cam3_uv1 = cam3.project(landmark1); +// measurements_cam1.push_back(cam1_uv1); +// measurements_cam1.push_back(cam2_uv1); +// measurements_cam1.push_back(cam3_uv1); +// +// Point2 cam1_uv2 = cam1.project(landmark2); +// Point2 cam2_uv2 = cam2.project(landmark2); +// Point2 cam3_uv2 = cam3.project(landmark2); +// measurements_cam2.push_back(cam1_uv2); +// measurements_cam2.push_back(cam2_uv2); +// measurements_cam2.push_back(cam3_uv2); +// +// Point2 cam1_uv3 = cam1.project(landmark3); +// Point2 cam2_uv3 = cam2.project(landmark3); +// Point2 cam3_uv3 = cam3.project(landmark3); +// measurements_cam3.push_back(cam1_uv3); +// measurements_cam3.push_back(cam2_uv3); +// measurements_cam3.push_back(cam3_uv3); +// +// double rankTol = 10; +// +// SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, Kbundler); +// +// SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, Kbundler); +// +// SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); +// smartFactor3->add(measurements_cam3, views, model, Kbundler); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = gtsam::Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* ************************************************************************* */ +//int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +///* ************************************************************************* */ + +