From 7a2af9999ae4f1a54d7af645411e0f6aff0ef1d8 Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Tue, 3 Jun 2014 11:21:11 -0400 Subject: [PATCH 01/89] 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); } +///* ************************************************************************* */ + + From fb50e20c820f08f35f32b4cb9ae71fe577d0800a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 3 Jun 2014 15:24:41 -0400 Subject: [PATCH 02/89] template on measurement so we can later have StereoPoint2 instead of Point2 --- gtsam/slam/SmartFactorBase.h | 199 ++++++++++++----------- gtsam/slam/SmartProjectionFactor.h | 4 +- gtsam/slam/SmartStereoProjectionFactor.h | 4 +- 3 files changed, 104 insertions(+), 103 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1235fc6fb..29bb26437 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -36,31 +36,32 @@ namespace gtsam { /// Base class with no internal point, completely functional -template +template class SmartFactorBase: public NonlinearFactor { + protected: // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views + std::vector measured_; ///< 2D measurement for each of the m views std::vector noise_; ///< noise model used ///< (important that the order is the same as the keys that we use to create the factor) boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; + typedef Eigen::Matrix Matrix2; /// shorthand for base class type typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; + typedef SmartFactorBase This; public: @@ -89,7 +90,7 @@ public: * @param poseKey is the index corresponding to the camera observing the landmark * @param noise_i is the measurement noise */ - void add(const Point2& measured_i, const Key& poseKey_i, + void add(const Z& measured_i, const Key& poseKey_i, const SharedNoiseModel& noise_i) { this->measured_.push_back(measured_i); this->keys_.push_back(poseKey_i); @@ -100,7 +101,7 @@ public: * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises */ // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); @@ -113,7 +114,7 @@ public: * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them */ // **************************************************************************************************** - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); @@ -127,16 +128,16 @@ public: * The noise is assumed to be the same for all measurements */ // **************************************************************************************************** - void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) { - 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); - this->noise_.push_back(noise); - } - } +// void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) { +// 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); +// this->noise_.push_back(noise); +// } +// } /** return the measurements */ - const std::vector& measured() const { + const std::vector& measured() const { return measured_; } @@ -179,27 +180,27 @@ public: } // **************************************************************************************************** - /// Calculate vector of re-projection errors, before applying noise model - Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - - Vector b = zero(2 * cameras.size()); - - size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); - try { - Point2 e(camera.project(point) - zi); - b[2 * i] = e.x(); - b[2 * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; - } - - return b; - } +// /// Calculate vector of re-projection errors, before applying noise model +// Vector reprojectionError(const Cameras& cameras, const Point3& point) const { +// +// Vector b = zero(2 * cameras.size()); +// +// size_t i = 0; +// BOOST_FOREACH(const Camera& camera, cameras) { +// const Z& zi = this->measured_.at(i); +// try { +// Z e(camera.project(point) - zi); +// b[2 * i] = e.x(); +// b[2 * i + 1] = e.y(); +// } catch (CheiralityException& e) { +// std::cout << "Cheirality exception " << std::endl; +// exit(EXIT_FAILURE); +// } +// i += 1; +// } +// +// return b; +// } // **************************************************************************************************** /** @@ -216,9 +217,9 @@ public: size_t i = 0; BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); + const Z& zi = this->measured_.at(i); try { - Point2 reprojectionError(camera.project(point) - zi); + Z reprojectionError(camera.project(point) - zi); overallError += 0.5 * this->noise_.at(i)->distance(reprojectionError.vector()); } catch (CheiralityException&) { @@ -232,28 +233,28 @@ public: // **************************************************************************************************** /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { - - int numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(2, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block<2, 3>(2 * i, 0) = Ei; - } - - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); - } +// void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, +// const Point3& point) const { +// +// int numKeys = this->keys_.size(); +// E = zeros(2 * numKeys, 3); +// Vector b = zero(2 * numKeys); +// +// Matrix Ei(2, 3); +// for (size_t i = 0; i < this->measured_.size(); i++) { +// try { +// cameras[i].project(point, boost::none, Ei); +// } catch (CheiralityException& e) { +// std::cout << "Cheirality exception " << std::endl; +// exit(EXIT_FAILURE); +// } +// this->noise_.at(i)->WhitenSystem(Ei, b); +// E.block<2, 3>(2 * i, 0) = Ei; +// } +// +// // Matrix PointCov; +// PointCov.noalias() = (E.transpose() * E).inverse(); +// } // **************************************************************************************************** /// Compute F, E only (called below in both vanilla and SVD versions) @@ -262,11 +263,11 @@ public: Vector& b, const Cameras& cameras, const Point3& point) const { size_t numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 3); - b = zero(2 * numKeys); + E = zeros(Z::Dim() * numKeys, 3); + b = zero(Z::Dim() * numKeys); double f = 0; - Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, D); + Matrix Fi(Z::Dim(), 6), Ei(Z::Dim(), 3), Hcali(Z::Dim(), D - 6), Hcam(Z::Dim(), D); for (size_t i = 0; i < this->measured_.size(); i++) { Vector bi; @@ -283,12 +284,12 @@ public: if (D == 6) { // optimize only camera pose Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); } else { - Hcam.block<2, 6>(0, 0) = Fi; // 2 x 6 block for the cameras - Hcam.block<2, D - 6>(0, 6) = Hcali; // 2 x nrCal block for the cameras + Hcam.block(0, 0) = Fi; // Z::Dim() x 6 block for the cameras + Hcam.block(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); } - E.block<2, 3>(2 * i, 0) = Ei; - subInsert(b, bi, 2 * i); + E.block(Z::dimension * i, 0) = Ei; + subInsert(b, bi, Z::Dim() * i); } return f; } @@ -329,10 +330,10 @@ public: std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); - F = zeros(2 * numKeys, D * numKeys); + F = zeros(Z::Dim() * numKeys, D * numKeys); for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras } return f; } @@ -351,9 +352,9 @@ public: // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(2 * numKeys, 2 * numKeys - 3); + // Enull = zeros(Z::Dim() * numKeys, Z::Dim() * numKeys - 3); size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns + Enull = svd.matrixU().block(0, 3, Z::Dim() * numKeys, Z::Dim() * numKeys - 3); // last Z::Dim()m-3 columns return f; } @@ -367,11 +368,11 @@ public: int numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(2 * numKeys, D * numKeys); + F.resize(Z::Dim() * numKeys, D * numKeys); F.setZero(); for (size_t i = 0; i < this->keys_.size(); ++i) - F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras return f; } @@ -432,9 +433,9 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(2 * numKeys, D * numKeys); + Matrix F = zeros(Z::Dim() * numKeys, D * numKeys); for (size_t i = 0; i < this->keys_.size(); ++i) - F.block<2, D>(2 * i, D * i) = Fblocks.at(i).second; // 2 x 6 block for the cameras + F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras Matrix H(D * numKeys, D * numKeys); Vector gs_vector; @@ -472,16 +473,16 @@ public: for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + const Matrix23 Ei1_P = E.block(Z::Dim() * i1, 0) * P; // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment(Z::Dim() * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(Z::Dim() * i1, 0).transpose() * Fi1); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -489,7 +490,7 @@ public: // (DxD) = (Dx2) * ( (2x2) * (2xD) ) augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + * (Ei1_P * E.block(Z::Dim() * i2, 0).transpose() * Fi2); } } // end of for over cameras } @@ -516,16 +517,16 @@ public: // X X X X 14 const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + const Matrix23 Ei1_P = E.block(Z::Dim() * i1, 0) * P; { // for i1 = i2 // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + gs.at(i1) = Fi1.transpose() * b.segment(Z::Dim() * i1) // F' * b + -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(Z::Dim() * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian @@ -534,7 +535,7 @@ public: // (DxD) = (Dx2) * ( (2x2) * (2xD) ) Gs.at(GsIndex) = -Fi1.transpose() - * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + * (Ei1_P * E.block(Z::Dim() * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras @@ -582,9 +583,9 @@ public: for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + const Matrix23 Ei1_P = E.block(Z::Dim() * i1, 0) * P; - // D = (Dx2) * (2) + // D = (DxZ::Dim()) * (Z::Dim()) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) // Key cameraKey_i1 = this->keys_[i1]; @@ -594,15 +595,15 @@ public: // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() - + Fi1.transpose() * b.segment<2>(2 * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + Fi1.transpose() * b.segment(Z::Dim() * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); // add contribution of current factor augmentedHessian(aug_i1, aug_i1) = matrixBlock + - ( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) ); + ( Fi1.transpose() * (Fi1 - Ei1_P * E.block(Z::Dim() * i1, 0).transpose() * Fi1) ); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -611,12 +612,12 @@ public: //Key cameraKey_i2 = this->keys_[i2]; DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xZ::Dim()) * (Z::Dim()xD) ) // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // add contribution of current factor augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + - Fi1.transpose() * (Ei1_P * E.block(Z::Dim() * i2, 0).transpose() * Fi2); } } // end of for over cameras @@ -654,7 +655,7 @@ public: size_t numKeys = this->keys_.size(); std::vector < KeyMatrix2D > Fblocks; Vector b; - Matrix Enull(2*numKeys, 2*numKeys-3); + Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 043528fea..b94b7239f 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -62,7 +62,7 @@ enum LinearizationMode { * TODO: why LANDMARK parameter? */ template -class SmartProjectionFactor: public SmartFactorBase { +class SmartProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 4c8403a99..cd9052f14 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -62,7 +62,7 @@ enum LinearizationMode { * TODO: why LANDMARK parameter? */ template -class SmartStereoProjectionFactor: public SmartFactorBase { +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate From c583564098b7a7bd0802fdf989f753336e82b1eb Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 3 Jun 2014 15:24:50 -0400 Subject: [PATCH 03/89] uncomment test main --- gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index c6192b1bd..e31693f46 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -1310,9 +1310,9 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) { // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} -// -///* ************************************************************************* */ -//int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -///* ************************************************************************* */ + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From 6d33b3c24e880cb7c4f384a6cfc1b33f175b9031 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 3 Jun 2014 16:13:50 -0400 Subject: [PATCH 04/89] template base class on Camera instead of Calibration --- gtsam/slam/SmartFactorBase.h | 9 ++++----- gtsam/slam/SmartProjectionFactor.h | 4 ++-- gtsam/slam/SmartStereoProjectionFactor.h | 4 ++-- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 29bb26437..7c5abca53 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -36,7 +36,7 @@ namespace gtsam { /// Base class with no internal point, completely functional -template +template class SmartFactorBase: public NonlinearFactor { protected: @@ -61,7 +61,7 @@ protected: typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; + typedef SmartFactorBase This; public: @@ -69,8 +69,7 @@ public: typedef boost::shared_ptr shared_ptr; /// shorthand for a pinhole camera - typedef PinholeCamera Camera; - typedef std::vector Cameras; + typedef std::vector Cameras; /** * Constructor @@ -216,7 +215,7 @@ public: double overallError = 0; size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { + BOOST_FOREACH(const CAMERA& camera, cameras) { const Z& zi = this->measured_.at(i); try { Z reprojectionError(camera.project(point) - zi); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index b94b7239f..04112e239 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -62,7 +62,7 @@ enum LinearizationMode { * TODO: why LANDMARK parameter? */ template -class SmartProjectionFactor: public SmartFactorBase { +class SmartProjectionFactor: public SmartFactorBase, D> { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index cd9052f14..56e5fdbf1 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -62,7 +62,7 @@ enum LinearizationMode { * TODO: why LANDMARK parameter? */ template -class SmartStereoProjectionFactor: public SmartFactorBase { +class SmartStereoProjectionFactor: public SmartFactorBase, D> { protected: // Some triangulation parameters @@ -92,7 +92,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate From 49ae04dc473a391cbf3df30487ec23d487a486ef Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 3 Jun 2014 16:51:03 -0400 Subject: [PATCH 05/89] start commenting out some unused code --- gtsam/slam/SmartFactorBase.h | 3 +- gtsam/slam/SmartStereoProjectionFactor.h | 146 +++++++++---------- gtsam/slam/SmartStereoProjectionPoseFactor.h | 12 +- 3 files changed, 80 insertions(+), 81 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 7c5abca53..69f92e402 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -25,10 +25,9 @@ #include "RegularHessianFactor.h" #include -#include +#include // for Cheirality exception #include #include -#include #include #include diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 56e5fdbf1..f93fb468b 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -408,44 +408,44 @@ public: 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_); - } +// // 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, @@ -546,41 +546,41 @@ public: } // 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_); - } +// /// 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, diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 06e82b2a7..4077071d9 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -172,12 +172,12 @@ public: 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; +// 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; From 5f56d70000e4192cad99334c77484465f712bac2 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 3 Jun 2014 17:11:01 -0400 Subject: [PATCH 06/89] comment out things that wouldn't work with conversion to Stereo --- gtsam/slam/SmartStereoProjectionFactor.h | 44 ++++++++++++++---------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index f93fb468b..9bfe72eae 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -110,7 +111,10 @@ public: typedef boost::shared_ptr shared_ptr; /// shorthand for a pinhole camera + // TODO: Switch to stereoCamera: typedef PinholeCamera Camera; +// typedef StereoCamera Camera; + typedef std::vector Cameras; /** @@ -243,8 +247,10 @@ public: // We triangulate the 3D position of the landmark try { // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(cameras, this->measured_, - rankTolerance_, enableEPI_); + + //TODO: Chris will replace this with something else for stereo +// point_ = triangulatePoint3(cameras, this->measured_, +// rankTolerance_, enableEPI_); degenerate_ = false; cheiralityException_ = false; @@ -639,23 +645,23 @@ public: } 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; + 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_); From c13569df4c659e5c27f9b819e5a93f4790f7c9ea Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Thu, 5 Jun 2014 09:25:06 -0400 Subject: [PATCH 07/89] Converted stereo factors to use stereo points and cameras; added operator<< to StereoPoint2 (STILL CAUSES LNK ERROR) --- gtsam/geometry/Cal3_S2Stereo.h | 5 +++++ gtsam/geometry/StereoPoint2.cpp | 5 +++++ gtsam/geometry/StereoPoint2.h | 2 ++ gtsam/slam/SmartFactorBase.h | 1 + gtsam/slam/SmartStereoProjectionFactor.h | 14 +++++++------- gtsam/slam/SmartStereoProjectionPoseFactor.h | 8 ++++---- .../testSmartStereoProjectionPoseFactor.cpp | 19 ++++++++++--------- 7 files changed, 34 insertions(+), 20 deletions(-) diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 811264967..b47153547 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -52,6 +52,11 @@ namespace gtsam { /// constructor from vector Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){} + /// easy constructor; field-of-view in degrees, assumes zero skew + Cal3_S2Stereo(double fov, int w, int h, double b) : + K_(fov, w, h), b_(b) { + } + /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index f599a2dea..4aaa513f5 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -26,3 +26,8 @@ void StereoPoint2::print(const string& s) const { cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl; } /* ************************************************************************* */ + +ostream &operator<<(ostream &os, const StereoPoint2& p) { + os << '(' << p.uL() << ", " << p.uR() << p.v() << ')'; + return os; +} diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8ce2e49bf..8e420fc16 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -88,6 +88,8 @@ namespace gtsam { StereoPoint2 operator-(const StereoPoint2& b) const { return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } + + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); /// @} /// @name Manifold diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 69f92e402..eab7810db 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -26,6 +26,7 @@ #include #include // for Cheirality exception +#include #include #include diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 9bfe72eae..9fa1a2b27 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -63,7 +63,7 @@ enum LinearizationMode { * TODO: why LANDMARK parameter? */ template -class SmartStereoProjectionFactor: public SmartFactorBase, D> { +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -93,7 +93,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase, D> Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -112,7 +112,7 @@ public: /// shorthand for a pinhole camera // TODO: Switch to stereoCamera: - typedef PinholeCamera Camera; + typedef StereoCamera Camera; // typedef StereoCamera Camera; typedef std::vector Cameras; @@ -264,9 +264,9 @@ public: degenerate_ = true; break; } - const Point2& zi = this->measured_.at(i); + const StereoPoint2& zi = this->measured_.at(i); try { - Point2 reprojectionError(camera.project(point_) - zi); + StereoPoint2 reprojectionError(camera.project(point_) - zi); totalReprojError += reprojectionError.vector().norm(); } catch (CheiralityException) { cheiralityException_ = true; @@ -652,10 +652,10 @@ public: // size_t i = 0; // double overallError = 0; // BOOST_FOREACH(const Camera& camera, cameras) { -// const Point2& zi = this->measured_.at(i); +// const StereoPoint2& zi = this->measured_.at(i); // if (i == 0) // first pose // this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity -// Point2 reprojectionError( +// StereoPoint2 reprojectionError( // camera.projectPointAtInfinity(this->point_) - zi); // overallError += 0.5 // * this->noise_.at(i)->distance(reprojectionError.vector()); diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 4077071d9..b6fad38fa 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -83,7 +83,7 @@ public: * @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, + 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); @@ -97,7 +97,7 @@ public: * @param noises vector of measurement noises * @param Ks vector of calibration objects */ - void add(std::vector measurements, std::vector poseKeys, + void add(std::vector measurements, std::vector poseKeys, std::vector noises, std::vector > Ks) { Base::add(measurements, poseKeys, noises); @@ -113,7 +113,7 @@ public: * @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, + 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); @@ -157,7 +157,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++]); + typename Base::Camera camera(pose, K_all_[i++]); cameras.push_back(camera); } return cameras; diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index e31693f46..5952dd9bb 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -36,9 +36,10 @@ static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees static size_t w=640,h=480; +static double b = 1; -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 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; @@ -57,19 +58,19 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Key poseKey1(x1); -static Point2 measurement1(323.0, 240.0); +static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? 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 SmartFactor; typedef SmartStereoProjectionPoseFactor SmartFactorBundler; void stereo_projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ + StereoCamera cam1, StereoCamera cam2, StereoCamera cam3, Point3 landmark, + vector& measurements_cam){ - Point2 cam1_uv1 = cam1.project(landmark); - Point2 cam2_uv1 = cam2.project(landmark); - Point2 cam3_uv1 = cam3.project(landmark); + StereoPoint2 cam1_uv1 = cam1.project(landmark); + StereoPoint2 cam2_uv1 = cam2.project(landmark); + StereoPoint2 cam3_uv1 = cam3.project(landmark); measurements_cam.push_back(cam1_uv1); measurements_cam.push_back(cam2_uv1); measurements_cam.push_back(cam3_uv1); From a343c947b4418914613c3f579e0311088cb0f0a4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 5 Jun 2014 10:59:06 -0400 Subject: [PATCH 08/89] fix linker error, namespace issue --- gtsam/geometry/StereoPoint2.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index 4aaa513f5..cd6f09507 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -19,15 +19,18 @@ #include using namespace std; -using namespace gtsam; + +namespace gtsam { /* ************************************************************************* */ void StereoPoint2::print(const string& s) const { cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl; } -/* ************************************************************************* */ +/* ************************************************************************* */ ostream &operator<<(ostream &os, const StereoPoint2& p) { - os << '(' << p.uL() << ", " << p.uR() << p.v() << ')'; + os << '(' << p.uL() << ", " << p.uR() << ", " << p.v() << ')'; return os; } + +} // namespace gtsam From 2120c7bcb911584c9c387347f28cb27cc772c75e Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 5 Jun 2014 11:00:38 -0400 Subject: [PATCH 09/89] Move friend to conventional place because it doesn't belong in "Group" --- gtsam/geometry/StereoPoint2.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8e420fc16..186afae55 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -89,8 +89,6 @@ namespace gtsam { return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); - /// @} /// @name Manifold /// @{ @@ -154,6 +152,9 @@ namespace gtsam { return Point2(uR_, v_); } + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); + private: /// @} From f84817e572774b126dbd941659ff6c0155c89570 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 5 Jun 2014 11:01:23 -0400 Subject: [PATCH 10/89] comment out degenerate part and throw --- gtsam/slam/SmartStereoProjectionFactor.h | 66 ++++++++++++------------ 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 9fa1a2b27..0d0d2bc0f 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -512,40 +512,40 @@ public: /// 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; + 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; +// } +// +// 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_); From e376ad8cecc7cb5881720b4cdb0b408ca3edda2d Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Mon, 9 Jun 2014 00:20:59 -0400 Subject: [PATCH 11/89] Completed SmartStereoProjectionPoseFactor unit tests --- .../testSmartStereoProjectionPoseFactor.cpp | 2115 +++++++++-------- 1 file changed, 1058 insertions(+), 1057 deletions(-) diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 5952dd9bb..5cdb4ff04 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -82,1084 +83,1084 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) { } /* ************************************************************************* */ -//TEST( SmartStereoProjectionPoseFactor, Constructor2) { -// SmartFactor factor1(rankTol, linThreshold); -//} -// +TEST( SmartStereoProjectionPoseFactor, Constructor2) { + SmartFactor factor1(rankTol, linThreshold); +} + ///* ************************************************************************* */ -//TEST( SmartStereoProjectionPoseFactor, Constructor3) { -// SmartFactor::shared_ptr factor1(new SmartFactor()); -// factor1->add(measurement1, poseKey1, model, K); -//} -// +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, 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, 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)); + StereoCamera 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)); + StereoCamera 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 + StereoPoint2 level_uv = level_camera.project(landmark); + StereoPoint2 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)); + StereoCamera 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)); + StereoCamera 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 + StereoPoint2 pixelError(0.2,0.2,0); + StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; + StereoPoint2 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)); + StereoCamera cam1(pose1, K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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)); + StereoCamera cam1(pose1, K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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) + StereoPoint2(10,10,1); // 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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)); + StereoCamera cam1(pose1, K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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 GenericStereoFactor 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera cam1(pose1, K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera 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 + StereoPoint2 cam1_uv1 = cam1.project(landmark1); + StereoPoint2 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)); + StereoCamera cam1(pose1, K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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)); + StereoCamera cam1(pose1, K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + StereoCamera cam2(pose2, K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + StereoCamera 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); +// StereoCamera 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); +// StereoCamera 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); +// StereoCamera 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; +// 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); +// StereoPoint2 cam1_uv1 = cam1.project(landmark1); +// StereoPoint2 cam2_uv1 = cam2.project(landmark1); +// StereoPoint2 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); +// StereoPoint2 cam1_uv2 = cam1.project(landmark2); +// StereoPoint2 cam2_uv2 = cam2.project(landmark2); +// StereoPoint2 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); +// StereoPoint2 cam1_uv3 = cam1.project(landmark3); +// StereoPoint2 cam2_uv3 = cam2.project(landmark3); +// StereoPoint2 cam3_uv3 = cam3.project(landmark3); // measurements_cam3.push_back(cam1_uv3); // measurements_cam3.push_back(cam2_uv3); // measurements_cam3.push_back(cam3_uv3); @@ -1212,8 +1213,8 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) { // EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); // if(isDebugTest) tictoc_print_(); // } -// -///* *************************************************************************/ + +/* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, Cal3BundlerRotationOnly ){ // // std::vector views; @@ -1223,41 +1224,41 @@ TEST( SmartStereoProjectionPoseFactor, Constructor) { // // // 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); +// StereoCamera 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); +// StereoCamera 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); +// StereoCamera 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; +// 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); +// StereoPoint2 cam1_uv1 = cam1.project(landmark1); +// StereoPoint2 cam2_uv1 = cam2.project(landmark1); +// StereoPoint2 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); +// StereoPoint2 cam1_uv2 = cam1.project(landmark2); +// StereoPoint2 cam2_uv2 = cam2.project(landmark2); +// StereoPoint2 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); +// StereoPoint2 cam1_uv3 = cam1.project(landmark3); +// StereoPoint2 cam2_uv3 = cam2.project(landmark3); +// StereoPoint2 cam3_uv3 = cam3.project(landmark3); // measurements_cam3.push_back(cam1_uv3); // measurements_cam3.push_back(cam2_uv3); // measurements_cam3.push_back(cam3_uv3); From 711c3c071592520d903c4f7176101aee0c3ab6a2 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 9 Jun 2014 16:29:32 -0400 Subject: [PATCH 12/89] temporary triangulation using only first camera --- gtsam/slam/SmartStereoProjectionFactor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 0d0d2bc0f..01c21e6a0 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -251,6 +251,11 @@ public: //TODO: Chris will replace this with something else for stereo // point_ = triangulatePoint3(cameras, this->measured_, // rankTolerance_, enableEPI_); + + // FIXME: temporary: triangulation using only first camera + const StereoPoint2& z0 = this->measured_.at(0); + point_ = cameras[0].backproject(z0); + degenerate_ = false; cheiralityException_ = false; From 608498ce894258dc03602e8b673f10aa57ae1430 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 9 Jun 2014 16:30:37 -0400 Subject: [PATCH 13/89] minor fixes, now runs without exceptions, but some tests still fail --- .../testSmartStereoProjectionPoseFactor.cpp | 213 +----------------- 1 file changed, 7 insertions(+), 206 deletions(-) diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 5cdb4ff04..4e3cbf70d 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -47,7 +47,7 @@ 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)); +static SharedNoiseModel model(noiseModel::Unit::Create(3)); // Convenience for named keys using symbol_shorthand::X; @@ -87,19 +87,19 @@ 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; @@ -107,7 +107,7 @@ TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { factor1.add(measurement1, poseKey1, model, K); } -///* ************************************************************************* */ +/* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { SmartFactor::shared_ptr factor1(new SmartFactor()); factor1->add(measurement1, poseKey1, model, K); @@ -142,8 +142,8 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ values.insert(x2, level_pose_right); SmartFactor factor1; - factor1.add(level_uv, x1, model, K); - factor1.add(level_uv_right, x2, model, K); + factor1.add(level_uv, x1, model, K2); + factor1.add(level_uv_right, x2, model, K2); double actualError = factor1.error(values); double expectedError = 0.0; @@ -1113,205 +1113,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ 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)); -// StereoCamera cam1(pose1, *Kbundler); -// -// // create second camera 1 meter to the right of first camera -// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); -// StereoCamera cam2(pose2, *Kbundler); -// -// // create third camera 1 meter above the first camera -// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); -// StereoCamera 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 -// StereoPoint2 cam1_uv1 = cam1.project(landmark1); -// StereoPoint2 cam2_uv1 = cam2.project(landmark1); -// StereoPoint2 cam3_uv1 = cam3.project(landmark1); -// measurements_cam1.push_back(cam1_uv1); -// measurements_cam1.push_back(cam2_uv1); -// measurements_cam1.push_back(cam3_uv1); -// -// StereoPoint2 cam1_uv2 = cam1.project(landmark2); -// StereoPoint2 cam2_uv2 = cam2.project(landmark2); -// StereoPoint2 cam3_uv2 = cam3.project(landmark2); -// measurements_cam2.push_back(cam1_uv2); -// measurements_cam2.push_back(cam2_uv2); -// measurements_cam2.push_back(cam3_uv2); -// -// StereoPoint2 cam1_uv3 = cam1.project(landmark3); -// StereoPoint2 cam2_uv3 = cam2.project(landmark3); -// StereoPoint2 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)); -// StereoCamera 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)); -// StereoCamera 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)); -// StereoCamera 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 -// StereoPoint2 cam1_uv1 = cam1.project(landmark1); -// StereoPoint2 cam2_uv1 = cam2.project(landmark1); -// StereoPoint2 cam3_uv1 = cam3.project(landmark1); -// measurements_cam1.push_back(cam1_uv1); -// measurements_cam1.push_back(cam2_uv1); -// measurements_cam1.push_back(cam3_uv1); -// -// StereoPoint2 cam1_uv2 = cam1.project(landmark2); -// StereoPoint2 cam2_uv2 = cam2.project(landmark2); -// StereoPoint2 cam3_uv2 = cam3.project(landmark2); -// measurements_cam2.push_back(cam1_uv2); -// measurements_cam2.push_back(cam2_uv2); -// measurements_cam2.push_back(cam3_uv2); -// -// StereoPoint2 cam1_uv3 = cam1.project(landmark3); -// StereoPoint2 cam2_uv3 = cam2.project(landmark3); -// StereoPoint2 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); } From a3b001be363f7dc29141539388cb7bada8acce54 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 9 Jun 2014 20:39:04 -0400 Subject: [PATCH 14/89] tiny fix to return vector of dim 3 instead 2 --- gtsam/slam/SmartStereoProjectionFactor.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 01c21e6a0..679a6548d 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -110,11 +110,10 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - // TODO: Switch to stereoCamera: + /// shorthand for a StereoCamera // TODO: Get rid of this? typedef StereoCamera Camera; -// typedef StereoCamera Camera; + /// Vector of cameras typedef std::vector Cameras; /** @@ -278,6 +277,7 @@ public: } i += 1; } + std::cout << "totalReprojError error: " << totalReprojError << std::endl; // we discard smart factors that have large reprojection error if(dynamicOutlierRejectionThreshold_ > 0 && totalReprojError/m > dynamicOutlierRejectionThreshold_) @@ -612,7 +612,7 @@ public: if (nonDegenerate) return reprojectionError(myCameras); else - return zero(myCameras.size() * 2); + return zero(myCameras.size() * 3); } /** From 2eef2c14b55efb3e4c607d3034ad33ce837f2abb Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Tue, 1 Jul 2014 08:40:27 -0400 Subject: [PATCH 15/89] Fixed abort due to unaligned memory allocation in SmartFactorBase --- .../Eigen/Eigen/src/StlSupport/StdVector.h | 3 ++- gtsam/slam/JacobianFactorQ.h | 2 +- gtsam/slam/JacobianFactorSVD.h | 2 +- gtsam/slam/SmartFactorBase.h | 15 +++++++++------ gtsam/slam/SmartStereoProjectionFactor.h | 2 ++ gtsam/slam/SmartStereoProjectionPoseFactor.h | 2 ++ .../tests/testSmartStereoProjectionPoseFactor.cpp | 1 + 7 files changed, 18 insertions(+), 9 deletions(-) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h index 40a9abefa..ea3b571c2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h @@ -11,7 +11,8 @@ #ifndef EIGEN_STDVECTOR_H #define EIGEN_STDVECTOR_H -#include "Eigen/src/StlSupport/details.h" +//#include "Eigen/src/StlSupport/details.h" +#include "gtsam/3rdparty/Eigen/Eigen/src/StlSupport/details.h" /** * This section contains a convenience MACRO which allows an easy specialization of diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..1e264ccba 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -37,7 +37,7 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector>& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e28185038..558a6c740 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -36,7 +36,7 @@ public: } /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, + JacobianFactorSVD(const std::vector>& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index eab7810db..a185cefd5 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { /// Base class with no internal point, completely functional @@ -65,6 +66,8 @@ protected: public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -258,7 +261,7 @@ public: // **************************************************************************************************** /// Compute F, E only (called below in both vanilla and SVD versions) /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector& Fblocks, Matrix& E, + double computeJacobians(std::vector>& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { size_t numKeys = this->keys_.size(); @@ -295,7 +298,7 @@ public: // **************************************************************************************************** /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, Matrix& E, + double computeJacobians(std::vector>& Fblocks, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -326,7 +329,7 @@ public: const double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector Fblocks; + std::vector> Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(Z::Dim() * numKeys, D * numKeys); @@ -339,7 +342,7 @@ public: // **************************************************************************************************** /// SVD version - double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + double computeJacobiansSVD(std::vector>& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -639,7 +642,7 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; + std::vector> Fblocks; Matrix E; Matrix3 PointCov; Vector b; @@ -652,7 +655,7 @@ public: boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector < KeyMatrix2D > Fblocks; + std::vector < KeyMatrix2D,Eigen::aligned_allocator > Fblocks; Vector b; Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 679a6548d..96f5568dd 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -43,6 +43,8 @@ protected: public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SmartStereoProjectionFactorState() { } // Hessian representation (after Schur complement) diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index b6fad38fa..9b7763ef2 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -47,6 +47,8 @@ protected: public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// shorthand for base class type typedef SmartStereoProjectionFactor Base; diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4e3cbf70d..b5ee3d61a 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -79,6 +79,7 @@ void stereo_projectToMultipleCameras( /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { + fprintf(stderr,"Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } From 391a29bcaf6de4a89067894595380b06cc516ad2 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 07:51:24 -0400 Subject: [PATCH 16/89] const correctness --- gtsam/geometry/StereoPoint2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 186afae55..4093413ca 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -143,12 +143,12 @@ namespace gtsam { } /** convenient function to get a Point2 from the left image */ - inline Point2 point2(){ + inline Point2 point2() const { return Point2(uL_, v_); } /** convenient function to get a Point2 from the right image */ - inline Point2 right(){ + inline Point2 const right(){ return Point2(uR_, v_); } From fc513e584d28fd1dc02ba1518a65f38ad9600e58 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 07:55:52 -0400 Subject: [PATCH 17/89] move derivative for calibration to third slot to be consistent with PinholeCamera interface --- gtsam/geometry/StereoCamera.cpp | 3 ++- gtsam/geometry/StereoCamera.h | 17 +++++++---------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index ed531a2bd..7412c42f6 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,7 +30,8 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2) const { + boost::optional H1, boost::optional H2, + boost::optional H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 70917b2c4..1e23bf826 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -114,21 +114,18 @@ public: /* * project 3D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const; - /* - * to accomodate tsam's assumption that K is estimated, too + /** + * */ - StereoPoint2 project(const Point3& point, - boost::optional H1, - boost::optional H1_k, - boost::optional H2) const { - return project(point, H1, H2); - } - Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); From 0801246058c72af0248b2c21bdde65fa2288e03f Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 07:57:19 -0400 Subject: [PATCH 18/89] revert include path change --- gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h index ea3b571c2..40a9abefa 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h @@ -11,8 +11,7 @@ #ifndef EIGEN_STDVECTOR_H #define EIGEN_STDVECTOR_H -//#include "Eigen/src/StlSupport/details.h" -#include "gtsam/3rdparty/Eigen/Eigen/src/StlSupport/details.h" +#include "Eigen/src/StlSupport/details.h" /** * This section contains a convenience MACRO which allows an easy specialization of From d191b0cebbe840b322ea66f5ea788044f66bc4a0 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 07:58:56 -0400 Subject: [PATCH 19/89] space between double angle-brackets --- gtsam/slam/JacobianFactorQ.h | 2 +- gtsam/slam/JacobianFactorSVD.h | 2 +- gtsam/slam/SmartFactorBase.h | 13 +++++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 1e264ccba..de13f7ef0 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -37,7 +37,7 @@ public: } /// Constructor - JacobianFactorQ(const std::vector>& Fblocks, + JacobianFactorQ(const std::vector >& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 558a6c740..0d3c04de1 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -36,7 +36,7 @@ public: } /// Constructor - JacobianFactorSVD(const std::vector>& Fblocks, const Matrix& Enull, const Vector& b, + JacobianFactorSVD(const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index a185cefd5..ae2dc8b0f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -33,7 +33,8 @@ #include #include #include -#include +#include +//#include namespace gtsam { /// Base class with no internal point, completely functional @@ -261,7 +262,7 @@ public: // **************************************************************************************************** /// Compute F, E only (called below in both vanilla and SVD versions) /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector>& Fblocks, Matrix& E, + double computeJacobians(std::vector >& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { size_t numKeys = this->keys_.size(); @@ -298,7 +299,7 @@ public: // **************************************************************************************************** /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector>& Fblocks, Matrix& E, + double computeJacobians(std::vector >& Fblocks, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -329,7 +330,7 @@ public: const double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector> Fblocks; + std::vector > Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(Z::Dim() * numKeys, D * numKeys); @@ -342,7 +343,7 @@ public: // **************************************************************************************************** /// SVD version - double computeJacobiansSVD(std::vector>& Fblocks, Matrix& Enull, + double computeJacobiansSVD(std::vector >& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -642,7 +643,7 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector> Fblocks; + std::vector > Fblocks; Matrix E; Matrix3 PointCov; Vector b; From 3bfdc12ae43366208d207cba2f6cef5832f73add Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 08:48:01 -0400 Subject: [PATCH 20/89] change landmark initialization to use monocular triangulation (doesn't really make a difference, but probably slower), and extra debug statements --- gtsam/slam/SmartStereoProjectionFactor.h | 32 ++++++++++++++++++++---- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 96f5568dd..29a942bae 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -253,9 +253,26 @@ public: // 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 Camera& 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, + rankTolerance_, enableEPI_); + + // // // End temporary hack + // FIXME: temporary: triangulation using only first camera - const StereoPoint2& z0 = this->measured_.at(0); - point_ = cameras[0].backproject(z0); +// const StereoPoint2& z0 = this->measured_.at(0); +// point_ = cameras[0].backproject(z0); degenerate_ = false; cheiralityException_ = false; @@ -344,11 +361,12 @@ public: } this->triangulateSafe(cameras); + if (isDebug) std::cout << "point_ = " << point_ << std::endl; if (numKeys < 2 || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { - // std::cout << "In linearize: exception" << std::endl; + if (isDebug) std::cout << "In linearize: exception" << std::endl; BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) @@ -360,10 +378,13 @@ public: // 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; } bool doLinearize = this->decideIfLinearize(cameras); + if (isDebug) std::cout << "doLinearize = " << doLinearize << std::endl; + 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(); @@ -397,8 +418,9 @@ public: 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; + + 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; From 94ca4463fe5921f4c2218d8d9384c2954d250678 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 08:48:37 -0400 Subject: [PATCH 21/89] throw when reaching parts of code that need to be fixed --- gtsam/slam/SmartStereoProjectionPoseFactor.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 9b7763ef2..60992281e 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -174,12 +174,14 @@ public: const Values& values) const { // depending on flag set on construction we may linearize to different linear factors switch(linearizeTo_){ -// case JACOBIAN_SVD : + case JACOBIAN_SVD : + throw("JacobianSVD not working yet!"); // return this->createJacobianSVDFactor(cameras(values), 0.0); -// break; -// case JACOBIAN_Q : + break; + case JACOBIAN_Q : + throw("JacobianQ not working yet!"); // return this->createJacobianQFactor(cameras(values), 0.0); -// break; + break; default: return this->createHessianFactor(cameras(values)); break; From ff30413f231a8514479a6253539984b6bc39a848 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 10 Jul 2014 08:50:23 -0400 Subject: [PATCH 22/89] minor cleanup. First few tests pass now due to making StereoCamera project like PinholeCamera --- .../testSmartStereoProjectionPoseFactor.cpp | 1510 ++++++++--------- 1 file changed, 754 insertions(+), 756 deletions(-) diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index b5ee3d61a..2659cb274 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,7 +32,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = false; +static bool isDebugTest = true; // make a realistic calibration matrix static double fov = 60; // degrees @@ -65,9 +65,11 @@ static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0 typedef SmartStereoProjectionPoseFactor SmartFactor; typedef SmartStereoProjectionPoseFactor SmartFactorBundler; -void stereo_projectToMultipleCameras( - StereoCamera cam1, StereoCamera cam2, StereoCamera cam3, Point3 landmark, - vector& measurements_cam){ +vector stereo_projectToMultipleCameras( + const StereoCamera& cam1, const StereoCamera& cam2, + const StereoCamera& cam3, Point3 landmark){ + + vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); StereoPoint2 cam2_uv1 = cam2.project(landmark); @@ -75,6 +77,8 @@ void stereo_projectToMultipleCameras( measurements_cam.push_back(cam1_uv1); measurements_cam.push_back(cam2_uv1); measurements_cam.push_back(cam3_uv1); + + return measurements_cam; } /* ************************************************************************* */ @@ -217,7 +221,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ){ /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + 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)); @@ -236,12 +240,10 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ 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); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); std::vector views; views.push_back(x1); @@ -321,12 +323,10 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_iterative_smart_projection_factor 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); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); smartFactor1->add(measurements_cam1, views, model, K); @@ -372,747 +372,745 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_iterative_smart_projection_factor 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)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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) + StereoPoint2(10,10,1); // 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)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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 GenericStereoFactor 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera 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)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera 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 - StereoPoint2 cam1_uv1 = cam1.project(landmark1); - StereoPoint2 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)); - StereoCamera cam1(pose1, K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - StereoCamera cam2(pose2, K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - StereoCamera 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, 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)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera 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); +// +// // 1. Project three landmarks into three cameras and triangulate +// vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); +// vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); +// vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); +// +// 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)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera 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)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera 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) + StereoPoint2(10,10,1); // 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)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera 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)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera 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 GenericStereoFactor 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera 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)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera 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 +// StereoPoint2 cam1_uv1 = cam1.project(landmark1); +// StereoPoint2 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)); +// StereoCamera cam1(pose1, K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera 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)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); +// StereoCamera 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) ); +//} /* ************************************************************************* */ From 4c076fca2d02d730d17cc25619a3fa74fc38b6b4 Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Tue, 15 Jul 2014 10:14:23 -0400 Subject: [PATCH 23/89] Added SmartStereoProjectionFactor example --- gtsam/slam/SmartStereoProjectionFactor.h | 2 +- .../SmartStereoProjectionFactorExample.cpp | 125 ++++++++++++++++++ 2 files changed, 126 insertions(+), 1 deletion(-) create mode 100644 gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 29a942bae..c79ad1a89 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -296,7 +296,7 @@ public: } i += 1; } - std::cout << "totalReprojError error: " << totalReprojError << std::endl; + //std::cout << "totalReprojError error: " << totalReprojError << std::endl; // we discard smart factors that have large reprojection error if(dynamicOutlierRejectionThreshold_ > 0 && totalReprojError/m > dynamicOutlierRejectionThreshold_) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp new file mode 100644 index 000000000..d740ebff8 --- /dev/null +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + +* 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 SmartProjectionFactorExample.cpp +* @brief A stereo visual odometry example +* @date May 30, 2014 +* @author Stephen Camp +* @author Chris Beall +*/ + + +/** + * A smart projection factor example based on stereo data, throwing away the + * measurement from the right camera + * -robot starts at origin + * -moves forward, taking periodic stereo measurements + * -makes monocular observations of many landmarks + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv){ + + typedef SmartStereoProjectionPoseFactor SmartFactor; + + Values initial_estimate; + NonlinearFactorGraph graph; + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + + string calibration_loc = findExampleDataFile("VO_calibration.txt"); + string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); + string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); + + //read camera calibration info from file + // focal lengths fx, fy, skew s, principal point u0, v0, baseline b + cout << "Reading calibration info" << endl; + ifstream calibration_file(calibration_loc.c_str()); + + double fx, fy, s, u0, v0, b; + calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; + const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0,b)); + + cout << "Reading camera poses" << endl; + ifstream pose_file(pose_loc.c_str()); + + int pose_id; + MatrixRowMajor m(4,4); + //read camera pose parameters and use to make initial estimates of camera poses + while (pose_file >> pose_id) { + for (int i = 0; i < 16; i++) { + pose_file >> m.data()[i]; + } + initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + } + + // camera and landmark keys + size_t x, l; + + // pixel coordinates uL, uR, v (same for left/right images due to rectification) + // landmark coordinates X, Y, Z in camera frame, resulting from triangulation + double uL, uR, v, X, Y, Z; + ifstream factor_file(factor_loc.c_str()); + cout << "Reading stereo factors" << endl; + + //read stereo measurements and construct smart factors + + SmartFactor::shared_ptr factor(new SmartFactor()); + 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()); + current_l = l; + } + factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K); + } + + Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + //constrain the first pose such that it cannot change from its original value during optimization + // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky + // QR is much slower than Cholesky, but numerically more stable + graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + + cout << "Optimizing" << endl; + //create Levenberg-Marquardt optimizer to optimize the factor graph + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); + Values result = optimizer.optimize(); + + cout << "Final result sample:" << endl; + Values pose_values = result.filter(); + pose_values.print("Final camera poses:\n"); + + return 0; +} From 48db3fc6c69d60c88555f64e065dc1a2f60b2a93 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 15 Jul 2014 13:08:51 -0400 Subject: [PATCH 24/89] remove duplicate test --- .../testSmartStereoProjectionPoseFactor.cpp | 74 ------------------- 1 file changed, 74 deletions(-) diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 2659cb274..0b9f55d0b 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -297,80 +297,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ 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)); - StereoCamera cam1(pose1, K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera 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); - - // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - - 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 ){ From d68e6b9addcf2fc1564aa5d74b9a311ede06ccf4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 15 Jul 2014 13:09:12 -0400 Subject: [PATCH 25/89] Noisemodel dimension 2->3 --- gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index d740ebff8..43e576e05 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -50,7 +50,7 @@ int main(int argc, char** argv){ Values initial_estimate; NonlinearFactorGraph graph; - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); From 089ac4e743b777c7b1e339d11af32bf10618f9fa Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 15 Jul 2014 14:43:01 -0400 Subject: [PATCH 26/89] Additionally templated JacobianFactorSVD on measurement type, and Jacobian_SVD unit tests now pass for SmartStereoFactor --- gtsam/slam/JacobianFactorSVD.h | 10 +- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam/slam/SmartStereoProjectionFactor.h | 16 +- gtsam/slam/SmartStereoProjectionPoseFactor.h | 3 +- .../testSmartStereoProjectionPoseFactor.cpp | 397 +++++++++--------- 6 files changed, 214 insertions(+), 216 deletions(-) diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 0d3c04de1..8bed77d0f 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -11,12 +11,12 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template +template class JacobianFactorSVD: public JacobianSchurFactor { public: - typedef Eigen::Matrix Matrix2D; + typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; @@ -38,8 +38,8 @@ public: /// Constructor JacobianFactorSVD(const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - size_t numKeys = Enull.rows() / 2; - size_t j = 0, m2 = 2*numKeys-3; + size_t numKeys = Enull.rows() / Z::Dim(); + size_t j = 0, m2 = Z::Dim()*numKeys-3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) @@ -48,7 +48,7 @@ public: std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second)); + QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, Z::Dim() * j++, m2, Z::Dim()) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ae2dc8b0f..03bd92f21 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -660,7 +660,7 @@ public: Vector b; Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + return boost::make_shared< JacobianFactorSVD<6, Z> >(Fblocks, Enull, b); } private: diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 04112e239..a08b79953 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -444,7 +444,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index c79ad1a89..71f5e11b3 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -473,14 +473,14 @@ public: // 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_); -// } + /// 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, diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam/slam/SmartStereoProjectionPoseFactor.h index 60992281e..9448f6498 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam/slam/SmartStereoProjectionPoseFactor.h @@ -175,8 +175,7 @@ public: // depending on flag set on construction we may linearize to different linear factors switch(linearizeTo_){ case JACOBIAN_SVD : - throw("JacobianSVD not working yet!"); -// return this->createJacobianSVDFactor(cameras(values), 0.0); + return this->createJacobianSVDFactor(cameras(values), 0.0); break; case JACOBIAN_Q : throw("JacobianQ not working yet!"); diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 0b9f55d0b..d9b86e158 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -298,206 +298,205 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ } -///* *************************************************************************/ -//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)); -// StereoCamera cam1(pose1, K); -// // create second camera 1 meter to the right of first camera -// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); -// StereoCamera cam2(pose2, K); -// // create third camera 1 meter above the first camera -// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); -// StereoCamera 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); -// -// // 1. Project three landmarks into three cameras and triangulate -// vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); -// vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); -// vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); -// -// 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, jacobianSVD ){ -///* *************************************************************************/ -//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)); -// StereoCamera cam1(pose1, K); -// // create second camera 1 meter to the right of first camera -// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); -// StereoCamera cam2(pose2, K); -// // create third camera 1 meter above the first camera -// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); -// StereoCamera 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)); -// StereoCamera cam1(pose1, K); -// // create second camera 1 meter to the right of first camera -// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); -// StereoCamera cam2(pose2, K); -// // create third camera 1 meter above the first camera -// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); -// StereoCamera 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) + StereoPoint2(10,10,1); // 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))); -//} + 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + + 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + + + 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)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera 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); + + // 1. Project four landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); + + + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // 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 ){ From e7c82652af3f4652a4587b9a87530e5bba507e8f Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Wed, 16 Jul 2014 14:45:56 -0400 Subject: [PATCH 27/89] Added option to increase initial error in SmartStereoProjectionFactorExample --- .../examples/SmartStereoProjectionFactorExample.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 43e576e05..9847ef5ed 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -52,6 +52,8 @@ int main(int argc, char** argv){ NonlinearFactorGraph graph; const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + bool add_initial_noise = true; + string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); @@ -75,6 +77,9 @@ int main(int argc, char** argv){ for (int i = 0; i < 16; i++) { pose_file >> m.data()[i]; } + if(add_initial_noise){ + m(0,3) += (pose_id % 10)/5; + } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } From a2974341488152d5afcf88edebf9884f814bed66 Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Tue, 22 Jul 2014 09:08:38 -0400 Subject: [PATCH 28/89] Modified SmartStereoProjectionFactorExample to write optimized poses to file --- .../examples/SmartStereoProjectionFactorExample.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 9847ef5ed..f4329e665 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -48,9 +48,12 @@ int main(int argc, char** argv){ typedef SmartStereoProjectionPoseFactor SmartFactor; + bool output_poses = true; + string poseOutput("../../../examples/data/optimized_poses.txt"); Values initial_estimate; NonlinearFactorGraph graph; const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); + ofstream pose3Out; bool add_initial_noise = true; @@ -126,5 +129,14 @@ int main(int argc, char** argv){ Values pose_values = result.filter(); pose_values.print("Final camera poses:\n"); + if(output_poses){ + pose3Out.open(poseOutput.c_str(),ios::out); + for(int i = 1; i<=pose_values.size(); i++){ + pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, + " ", " ")) << endl; + } + cout << "Writing output" << endl; + } + return 0; } From 8bad2952099c89465ea4c7e03479c6c6c26f377c Mon Sep 17 00:00:00 2001 From: Stephen Camp Date: Tue, 22 Jul 2014 12:49:09 -0400 Subject: [PATCH 29/89] Changed initial noise generation for SmartStereoProjectionFactorExample to Y direction, changed noise generation scale --- .../SmartStereoProjectionFactorExample.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index f4329e665..7950bdb23 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -49,11 +49,13 @@ int main(int argc, char** argv){ typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; + bool output_initial_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); + string init_poseOutput("../../../examples/data/initial_poses.txt"); Values initial_estimate; NonlinearFactorGraph graph; const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); - ofstream pose3Out; + ofstream pose3Out, init_pose3Out; bool add_initial_noise = true; @@ -81,10 +83,19 @@ int main(int argc, char** argv){ pose_file >> m.data()[i]; } if(add_initial_noise){ - m(0,3) += (pose_id % 10)/5; + m(1,3) += (pose_id % 10)/10; } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } + + Values initial_pose_values = initial_estimate.filter(); + if(output_poses){ + init_pose3Out.open(init_poseOutput.c_str(),ios::out); + for(int i = 1; i<=initial_pose_values.size(); i++){ + init_pose3Out << i << " " << initial_pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, + " ", " ")) << endl; + } + } // camera and landmark keys size_t x, l; From f75f26fb637f3fbaa3f108f70ab3aa3e09c12757 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 22 Jul 2014 22:00:33 -0400 Subject: [PATCH 30/89] Fix for Mac --- gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 7950bdb23..841092ec9 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -83,7 +83,7 @@ int main(int argc, char** argv){ pose_file >> m.data()[i]; } if(add_initial_noise){ - m(1,3) += (pose_id % 10)/10; + m(1,3) += (pose_id % 10)/10.0; } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } From cbad9aa78395b6590928691c4a27cf8e335d68b2 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 22 Jul 2014 22:02:20 -0400 Subject: [PATCH 31/89] Hessian tests work, too --- .../testSmartStereoProjectionPoseFactor.cpp | 409 +++++++++--------- 1 file changed, 201 insertions(+), 208 deletions(-) diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index d9b86e158..e50616163 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -626,85 +626,84 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // 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)); -// StereoCamera 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)); -// StereoCamera 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)); -// StereoCamera 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, 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)); + StereoCamera 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)); + StereoCamera 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)); + StereoCamera 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); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + + + 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 ){ @@ -907,135 +906,129 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // // 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)); -// StereoCamera cam1(pose1, K); -// -// // create second camera 1 meter to the right of first camera -// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); -// StereoCamera cam2(pose2, K); -// -// // create third camera 1 meter above the first camera -// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); -// StereoCamera 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)); -// StereoCamera cam1(pose1, K2); -// -// // create second camera 1 meter to the right of first camera -// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); -// StereoCamera cam2(pose2, K2); -// -// // create third camera 1 meter above the first camera -// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); -// StereoCamera 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, 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)); + StereoCamera cam1(pose1, K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + StereoCamera cam2(pose2, K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + StereoCamera cam3(pose3, K); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + + 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)); + StereoCamera cam1(pose1, K2); + + // Second and third cameras in same place, which is a degenerate configuration + Pose3 pose2 = pose1; + Pose3 pose3 = pose1; + StereoCamera cam2(pose2, K2); + StereoCamera cam3(pose3, K2); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + + 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) ); +} /* ************************************************************************* */ From fb55e2c17f3138f6840e8bf9e524ca6c5093e530 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Tue, 22 Jul 2014 22:03:04 -0400 Subject: [PATCH 32/89] print linearization threshold --- gtsam/slam/SmartStereoProjectionFactor.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 71f5e11b3..9a73b84fe 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -156,6 +156,7 @@ public: std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; std::cout << "degenerate_ = " << degenerate_ << std::endl; std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << std::endl; Base::print("", keyFormatter); } From f447481844a884a51cd7f6a5aa57063634fa246e Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 3 Oct 2014 12:18:42 -0400 Subject: [PATCH 33/89] initial stub for metis ordering --- gtsam/inference/Ordering.cpp | 7 +++++++ gtsam/inference/Ordering.h | 8 ++++++++ gtsam/inference/tests/testOrdering.cpp | 5 +++++ 3 files changed, 20 insertions(+) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 4f4b14bb5..7d3d7cc0b 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; @@ -196,6 +197,12 @@ namespace gtsam { return Ordering::COLAMDConstrained(variableIndex, cmember); } + /* ************************************************************************* */ + Ordering Ordering::METIS(const VariableIndex& variableIndex) + { + gttic(Ordering_METIS); + } + /* ************************************************************************* */ void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const { diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 7b1a2bb2e..1260c15fb 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -146,6 +146,14 @@ namespace gtsam { return Ordering(keys); } + + /// Compute an ordering determined by METIS from a VariableIndex + static GTSAM_EXPORT Ordering METIS(const VariableIndex& variableIndex); + + template + static Ordering METIS(const FactorGraph& graph){ + return METIS(VariableIndex(graph)); } + /// @} /// @name Testable @{ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 3bf6f7ca0..5fcac15b4 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -77,6 +77,11 @@ TEST(Ordering, grouped_constrained_ordering) { EXPECT(assert_equal(expConstrained, actConstrained)); } +/* ************************************************************************* */ +TEST(Ordering, metis_ordering) { + +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From bf22a49504749af3a1b33faa21fd11346b9e5aae Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 13 Oct 2014 13:15:05 -0400 Subject: [PATCH 34/89] Working ordering format for Metis_NodeND --- gtsam/inference/MetisIndex-inl.h | 59 +++++++++++++++++++ gtsam/inference/MetisIndex.cpp | 0 gtsam/inference/MetisIndex.h | 78 ++++++++++++++++++++++++++ gtsam/inference/Ordering.cpp | 13 ++++- gtsam/inference/Ordering.h | 9 ++- gtsam/inference/tests/testOrdering.cpp | 42 +++++++++++++- 6 files changed, 196 insertions(+), 5 deletions(-) create mode 100644 gtsam/inference/MetisIndex-inl.h create mode 100644 gtsam/inference/MetisIndex.cpp create mode 100644 gtsam/inference/MetisIndex.h diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h new file mode 100644 index 000000000..35d8c00fc --- /dev/null +++ b/gtsam/inference/MetisIndex-inl.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + +* 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 MetisIndex-inl.h +* @author Andrew Melim +* @date Oct. 10, 2014 +*/ + +#include +#include + +namespace gtsam { + + MetisIndex::~MetisIndex(){} + + std::vector MetisIndex::xadj() const { return xadj_; } + std::vector MetisIndex::adj() const { return adj_; } + + /* ************************************************************************* */ + template + void MetisIndex::augment(const FactorGraph& factors) + { + std::map > adjMap; + std::map >::iterator adjMapIt; + + /* ********** Convert to CSR format ********** */ + // Assuming that vertex numbering starts from 0 (C style), + // then the adjacency list of vertex i is stored in array adjncy + // starting at index xadj[i] and ending at(but not including) + // index xadj[i + 1](i.e., adjncy[xadj[i]] through + // and including adjncy[xadj[i + 1] - 1]). + for (size_t i = 0; i < factors.size(); i++){ + if (factors[i]) + BOOST_FOREACH(const Key& k1, *factors[i]) + BOOST_FOREACH(const Key& k2, *factors[i]) + if (k1 != k2) + adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end + } + + + xadj_.push_back(0);// Always set the first index to zero + for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { + vector temp; + copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + adj_.insert(adj_.end(), temp.begin(), temp.end()); + //adj_.push_back(temp); + xadj_.push_back(adj_.size()); + } + } +} diff --git a/gtsam/inference/MetisIndex.cpp b/gtsam/inference/MetisIndex.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h new file mode 100644 index 000000000..57b999d7d --- /dev/null +++ b/gtsam/inference/MetisIndex.h @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------------- + +* 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 MetisIndex.h +* @author Andrew Melim +* @date Oct. 10, 2014 +*/ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + /** + * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in + * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built + * fromt a factor graph prior to elimination, and stores the list of factors + * that involve each variable. + * \nosubgrouping + */ +class GTSAM_EXPORT MetisIndex +{ +public: + typedef boost::shared_ptr shared_ptr; + +private: + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + size_t nFactors_; // Number of factors in the original factor graph + size_t nValues_; // + +public: + /// @name Standard Constructors + /// @{ + + /** Default constructor, creates empty MetisIndex */ + MetisIndex() : nFactors_(0), nValues_(0) {} + + template + MetisIndex(const FG& factorGraph) : nFactors_(0), nValues_(0) { + augment(factorGraph); } + + ~MetisIndex(); + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FactorGraph& factors); + + std::vector xadj() const; + std::vector adj() const; + + /// @} +}; +} + +#include diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 7d3d7cc0b..33cf2092d 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -197,10 +197,21 @@ namespace gtsam { return Ordering::COLAMDConstrained(variableIndex, cmember); } + /* ************************************************************************* */ - Ordering Ordering::METIS(const VariableIndex& variableIndex) + template + Ordering Ordering::METIS(const FactorGraph& graph) { gttic(Ordering_METIS); + // First develop the adjacency matrix for the + // graph as described in Section 5.5 of the METIS manual + // CSR Format + // xadj is of size n+1 + // metis vars + + + //METIS_NodeND(graph.keys().size(), xadj, adj); + } /* ************************************************************************* */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 1260c15fb..fad9fe9e9 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -146,13 +146,15 @@ namespace gtsam { return Ordering(keys); } + /// METIS Formatting function + template + static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering METIS(const VariableIndex& variableIndex); + //static GTSAM_EXPORT Ordering METIS(const VariableIndex& variableIndex); template - static Ordering METIS(const FactorGraph& graph){ - return METIS(VariableIndex(graph)); } + static GTSAM_EXPORT Ordering METIS(const FactorGraph& graph); /// @} @@ -169,6 +171,7 @@ namespace gtsam { static GTSAM_EXPORT Ordering COLAMDConstrained( const VariableIndex& variableIndex, std::vector& cmember); + /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 5fcac15b4..252106f88 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -78,7 +79,46 @@ TEST(Ordering, grouped_constrained_ordering) { } /* ************************************************************************* */ -TEST(Ordering, metis_ordering) { +TEST(Ordering, csr_format) { + + + // Example in METIS manual + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(5, 6); + sfg.push_factor(6, 7); + sfg.push_factor(7, 8); + sfg.push_factor(8, 9); + sfg.push_factor(10, 11); + sfg.push_factor(11, 12); + sfg.push_factor(12, 13); + sfg.push_factor(13, 14); + + sfg.push_factor(0, 5); + sfg.push_factor(5, 10); + sfg.push_factor(1, 6); + sfg.push_factor(6, 11); + sfg.push_factor(2, 7); + sfg.push_factor(7, 12); + sfg.push_factor(3, 8); + sfg.push_factor(8, 13); + sfg.push_factor(4, 9); + sfg.push_factor(9, 14); + + MetisIndex mi(sfg); + + vector xadjExpected{ 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44}; + vector adjExpected{ 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, + 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, + 13, 8, 12, 14, 9, 13 }; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT( adjExpected == mi.adj()); + } From 0771b1658b468b5caf31de6dcff96da13a63d663 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 14 Oct 2014 15:18:05 -0400 Subject: [PATCH 35/89] Ordering implementation, unit tests --- CMakeLists.txt | 1 - gtsam/3rdparty/metis-5.1.0/CMakeLists.txt | 1 + .../metis-5.1.0/libmetis/CMakeLists.txt | 1 + gtsam/CMakeLists.txt | 7 +++-- gtsam/inference/MetisIndex-inl.h | 10 +++---- gtsam/inference/MetisIndex.h | 8 +++--- gtsam/inference/Ordering.cpp | 26 ++++++++++++------- gtsam/inference/Ordering.h | 8 ++++-- gtsam/inference/tests/testOrdering.cpp | 14 +++++++--- 9 files changed, 48 insertions(+), 28 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fcce54b6..ba159c8e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -216,7 +216,6 @@ configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eig # Install the configuration file for Eigen install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty) - ############################################################################### # Global compile options diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt index 93c546be8..d3f2f1b0f 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt @@ -46,3 +46,4 @@ add_subdirectory("libmetis") if(GTSAM_BUILD_METIS_EXECUTABLES) add_subdirectory("programs") endif() + diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt index 66cfcba4a..df67d26b4 100644 --- a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt @@ -14,3 +14,4 @@ install(TARGETS metis RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib) + diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 9fac3b00b..ced644545 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -19,7 +19,6 @@ set(gtsam_srcs) message(STATUS "Building 3rdparty") add_subdirectory(3rdparty) -# build convenience library set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c @@ -91,12 +90,12 @@ set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSIO set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM Version: ${gtsam_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") - +message(STATUS "GTSAM Additional: ${GTSAM_ADDITIONAL_LIBRARIES}") # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM - static") add_library(gtsam STATIC ${gtsam_srcs}) - target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} metis ${GTSAM_ADDITIONAL_LIBRARIES}) set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 @@ -113,7 +112,7 @@ if (GTSAM_BUILD_STATIC_LIBRARY) else() message(STATUS "Building GTSAM - shared") add_library(gtsam SHARED ${gtsam_srcs}) - target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} metis ${GTSAM_ADDITIONAL_LIBRARIES}) set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 35d8c00fc..356118dbb 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -15,15 +15,12 @@ * @date Oct. 10, 2014 */ -#include +#pragma once + #include namespace gtsam { - MetisIndex::~MetisIndex(){} - - std::vector MetisIndex::xadj() const { return xadj_; } - std::vector MetisIndex::adj() const { return adj_; } /* ************************************************************************* */ template @@ -50,10 +47,13 @@ namespace gtsam { xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { vector temp; + // Copy from the FastSet into a temporary vector copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); xadj_.push_back(adj_.size()); } } + } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 57b999d7d..bcc9fc23f 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { /** @@ -56,7 +57,7 @@ public: MetisIndex(const FG& factorGraph) : nFactors_(0), nValues_(0) { augment(factorGraph); } - ~MetisIndex(); + ~MetisIndex(){} /// @} /// @name Advanced Interface /// @{ @@ -68,11 +69,12 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const; - std::vector adj() const; + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } /// @} }; + } #include diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 33cf2092d..d8c590361 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -199,19 +199,27 @@ namespace gtsam { /* ************************************************************************* */ - template - Ordering Ordering::METIS(const FactorGraph& graph) + Ordering Ordering::METIS(const MetisIndex& met) { gttic(Ordering_METIS); - // First develop the adjacency matrix for the - // graph as described in Section 5.5 of the METIS manual - // CSR Format - // xadj is of size n+1 - // metis vars + + vector xadj = met.xadj(); + vector adj = met.adj(); + + vector perm, iperm; + int outputError; + idx_t size = xadj.size(); + outputError = METIS_NodeND(&size, xadj.data(), adj.data(), NULL, NULL, perm.data(), iperm.data()); + Ordering result; + + if (outputError != METIS_OK) + { + std::cout << "METIS ordering error!\n"; + return result; + } - //METIS_NodeND(graph.keys().size(), xadj, adj); - + return result; } /* ************************************************************************* */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index fad9fe9e9..86038b028 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace gtsam { @@ -151,10 +152,13 @@ namespace gtsam { static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); /// Compute an ordering determined by METIS from a VariableIndex - //static GTSAM_EXPORT Ordering METIS(const VariableIndex& variableIndex); + static GTSAM_EXPORT Ordering METIS(const MetisIndex& met); template - static GTSAM_EXPORT Ordering METIS(const FactorGraph& graph); + static GTSAM_EXPORT Ordering METIS(const FactorGraph& graph) + { + return METIS(MetisIndex(graph)); + } /// @} diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 252106f88..22c86e191 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -80,8 +80,6 @@ TEST(Ordering, grouped_constrained_ordering) { /* ************************************************************************* */ TEST(Ordering, csr_format) { - - // Example in METIS manual SymbolicFactorGraph sfg; sfg.push_factor(0, 1); @@ -118,10 +116,18 @@ TEST(Ordering, csr_format) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT( adjExpected == mi.adj()); - - } +/* ************************************************************************* */ +TEST(Ordering, metis) { + SymbolicFactorGraph sfg; + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + + Ordering::METIS(sfg); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From ad74a4b8c9bbb01190862992b54859c6ca3728b3 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 14 Oct 2014 19:14:59 -0400 Subject: [PATCH 36/89] Update ms_stdint.h in metis. Export libraries correctly --- CMakeLists.txt | 10 ++- gtsam/3rdparty/CMakeLists.txt | 6 +- gtsam/3rdparty/metis-5.1.0/CMakeLists.txt | 2 + gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h | 81 ++++++++++++++----- .../metis-5.1.0/libmetis/CMakeLists.txt | 3 + gtsam/CMakeLists.txt | 48 ++++++----- gtsam/inference/Ordering.h | 2 +- 7 files changed, 101 insertions(+), 51 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ba159c8e9..951a0ec7e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) @@ -197,13 +197,13 @@ set(GTSAM_USE_SYSTEM_EIGEN OFF) if(GTSAM_USE_SYSTEM_EIGEN) # Use generic Eigen include paths e.g. set(GTSAM_EIGEN_INCLUDE_PREFIX "") - + find_package(Eigen3 REQUIRED) include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") else() # Use bundled Eigen include paths e.g. set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/") - + # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) @@ -262,7 +262,7 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR}) # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. include_directories(BEFORE - gtsam/3rdparty/UFconfig + gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include gtsam/3rdparty/metis-5.1.0/include gtsam/3rdparty/metis-5.1.0/libmetis @@ -322,8 +322,10 @@ endif(GTSAM_BUILD_UNSTABLE) # Install config and export files GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") +message("GTSAM export: ${GTSAM_EXPORTED_TARGETS}") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) + # Check for doxygen availability - optional dependency find_package(Doxygen) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 576da93bd..38c084e25 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -18,7 +18,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) # do the same for the unsupported eigen folder file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") - + file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*") foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all}) get_filename_component(filename ${unsupported_eigen_dir} NAME) @@ -36,7 +36,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) install(DIRECTORY Eigen/Eigen DESTINATION include/gtsam/3rdparty/Eigen FILES_MATCHING PATTERN "*.h") - + install(DIRECTORY Eigen/unsupported/Eigen DESTINATION include/gtsam/3rdparty/Eigen/unsupported/ FILES_MATCHING PATTERN "*.h") @@ -73,3 +73,5 @@ endif() if(GTSAM_INSTALL_GEOGRAPHICLIB) add_subdirectory(GeographicLib) endif() + +set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt index d3f2f1b0f..fd9c7eaf7 100644 --- a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt @@ -47,3 +47,5 @@ if(GTSAM_BUILD_METIS_EXECUTABLES) add_subdirectory("programs") endif() +set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) + diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h b/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h index 7e200dc6f..39b8aed9d 100644 --- a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h +++ b/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h @@ -1,7 +1,7 @@ // ISO C9x compliant stdint.h for Microsoft Visual Studio // Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124 // -// Copyright (c) 2006 Alexander Chemeris +// Copyright (c) 2006-2013 Alexander Chemeris // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -13,8 +13,9 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// 3. The name of the author may be used to endorse or promote products -// derived from this software without specific prior written permission. +// 3. Neither the name of the product nor the names of its contributors may +// be used to endorse or promote products derived from this software +// without specific prior written permission. // // THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF @@ -40,30 +41,59 @@ #pragma once #endif +#if _MSC_VER >= 1600 // [ +#include +#else // ] _MSC_VER >= 1600 [ + #include -// For Visual Studio 6 in C++ mode wrap include with 'extern "C++" {}' +// For Visual Studio 6 in C++ mode and for many Visual Studio versions when +// compiling for ARM we should wrap include with 'extern "C++" {}' // or compiler give many errors like this: // error C2733: second C linkage of overloaded function 'wmemchr' not allowed -#if (_MSC_VER < 1300) && defined(__cplusplus) - extern "C++" { -#endif -# include -#if (_MSC_VER < 1300) && defined(__cplusplus) - } +#ifdef __cplusplus +extern "C" { #endif +# include +#ifdef __cplusplus +} +#endif + +// Define _W64 macros to mark types changing their size, like intptr_t. +#ifndef _W64 +# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300 +# define _W64 __w64 +# else +# define _W64 +# endif +#endif + // 7.18.1 Integer types // 7.18.1.1 Exact-width integer types -typedef __int8 int8_t; -typedef __int16 int16_t; -typedef __int32 int32_t; -typedef __int64 int64_t; + +// Visual Studio 6 and Embedded Visual C++ 4 doesn't +// realize that, e.g. char has the same size as __int8 +// so we give up on __intX for them. +#if (_MSC_VER < 1300) +typedef signed char int8_t; +typedef signed short int16_t; +typedef signed int int32_t; +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned int uint32_t; +#else +typedef signed __int8 int8_t; +typedef signed __int16 int16_t; +typedef signed __int32 int32_t; typedef unsigned __int8 uint8_t; typedef unsigned __int16 uint16_t; typedef unsigned __int32 uint32_t; -typedef unsigned __int64 uint64_t; +#endif +typedef signed __int64 int64_t; +typedef unsigned __int64 uint64_t; + // 7.18.1.2 Minimum-width integer types typedef int8_t int_least8_t; @@ -87,11 +117,11 @@ typedef uint64_t uint_fast64_t; // 7.18.1.4 Integer types capable of holding object pointers #ifdef _WIN64 // [ - typedef __int64 intptr_t; - typedef unsigned __int64 uintptr_t; +typedef signed __int64 intptr_t; +typedef unsigned __int64 uintptr_t; #else // _WIN64 ][ - typedef int intptr_t; - typedef unsigned int uintptr_t; +typedef _W64 signed int intptr_t; +typedef _W64 unsigned int uintptr_t; #endif // _WIN64 ] // 7.18.1.5 Greatest-width integer types @@ -213,10 +243,17 @@ typedef uint64_t uintmax_t; #define UINT64_C(val) val##ui64 // 7.18.4.2 Macros for greatest-width integer constants -#define INTMAX_C INT64_C -#define UINTMAX_C UINT64_C +// These #ifndef's are needed to prevent collisions with . +// Check out Issue 9 for the details. +#ifndef INTMAX_C // [ +# define INTMAX_C INT64_C +#endif // INTMAX_C ] +#ifndef UINTMAX_C // [ +# define UINTMAX_C UINT64_C +#endif // UINTMAX_C ] #endif // __STDC_CONSTANT_MACROS ] +#endif // _MSC_VER >= 1600 ] -#endif // _MSC_STDINT_H_ ] +#endif // _MSC_STDINT_H_ ] \ No newline at end of file diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt index df67d26b4..a18973427 100644 --- a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt @@ -14,4 +14,7 @@ install(TARGETS metis RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib) +install(TARGETS metis EXPORT GTSAM-exports ARCHIVE DESTINATION lib) +list(APPEND GTSAM_EXPORTED_TARGETS metis) +set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index ced644545..2d5706f33 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -1,14 +1,14 @@ # We split the library in to separate subfolders, each containing # tests, timing, and an optional convenience library. # The following variable is the master list of subdirs to add -set (gtsam_subdirs - base - geometry - inference - symbolic - discrete - linear - nonlinear +set (gtsam_subdirs + base + geometry + inference + symbolic + discrete + linear + nonlinear slam navigation ) @@ -16,12 +16,12 @@ set (gtsam_subdirs set(gtsam_srcs) # Build 3rdparty separately -message(STATUS "Building 3rdparty") +message(STATUS "Building 3rdparty") add_subdirectory(3rdparty) -set (3rdparty_srcs +set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure @@ -36,7 +36,7 @@ set (excluded_sources #"") set (excluded_headers #"") "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" ) - + if(GTSAM_USE_QUATERNIONS) set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3M.cpp") else() @@ -58,10 +58,10 @@ foreach(subdir ${gtsam_subdirs}) set(${subdir}_srcs ${subdir_srcs}) # Build local library and tests - message(STATUS "Building ${subdir}") + message(STATUS "Building ${subdir}") add_subdirectory(${subdir}) endforeach(subdir) - + # To add additional sources to gtsam when building the full library (static or shared) # Add the subfolder with _srcs appended to the end to this list set(gtsam_srcs @@ -77,7 +77,7 @@ set(gtsam_srcs ${navigation_srcs} ${gtsam_core_headers} ) - + # Generate and install config and dllexport files configure_file(config.h.in config.h) set(library_name GTSAM) # For substitution in dllexport.h.in @@ -85,18 +85,22 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) +list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) + # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM Version: ${gtsam_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") -message(STATUS "GTSAM Additional: ${GTSAM_ADDITIONAL_LIBRARIES}") +message("GTSAM Additional: ${GTSAM_ADDITIONAL_LIBRARIES}") +message("GTSAM Exports: ${GTSAM_EXPORTED_TARGETS}") + # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM - static") add_library(gtsam STATIC ${gtsam_srcs}) - target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} metis ${GTSAM_ADDITIONAL_LIBRARIES}) - set_target_properties(gtsam PROPERTIES + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} @@ -112,8 +116,8 @@ if (GTSAM_BUILD_STATIC_LIBRARY) else() message(STATUS "Building GTSAM - shared") add_library(gtsam SHARED ${gtsam_srcs}) - target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} metis ${GTSAM_ADDITIONAL_LIBRARIES}) - set_target_properties(gtsam PROPERTIES + target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} @@ -134,7 +138,7 @@ set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" APPEND PROPERTY COMPILE_DEFINITIONS "SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\"" "INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"") - + # Special cases if(MSVC) set_property(SOURCE @@ -147,7 +151,7 @@ endif() if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Set up codegen include(GtsamMatlabWrap) - + # Generate, build and install toolbox set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") if(GTSAM_BUILD_STATIC_LIBRARY) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 86038b028..45f53f2ad 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -155,7 +155,7 @@ namespace gtsam { static GTSAM_EXPORT Ordering METIS(const MetisIndex& met); template - static GTSAM_EXPORT Ordering METIS(const FactorGraph& graph) + static Ordering METIS(const FactorGraph& graph) { return METIS(MetisIndex(graph)); } From 99caf8833a29a694fefe1b74841e491cc2d761bc Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 14 Oct 2014 23:46:12 -0400 Subject: [PATCH 37/89] Finished ordering implementation --- gtsam/inference/Ordering.cpp | 18 ++++++++++++++---- gtsam/inference/tests/testOrdering.cpp | 11 +++++------ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index d8c590361..99b116009 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -207,18 +207,28 @@ namespace gtsam { vector adj = met.adj(); vector perm, iperm; + + idx_t size = xadj.size() - 1; + for (int i = 0; i < size; i++) + { + perm.push_back(0); + iperm.push_back(0); + } + int outputError; - idx_t size = xadj.size(); - outputError = METIS_NodeND(&size, xadj.data(), adj.data(), NULL, NULL, perm.data(), iperm.data()); + + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); Ordering result; if (outputError != METIS_OK) { - std::cout << "METIS ordering error!\n"; + std::cout << "METIS failed during Nested Dissection ordering!\n"; return result; } - + result.resize(size); + for (size_t j = 0; j < size; ++j) + result[j] = perm[j]; return result; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 22c86e191..90e1cbe66 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -119,14 +119,13 @@ TEST(Ordering, csr_format) { } /* ************************************************************************* */ TEST(Ordering, metis) { - + SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); - sfg.push_factor(1, 2); - sfg.push_factor(2, 3); - sfg.push_factor(3, 4); - Ordering::METIS(sfg); + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + + Ordering metis = Ordering::METIS(sfg); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From 103ec596d7d840418c5c338ab525775bcca6c193 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Wed, 15 Oct 2014 00:03:57 -0400 Subject: [PATCH 38/89] Remove empty file and some code cleanup --- gtsam/inference/MetisIndex.cpp | 0 gtsam/inference/Ordering.cpp | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 100644 gtsam/inference/MetisIndex.cpp diff --git a/gtsam/inference/MetisIndex.cpp b/gtsam/inference/MetisIndex.cpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 99b116009..685bbcd0d 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -209,7 +209,7 @@ namespace gtsam { vector perm, iperm; idx_t size = xadj.size() - 1; - for (int i = 0; i < size; i++) + for (idx_t i = 0; i < size; i++) { perm.push_back(0); iperm.push_back(0); From 8cd17f6a3065a221fe1f67f2eb45c192076a1a8d Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 17 Oct 2014 15:50:13 -0400 Subject: [PATCH 39/89] Updating nonlinear params to allow selection of orderings --- gtsam/inference/Ordering.cpp | 1 + gtsam/inference/Ordering.h | 1 + gtsam/inference/tests/testOrdering.cpp | 1 + gtsam/nonlinear/NonlinearOptimizerParams.cpp | 43 ++++++++++++++++++-- gtsam/nonlinear/NonlinearOptimizerParams.h | 27 ++++++++++-- 5 files changed, 66 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 685bbcd0d..fbda41ac0 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -12,6 +12,7 @@ /** * @file Ordering.cpp * @author Richard Roberts + * @author Andrew Melim * @date Sep 2, 2010 */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 45f53f2ad..6cf125551 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -12,6 +12,7 @@ /** * @file Ordering.h * @author Richard Roberts + * @author Andrew Melim * @date Sep 2, 2010 */ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 90e1cbe66..b769551bf 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -12,6 +12,7 @@ /** * @file testOrdering * @author Alex Cunningham + * @author Andrew Melim */ #include diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index ef0f2aa9b..f20a36b5d 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -107,10 +107,17 @@ void NonlinearOptimizerParams::print(const std::string& str) const { break; } - if (ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; + switch (orderingType){ + case COLAMD: + std::cout << " ordering: COLAMD\n"; + break; + case METIS: + std::cout << " ordering: METIS\n"; + break; + default: + std::cout << " ordering: custom\n"; + break; + } std::cout.flush(); } @@ -155,6 +162,34 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } + /* ************************************************************************* */ +std::string NonlinearOptimizerParams::orderingTypeTranslator(NonlinearOptimizerParams::OrderingType type) const +{ + switch (type) { + case METIS: + return "METIS"; + case COLAMD: + return "COLAMD"; + default: + if (ordering) + return "CUSTOM"; + else + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); + } +} + +/* ************************************************************************* */ +NonlinearOptimizerParams::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const +{ + if (type == "METIS") + return METIS; + if (type == "COLAMD") + return COLAMD; + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); +} + } // namespace diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index dafc1f065..a390555e2 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -37,15 +37,21 @@ public: SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; + /** See NonlinearOptimizer::orderingType */ + enum OrderingType { + COLAMD, METIS, CUSTOM + }; + int maxIterations; ///< The maximum iterations to stop iterating (default 100) double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) + OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY) { + 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(COLAMD) { } virtual ~NonlinearOptimizerParams() { @@ -152,12 +158,27 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; + this->orderingType = CUSTOM; + } + + std::string getOrderingType() const { + return orderingTypeTranslator(orderingType); + } + + // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type + void setOrderingType(const std::string& ordering){ + orderingType = orderingTypeTranslator(ordering); } private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator( - const std::string& linearSolverType) const; + + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; + + std::string orderingTypeTranslator(OrderingType type) const; + + OrderingType orderingTypeTranslator(const std::string& type) const; + }; // For backward compatibility: From 1d7bcb301a08725b74ebcee5e10a56e71e5741d1 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 20 Oct 2014 18:58:15 -0400 Subject: [PATCH 40/89] Ordering type enum --- gtsam/inference/Ordering.h | 6 ++++++ gtsam/nonlinear/NonlinearOptimizerParams.cpp | 17 +++++++++-------- gtsam/nonlinear/NonlinearOptimizerParams.h | 16 ++++++---------- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 6cf125551..dfb1deb0e 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -37,6 +37,12 @@ namespace gtsam { typedef Ordering This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + /** See NonlinearOptimizer::orderingType */ + enum Type { + COLAMD_, METIS_, CUSTOM_ // Add underscores to prevent declaration errors + }; + + /// Create an empty ordering GTSAM_EXPORT Ordering() {} diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index f20a36b5d..1e964a481 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -5,6 +5,7 @@ * @author Yong-Dian Jian * @author Richard Roberts * @author Frank Dellaert + * @author Andrew Melim */ #include @@ -108,10 +109,10 @@ void NonlinearOptimizerParams::print(const std::string& str) const { } switch (orderingType){ - case COLAMD: + case Ordering::Type::COLAMD_: std::cout << " ordering: COLAMD\n"; break; - case METIS: + case Ordering::Type::METIS_: std::cout << " ordering: METIS\n"; break; default: @@ -164,12 +165,12 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(NonlinearOptimizerParams::OrderingType type) const +std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type) const { switch (type) { - case METIS: + case Ordering::Type::METIS_: return "METIS"; - case COLAMD: + case Ordering::Type::COLAMD_: return "COLAMD"; default: if (ordering) @@ -181,12 +182,12 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(NonlinearOptimizerP } /* ************************************************************************* */ -NonlinearOptimizerParams::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const +Ordering::Type NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const { if (type == "METIS") - return METIS; + return Ordering::Type::METIS_; if (type == "COLAMD") - return COLAMD; + return Ordering::Type::COLAMD_; throw std::invalid_argument( "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index a390555e2..04877c72e 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -15,6 +15,7 @@ * @author Yong-Dian Jian * @author Richard Roberts * @author Frank Dellaert + * @author Andrew Melim * @date Apr 1, 2012 */ @@ -37,21 +38,16 @@ public: SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - /** See NonlinearOptimizer::orderingType */ - enum OrderingType { - COLAMD, METIS, CUSTOM - }; - int maxIterations; ///< The maximum iterations to stop iterating (default 100) double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) + Ordering::Type orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(COLAMD) { + 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(Ordering::Type::COLAMD_) { } virtual ~NonlinearOptimizerParams() { @@ -158,7 +154,7 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = CUSTOM; + this->orderingType = Ordering::Type::CUSTOM_; } std::string getOrderingType() const { @@ -175,9 +171,9 @@ private: LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(OrderingType type) const; + std::string orderingTypeTranslator(Ordering::Type type) const; - OrderingType orderingTypeTranslator(const std::string& type) const; + Ordering::Type orderingTypeTranslator(const std::string& type) const; }; From 81dfa6fe0ad27d39903654a578e05a4276541cd7 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 21 Oct 2014 01:34:47 -0400 Subject: [PATCH 41/89] Adding METIS ordering logic to elimination --- .../inference/EliminateableFactorGraph-inst.h | 20 +++++++++++++------ gtsam/inference/EliminateableFactorGraph.h | 13 ++++++++++-- gtsam/inference/MetisIndex-inl.h | 5 +++-- gtsam/nonlinear/NonlinearOptimizer.cpp | 3 ++- 4 files changed, 30 insertions(+), 11 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b64ebe259..0fe65e4c0 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,7 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateSequential); @@ -47,13 +48,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateSequential(ordering, function, VariableIndex(asDerived())); + return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - return eliminateSequential(Ordering::COLAMD(*variableIndex), function); + if (orderingType == Ordering::Type::METIS_) + return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); } } @@ -61,7 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateMultifrontal); @@ -81,13 +86,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived())); + return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function); + if (orderingType == Ordering::Type::METIS_) + return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + else + return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 717f49167..820bb2fd3 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -94,6 +94,9 @@ namespace gtsam { /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; + /// Typedef for an optional ordering type + typedef boost::optional OptionalOrderingType; + /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. * @@ -101,6 +104,10 @@ namespace gtsam { * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); * \endcode + * + * Example - METIS ordering for elimination + * \code + * boost::shared_ptr result = graph.eliminateSequential(Ordering::Type::METIS_); * * Example - Full QR elimination in specified order: * \code @@ -117,7 +124,8 @@ namespace gtsam { boost::shared_ptr eliminateSequential( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -142,7 +150,8 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 356118dbb..6da83b8bc 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { @@ -46,9 +47,9 @@ namespace gtsam { xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - vector temp; + std::vector temp; // Copy from the FastSet into a temporary vector - copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b4498bee4..eadfc5bb5 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -107,7 +107,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), + boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams From 01f80c1fad2a2dc91eca53926fbe26f01e97bc07 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 21 Oct 2014 12:29:06 -0400 Subject: [PATCH 42/89] Correct installation of dll file for metis on windows --- gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt index a18973427..67c11e43e 100644 --- a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt @@ -9,12 +9,14 @@ if(UNIX) endif() -install(TARGETS metis - LIBRARY DESTINATION include/gtsam/3rdparty/metis/lib - RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib - ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib) -install(TARGETS metis EXPORT GTSAM-exports ARCHIVE DESTINATION lib) +if(WIN32) + set_target_properties(metis PROPERTIES + PREFIX "" + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") +endif() +install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) list(APPEND GTSAM_EXPORTED_TARGETS metis) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) + From 49d6b04eb8895eeb92e3e871b56a072a2c1c955d Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 21 Oct 2014 13:15:48 -0400 Subject: [PATCH 43/89] Metis ordering example --- examples/METISOrderingExample.cpp | 100 ++++++++++++++++++ .../metis-5.1.0/libmetis/CMakeLists.txt | 3 - 2 files changed, 100 insertions(+), 3 deletions(-) create mode 100644 examples/METISOrderingExample.cpp diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp new file mode 100644 index 000000000..145b8d5ca --- /dev/null +++ b/examples/METISOrderingExample.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 METISOrdering.cpp + * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering + * @author Frank Dellaert + * @author Andrew Melim + */ + +/** + * Example of a simple 2D localization example + * - Robot poses are facing along the X axis (horizontal, to the right in 2D) + * - The robot moves 2 meters each step + * - We have full odometry between poses + */ + +// We will use Pose2 variables (x, y, theta) to represent the robot positions +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// Here we will use Between factors for the relative motion described by odometry measurements. +// Also, we will initialize the robot at the origin using a Prior factor. +#include +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// Levenberg-Marquardt solver +#include + +// Once the optimized values have been calculated, we can also calculate the marginal covariance +// of desired variables +#include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv) { + + // Create an empty nonlinear factor graph + NonlinearFactorGraph graph; + + // Add a prior on the first pose, setting it to the origin + // A prior factor consists of a mean and a noise model (covariance matrix) + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); + graph.add(PriorFactor(1, priorMean, priorNoise)); + + // Add odometry factors + Pose2 odometry(2.0, 0.0, 0.0); + // For simplicity, we will use the same noise model for each odometry factor + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + // Create odometry (Between) factors between consecutive poses + graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); + graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.print("\nFactor Graph:\n"); // print + + // Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initial; + initial.insert(1, Pose2(0.5, 0.0, 0.2)); + initial.insert(2, Pose2(2.3, 0.1, -0.2)); + initial.insert(3, Pose2(4.1, 0.1, 0.1)); + initial.print("\nInitial Estimate:\n"); // print + + // optimize using Levenberg-Marquardt optimization + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + result.print("Final Result:\n"); + + // Calculate and print marginal covariances for all variables + cout.precision(2); + Marginals marginals(graph, result); + cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; + + return 0; +} \ No newline at end of file diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt index 67c11e43e..ea3dd0298 100644 --- a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt @@ -8,8 +8,6 @@ if(UNIX) target_link_libraries(metis m) endif() - - if(WIN32) set_target_properties(metis PROPERTIES PREFIX "" @@ -19,4 +17,3 @@ endif() install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) list(APPEND GTSAM_EXPORTED_TARGETS metis) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) - From a281240ff190998bb75f9bf0524d5372b8f9613b Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 21 Oct 2014 15:56:40 -0400 Subject: [PATCH 44/89] METIS ordering only works on values that are 0 indexed. Otherwise heap corruption occurs inside metis ordering function. Not sure how to fix/enforce --- examples/METISOrderingExample.cpp | 5 ++- .../inference/EliminateableFactorGraph-inst.h | 6 +-- gtsam/inference/MetisIndex-inl.h | 17 +++++--- gtsam/inference/MetisIndex.h | 1 + gtsam/inference/Ordering.cpp | 26 +++++------ gtsam/inference/tests/testOrdering.cpp | 43 +++++++++++++++++-- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 5 ++- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- 8 files changed, 77 insertions(+), 28 deletions(-) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 145b8d5ca..1b84364c0 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -86,7 +86,10 @@ int main(int argc, char** argv) { initial.print("\nInitial Estimate:\n"); // print // optimize using Levenberg-Marquardt optimization - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + LevenbergMarquardtParams params; + params.orderingType = Ordering::Type::METIS_; + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); result.print("Final Result:\n"); // Calculate and print marginal covariances for all variables diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 0fe65e4c0..e14ba329b 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -55,9 +55,9 @@ namespace gtsam { // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. if (orderingType == Ordering::Type::METIS_) - return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + else + return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); } } diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 6da83b8bc..08a0566a2 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -29,6 +29,7 @@ namespace gtsam { { std::map > adjMap; std::map >::iterator adjMapIt; + std::set values; /* ********** Convert to CSR format ********** */ // Assuming that vertex numbering starts from 0 (C style), @@ -37,13 +38,19 @@ namespace gtsam { // index xadj[i + 1](i.e., adjncy[xadj[i]] through // and including adjncy[xadj[i + 1] - 1]). for (size_t i = 0; i < factors.size(); i++){ - if (factors[i]) - BOOST_FOREACH(const Key& k1, *factors[i]) - BOOST_FOREACH(const Key& k2, *factors[i]) - if (k1 != k2) - adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end + if (factors[i]){ + BOOST_FOREACH(const Key& k1, *factors[i]){ + BOOST_FOREACH(const Key& k2, *factors[i]){ + if (k1 != k2) + adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end + } + values.insert(values.end(), k1); // Keep a track of all unique values + } + } } + // Number of values referenced in this factorgraph + nValues_ = values.size(); xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index bcc9fc23f..7bc595aba 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -71,6 +71,7 @@ public: std::vector xadj() const { return xadj_; } std::vector adj() const { return adj_; } + size_t nValues() const { return nValues_; } /// @} }; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index fbda41ac0..2618d8f50 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -202,30 +202,30 @@ namespace gtsam { /* ************************************************************************* */ Ordering Ordering::METIS(const MetisIndex& met) { - gttic(Ordering_METIS); + gttic(Ordering_METIS); - vector xadj = met.xadj(); - vector adj = met.adj(); + vector xadj = met.xadj(); + vector adj = met.adj(); - vector perm, iperm; + vector perm, iperm; - idx_t size = xadj.size() - 1; + idx_t size = met.nValues(); for (idx_t i = 0; i < size; i++) { perm.push_back(0); iperm.push_back(0); } - int outputError; + int outputError; - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); - Ordering result; + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); + Ordering result; - if (outputError != METIS_OK) - { - std::cout << "METIS failed during Nested Dissection ordering!\n"; - return result; - } + if (outputError != METIS_OK) + { + std::cout << "METIS failed during Nested Dissection ordering!\n"; + return result; + } result.resize(size); for (size_t j = 0; j < size; ++j) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index b769551bf..424eb7c19 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -116,17 +116,52 @@ TEST(Ordering, csr_format) { EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT( adjExpected == mi.adj()); + EXPECT(adjExpected == mi.adj()); } +/* ************************************************************************* */ + +TEST(Ordering, csr_format_2) { + SymbolicFactorGraph sfg; + + sfg.push_factor(0); + sfg.push_factor(0, 1); + sfg.push_factor(1, 2); + sfg.push_factor(2, 3); + sfg.push_factor(3, 4); + sfg.push_factor(4, 1); + + MetisIndex mi(sfg); + + vector xadjExpected { 0, 1, 4, 6, 8, 10 }; + vector adjExpected { 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); + + //Ordering metis = Ordering::METIS(sfg); + +} + /* ************************************************************************* */ TEST(Ordering, metis) { - SymbolicFactorGraph sfg; + SymbolicFactorGraph sfg; - sfg.push_factor(0, 1); + sfg.push_factor(0); + sfg.push_factor(0, 1); sfg.push_factor(1, 2); - Ordering metis = Ordering::METIS(sfg); + MetisIndex mi(sfg); + + vector xadjExpected{ 0, 1, 3, 4 }; + vector adjExpected { 1, 0, 2, 1 }; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); + + Ordering metis = Ordering::METIS(sfg); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 002f8b237..5fca680a2 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -341,7 +341,10 @@ void LevenbergMarquardtOptimizer::iterate() { LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { if (!params.ordering) - params.ordering = Ordering::COLAMD(graph); + if (params.orderingType = Ordering::Type::METIS_) + params.ordering = Ordering::METIS(graph); + else + params.ordering = Ordering::COLAMD(graph); return params; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 04877c72e..d7ead9ca3 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -154,7 +154,7 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = Ordering::Type::CUSTOM_; + this->orderingType = Ordering::Type::CUSTOM_; } std::string getOrderingType() const { From 88a11329c09b375e93dbe0a8d67ffb28271d5ac4 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 7 Nov 2014 21:54:59 -0500 Subject: [PATCH 45/89] Correct key index issue with metis ordering --- gtsam/inference/MetisIndex-inl.h | 22 ++++++++++++++++------ gtsam/inference/MetisIndex.h | 14 +++++++------- gtsam/inference/tests/testOrdering.cpp | 22 +++++++++++++++++++++- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 08a0566a2..43bc7a111 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -29,7 +29,7 @@ namespace gtsam { { std::map > adjMap; std::map >::iterator adjMapIt; - std::set values; + std::set keySet; /* ********** Convert to CSR format ********** */ // Assuming that vertex numbering starts from 0 (C style), @@ -44,17 +44,22 @@ namespace gtsam { if (k1 != k2) adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end } - values.insert(values.end(), k1); // Keep a track of all unique values + keySet.insert(keySet.end(), k1); // Keep a track of all unique keySet } } } - // Number of values referenced in this factorgraph - nValues_ = values.size(); - + // Number of keys referenced in this factor graph + nKeys_ = keySet.size(); + + + // Starting with a nonzero key crashes METIS + // Find the smallest key in the graph + size_t minKey = *keySet.begin(); // set is ordered + xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - std::vector temp; + std::vector temp; // Copy from the FastSet into a temporary vector std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ @@ -62,6 +67,11 @@ namespace gtsam { //adj_.push_back(temp); xadj_.push_back(adj_.size()); } + + // Normalize, subtract the smallest key + std::transform(adj_.begin(), adj_.end(), adj_.begin(), std::bind2nd(std::minus(), minKey)); + + } } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 7bc595aba..6aaa9246d 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -43,18 +43,18 @@ public: private: FastVector xadj_; // Index of node's adjacency list in adj FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - size_t nFactors_; // Number of factors in the original factor graph - size_t nValues_; // + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; // public: /// @name Standard Constructors /// @{ /** Default constructor, creates empty MetisIndex */ - MetisIndex() : nFactors_(0), nValues_(0) {} + MetisIndex() : nFactors_(0), nKeys_(0) {} template - MetisIndex(const FG& factorGraph) : nFactors_(0), nValues_(0) { + MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { augment(factorGraph); } ~MetisIndex(){} @@ -69,9 +69,9 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nValues_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } /// @} }; diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 424eb7c19..f2a0e1443 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -139,7 +139,27 @@ TEST(Ordering, csr_format_2) { EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - //Ordering metis = Ordering::METIS(sfg); +} +/* ************************************************************************* */ + +TEST(Ordering, csr_format_3) { + SymbolicFactorGraph sfg; + + sfg.push_factor(100); + sfg.push_factor(100, 101); + sfg.push_factor(101, 102); + sfg.push_factor(102, 103); + sfg.push_factor(103, 104); + sfg.push_factor(104, 101); + + MetisIndex mi(sfg); + + vector xadjExpected{ 0, 1, 4, 6, 8, 10 }; + vector adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == mi.adj()); } From ea19fae15582c4f681994f25d5640efac935d27a Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 7 Nov 2014 22:00:19 -0500 Subject: [PATCH 46/89] Formatting --- gtsam/inference/MetisIndex-inl.h | 86 ++++++++++++++++---------------- gtsam/inference/MetisIndex.h | 78 ++++++++++++++--------------- 2 files changed, 81 insertions(+), 83 deletions(-) diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 43bc7a111..cab184ad0 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -23,55 +23,53 @@ namespace gtsam { - /* ************************************************************************* */ - template - void MetisIndex::augment(const FactorGraph& factors) - { - std::map > adjMap; - std::map >::iterator adjMapIt; - std::set keySet; +/* ************************************************************************* */ +template +void MetisIndex::augment(const FactorGraph& factors) +{ + std::map > adjMap; + std::map >::iterator adjMapIt; + std::set keySet; - /* ********** Convert to CSR format ********** */ - // Assuming that vertex numbering starts from 0 (C style), - // then the adjacency list of vertex i is stored in array adjncy - // starting at index xadj[i] and ending at(but not including) - // index xadj[i + 1](i.e., adjncy[xadj[i]] through - // and including adjncy[xadj[i + 1] - 1]). - for (size_t i = 0; i < factors.size(); i++){ - if (factors[i]){ - BOOST_FOREACH(const Key& k1, *factors[i]){ - BOOST_FOREACH(const Key& k2, *factors[i]){ - if (k1 != k2) - adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end - } - keySet.insert(keySet.end(), k1); // Keep a track of all unique keySet - } - } - } + /* ********** Convert to CSR format ********** */ + // Assuming that vertex numbering starts from 0 (C style), + // then the adjacency list of vertex i is stored in array adjncy + // starting at index xadj[i] and ending at(but not including) + // index xadj[i + 1](i.e., adjncy[xadj[i]] through + // and including adjncy[xadj[i + 1] - 1]). + for (size_t i = 0; i < factors.size(); i++){ + if (factors[i]){ + BOOST_FOREACH(const Key& k1, *factors[i]){ + BOOST_FOREACH(const Key& k2, *factors[i]){ + if (k1 != k2) + adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end + } + keySet.insert(keySet.end(), k1); // Keep a track of all unique keySet + } + } + } - // Number of keys referenced in this factor graph - nKeys_ = keySet.size(); + // Number of keys referenced in this factor graph + nKeys_ = keySet.size(); - // Starting with a nonzero key crashes METIS - // Find the smallest key in the graph - size_t minKey = *keySet.begin(); // set is ordered + // Starting with a nonzero key crashes METIS + // Find the smallest key in the graph + size_t minKey = *keySet.begin(); // set is ordered - xadj_.push_back(0);// Always set the first index to zero - for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - std::vector temp; - // Copy from the FastSet into a temporary vector - std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); - // Insert each index's set in order by appending them to the end of adj_ - adj_.insert(adj_.end(), temp.begin(), temp.end()); - //adj_.push_back(temp); - xadj_.push_back(adj_.size()); - } + xadj_.push_back(0);// Always set the first index to zero + for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { + std::vector temp; + // Copy from the FastSet into a temporary vector + std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + // Insert each index's set in order by appending them to the end of adj_ + adj_.insert(adj_.end(), temp.begin(), temp.end()); + //adj_.push_back(temp); + xadj_.push_back(adj_.size()); + } - // Normalize, subtract the smallest key - std::transform(adj_.begin(), adj_.end(), adj_.begin(), std::bind2nd(std::minus(), minKey)); - - - } + // Normalize, subtract the smallest key + std::transform(adj_.begin(), adj_.end(), adj_.begin(), std::bind2nd(std::minus(), minKey)); +} } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 6aaa9246d..eaff2546f 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -28,53 +28,53 @@ #include namespace gtsam { - /** - * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in - * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built - * fromt a factor graph prior to elimination, and stores the list of factors - * that involve each variable. - * \nosubgrouping - */ -class GTSAM_EXPORT MetisIndex -{ -public: - typedef boost::shared_ptr shared_ptr; + /** + * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in + * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built + * fromt a factor graph prior to elimination, and stores the list of factors + * that involve each variable. + * \nosubgrouping + */ + class GTSAM_EXPORT MetisIndex + { + public: + typedef boost::shared_ptr shared_ptr; -private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - size_t nFactors_; // Number of factors in the original factor graph - size_t nKeys_; // + private: + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; // -public: - /// @name Standard Constructors - /// @{ + public: + /// @name Standard Constructors + /// @{ - /** Default constructor, creates empty MetisIndex */ - MetisIndex() : nFactors_(0), nKeys_(0) {} + /** Default constructor, creates empty MetisIndex */ + MetisIndex() : nFactors_(0), nKeys_(0) {} - template - MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { - augment(factorGraph); } + template + MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { + augment(factorGraph); } - ~MetisIndex(){} - /// @} - /// @name Advanced Interface - /// @{ + ~MetisIndex(){} + /// @} + /// @name Advanced Interface + /// @{ - /** - * Augment the variable index with new factors. This can be used when - * solving problems incrementally. - */ - template - void augment(const FactorGraph& factors); + /** + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } - /// @} -}; + /// @} + }; } From c520bf2b47055ca196c1919d14df9375ec1bb17a Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 7 Nov 2014 22:38:37 -0500 Subject: [PATCH 47/89] Working METIS ordering example. --- gtsam/inference/MetisIndex-inl.h | 19 ++++++++----------- gtsam/inference/MetisIndex.h | 8 +++++--- gtsam/inference/Ordering.cpp | 12 +++++++++--- gtsam/inference/tests/testOrdering.cpp | 8 +++++++- 4 files changed, 29 insertions(+), 18 deletions(-) diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index cab184ad0..d9fb4cfd1 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -55,21 +55,18 @@ void MetisIndex::augment(const FactorGraph& factors) // Starting with a nonzero key crashes METIS // Find the smallest key in the graph - size_t minKey = *keySet.begin(); // set is ordered + minKey_ = *keySet.begin(); // set is ordered xadj_.push_back(0);// Always set the first index to zero for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - std::vector temp; - // Copy from the FastSet into a temporary vector - std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); - // Insert each index's set in order by appending them to the end of adj_ - adj_.insert(adj_.end(), temp.begin(), temp.end()); - //adj_.push_back(temp); - xadj_.push_back(adj_.size()); + std::vector temp; + // Copy from the FastSet into a temporary vector + std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + // Insert each index's set in order by appending them to the end of adj_ + adj_.insert(adj_.end(), temp.begin(), temp.end()); + //adj_.push_back(temp); + xadj_.push_back(adj_.size()); } - - // Normalize, subtract the smallest key - std::transform(adj_.begin(), adj_.end(), adj_.begin(), std::bind2nd(std::minus(), minKey)); } } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index eaff2546f..57d097876 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -45,6 +45,7 @@ namespace gtsam { FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector size_t nFactors_; // Number of factors in the original factor graph size_t nKeys_; // + size_t minKey_; public: /// @name Standard Constructors @@ -69,9 +70,10 @@ namespace gtsam { template void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } + size_t minKey() const { return minKey_; } /// @} }; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2618d8f50..654740163 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -206,6 +206,10 @@ namespace gtsam { vector xadj = met.xadj(); vector adj = met.adj(); + size_t minKey = met.minKey(); + + // Normalize, subtract the smallest key + std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus(), minKey)); vector perm, iperm; @@ -228,9 +232,11 @@ namespace gtsam { } result.resize(size); - for (size_t j = 0; j < size; ++j) - result[j] = perm[j]; - return result; + for (size_t j = 0; j < size; ++j){ + // We have to add the minKey value back to obtain the original key in the Values + result[j] = perm[j] + minKey; + } + return result; } /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index f2a0e1443..7e1ccb838 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -156,10 +156,16 @@ TEST(Ordering, csr_format_3) { vector xadjExpected{ 0, 1, 4, 6, 8, 10 }; vector adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; + size_t minKey = mi.minKey(); + + vector adjAcutal = mi.adj(); + + // Normalize, subtract the smallest key + std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), std::bind2nd(std::minus(), minKey)); EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT(adjExpected == mi.adj()); + EXPECT(adjExpected == adjAcutal); } From f00f8d1d7afcf8c9c3cab39d34481b1aed08d162 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 17 Nov 2014 11:31:11 -0500 Subject: [PATCH 48/89] Formatting changes --- .../inference/EliminateableFactorGraph-inst.h | 14 +-- gtsam/inference/EliminateableFactorGraph.h | 18 ++-- gtsam/inference/MetisIndex.h | 93 +++++++++---------- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 3 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 6 +- 5 files changed, 66 insertions(+), 68 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index e14ba329b..153b828d9 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -65,8 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateMultifrontal); @@ -86,16 +86,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); + return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::Type::METIS_) - return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); - else - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::Type::METIS_) + return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + else + return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 820bb2fd3..c048de94b 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -94,8 +94,8 @@ namespace gtsam { /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; - /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + /// Typedef for an optional ordering type + typedef boost::optional OptionalOrderingType; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -104,10 +104,10 @@ namespace gtsam { * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); * \endcode - * - * Example - METIS ordering for elimination - * \code - * boost::shared_ptr result = graph.eliminateSequential(Ordering::Type::METIS_); + * + * Example - METIS ordering for elimination + * \code + * boost::shared_ptr result = graph.eliminateSequential(Ordering::Type::METIS_); * * Example - Full QR elimination in specified order: * \code @@ -125,7 +125,7 @@ namespace gtsam { OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -150,8 +150,8 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 57d097876..b058b5564 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -9,7 +9,6 @@ * -------------------------------------------------------------------------- */ - /** * @file MetisIndex.h * @author Andrew Melim @@ -28,55 +27,55 @@ #include namespace gtsam { +/** + * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in + * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built + * fromt a factor graph prior to elimination, and stores the list of factors + * that involve each variable. + * \nosubgrouping + */ +class GTSAM_EXPORT MetisIndex +{ +public: + typedef boost::shared_ptr shared_ptr; + +private: + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; // + size_t minKey_; + +public: + /// @name Standard Constructors + /// @{ + + /** Default constructor, creates empty MetisIndex */ + MetisIndex() : nFactors_(0), nKeys_(0) {} + + template + MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { + augment(factorGraph); } + + ~MetisIndex(){} + /// @} + /// @name Advanced Interface + /// @{ + /** - * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in - * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built - * fromt a factor graph prior to elimination, and stores the list of factors - * that involve each variable. - * \nosubgrouping - */ - class GTSAM_EXPORT MetisIndex - { - public: - typedef boost::shared_ptr shared_ptr; + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FactorGraph& factors); - private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - size_t nFactors_; // Number of factors in the original factor graph - size_t nKeys_; // - size_t minKey_; + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } + size_t minKey() const { return minKey_; } - public: - /// @name Standard Constructors - /// @{ - - /** Default constructor, creates empty MetisIndex */ - MetisIndex() : nFactors_(0), nKeys_(0) {} - - template - MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { - augment(factorGraph); } - - ~MetisIndex(){} - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Augment the variable index with new factors. This can be used when - * solving problems incrementally. - */ - template - void augment(const FactorGraph& factors); - - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } - size_t minKey() const { return minKey_; } - - /// @} - }; + /// @} +}; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 5fca680a2..a8dab8266 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -340,11 +340,12 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { - if (!params.ordering) + if (!params.ordering){ if (params.orderingType = Ordering::Type::METIS_) params.ordering = Ordering::METIS(graph); else params.ordering = Ordering::COLAMD(graph); + } return params; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 1e964a481..626758bcb 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -165,8 +165,7 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type) const -{ +std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type) const{ switch (type) { case Ordering::Type::METIS_: return "METIS"; @@ -182,8 +181,7 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type } /* ************************************************************************* */ -Ordering::Type NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const -{ +Ordering::Type NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ if (type == "METIS") return Ordering::Type::METIS_; if (type == "COLAMD") From ffae14d42e6ee9b06b13848d2ee72147a54c9818 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 17 Nov 2014 11:57:22 -0500 Subject: [PATCH 49/89] Corrected scoped enum issue for non c++11 compilers --- examples/METISOrderingExample.cpp | 2 +- gtsam/inference/EliminateableFactorGraph-inst.h | 4 ++-- gtsam/inference/EliminateableFactorGraph.h | 4 ++-- gtsam/inference/Ordering.h | 11 +++++------ gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 16 ++++++++-------- gtsam/nonlinear/NonlinearOptimizerParams.h | 10 +++++----- 7 files changed, 24 insertions(+), 25 deletions(-) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 1b84364c0..6b54b8d70 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -87,7 +87,7 @@ int main(int argc, char** argv) { // optimize using Levenberg-Marquardt optimization LevenbergMarquardtParams params; - params.orderingType = Ordering::Type::METIS_; + params.orderingType = OrderingType::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); result.print("Final Result:\n"); diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 153b828d9..c0c95ce88 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -54,7 +54,7 @@ namespace gtsam { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::Type::METIS_) + if (orderingType == OrderingType::METIS) return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); else return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); @@ -92,7 +92,7 @@ namespace gtsam { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::Type::METIS_) + if (orderingType == OrderingType::METIS) return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); else return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index c048de94b..f0baf55a0 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -95,7 +95,7 @@ namespace gtsam { typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + typedef boost::optional OptionalOrderingType; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -107,7 +107,7 @@ namespace gtsam { * * Example - METIS ordering for elimination * \code - * boost::shared_ptr result = graph.eliminateSequential(Ordering::Type::METIS_); + * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); * * Example - Full QR elimination in specified order: * \code diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index dfb1deb0e..3c5b41535 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -29,6 +29,11 @@ #include namespace gtsam { + + enum OrderingType { + COLAMD, METIS, CUSTOM + }; + class Ordering : public std::vector { protected: typedef std::vector Base; @@ -37,12 +42,6 @@ namespace gtsam { typedef Ordering This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - /** See NonlinearOptimizer::orderingType */ - enum Type { - COLAMD_, METIS_, CUSTOM_ // Add underscores to prevent declaration errors - }; - - /// Create an empty ordering GTSAM_EXPORT Ordering() {} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index a8dab8266..59acad3c3 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -341,7 +341,7 @@ void LevenbergMarquardtOptimizer::iterate() { LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { if (!params.ordering){ - if (params.orderingType = Ordering::Type::METIS_) + if (params.orderingType = OrderingType::METIS) params.ordering = Ordering::METIS(graph); else params.ordering = Ordering::COLAMD(graph); diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 626758bcb..79bc64414 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -109,10 +109,10 @@ void NonlinearOptimizerParams::print(const std::string& str) const { } switch (orderingType){ - case Ordering::Type::COLAMD_: + case OrderingType::COLAMD: std::cout << " ordering: COLAMD\n"; break; - case Ordering::Type::METIS_: + case OrderingType::METIS: std::cout << " ordering: METIS\n"; break; default: @@ -165,11 +165,11 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type) const{ +std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type) const{ switch (type) { - case Ordering::Type::METIS_: + case OrderingType::METIS: return "METIS"; - case Ordering::Type::COLAMD_: + case OrderingType::COLAMD: return "COLAMD"; default: if (ordering) @@ -181,11 +181,11 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::Type type } /* ************************************************************************* */ -Ordering::Type NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ +OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ if (type == "METIS") - return Ordering::Type::METIS_; + return OrderingType::METIS; if (type == "COLAMD") - return Ordering::Type::COLAMD_; + return OrderingType::COLAMD; throw std::invalid_argument( "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index d7ead9ca3..2cb055465 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -43,11 +43,11 @@ public: double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - Ordering::Type orderingType; ///< The method of ordering use during variable elimination (default COLAMD) + OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(Ordering::Type::COLAMD_) { + 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(COLAMD) { } virtual ~NonlinearOptimizerParams() { @@ -154,7 +154,7 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = Ordering::Type::CUSTOM_; + this->orderingType = OrderingType::CUSTOM; } std::string getOrderingType() const { @@ -171,9 +171,9 @@ private: LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::Type type) const; + std::string orderingTypeTranslator(OrderingType type) const; - Ordering::Type orderingTypeTranslator(const std::string& type) const; + OrderingType orderingTypeTranslator(const std::string& type) const; }; From 9c2dcfb70c57e520088834220042c144600b19d3 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 17 Nov 2014 12:06:59 -0500 Subject: [PATCH 50/89] Slim down example to remove verbosity, added explanation on orderingType --- examples/METISOrderingExample.cpp | 47 +++---------------------------- 1 file changed, 4 insertions(+), 43 deletions(-) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 6b54b8d70..1fae4c0b9 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -17,68 +17,34 @@ */ /** - * Example of a simple 2D localization example - * - Robot poses are facing along the X axis (horizontal, to the right in 2D) - * - The robot moves 2 meters each step - * - We have full odometry between poses + * Example of a simple 2D localization example optimized using METIS ordering + * - For more details on the full optimization pipeline, see OdometryExample.cpp */ -// We will use Pose2 variables (x, y, theta) to represent the robot positions #include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// Also, we will initialize the robot at the origin using a Prior factor. #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. #include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the -// Levenberg-Marquardt solver #include - -// Once the optimized values have been calculated, we can also calculate the marginal covariance -// of desired variables #include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. #include using namespace std; using namespace gtsam; int main(int argc, char** argv) { - - // Create an empty nonlinear factor graph NonlinearFactorGraph graph; - // Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); graph.add(PriorFactor(1, priorMean, priorNoise)); - // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); graph.print("\nFactor Graph:\n"); // print - // Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values Values initial; initial.insert(1, Pose2(0.5, 0.0, 0.2)); initial.insert(2, Pose2(2.3, 0.1, -0.2)); @@ -87,17 +53,12 @@ int main(int argc, char** argv) { // optimize using Levenberg-Marquardt optimization LevenbergMarquardtParams params; + // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" + // By default this parameter is set to OrderingType::COLAMD params.orderingType = OrderingType::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); result.print("Final Result:\n"); - // Calculate and print marginal covariances for all variables - cout.precision(2); - Marginals marginals(graph, result); - cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; - cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; - cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; - return 0; } \ No newline at end of file From 36a485169d15e603f2e7607efd16e6d5edf5abf1 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 17 Nov 2014 16:16:52 -0500 Subject: [PATCH 51/89] Refactor Ordering parameters. Now compiles and passes with gcc --- examples/METISOrderingExample.cpp | 4 +- examples/SolverComparer.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 22 ++++---- gtsam/inference/EliminateableFactorGraph.h | 2 +- gtsam/inference/ISAM-inst.h | 2 +- gtsam/inference/Ordering.cpp | 20 +++---- gtsam/inference/Ordering.h | 42 ++++++++------- gtsam/inference/tests/testOrdering.cpp | 53 ++++++++++--------- gtsam/nonlinear/DoglegOptimizer.cpp | 2 +- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 2 +- gtsam/nonlinear/ISAM2.cpp | 8 +-- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 6 +-- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 16 +++--- gtsam/nonlinear/NonlinearOptimizerParams.h | 12 ++--- .../nonlinear/BatchFixedLagSmoother.cpp | 2 +- .../nonlinear/ConcurrentBatchFilter.cpp | 4 +- .../nonlinear/ConcurrentBatchSmoother.cpp | 2 +- tests/testNonlinearFactorGraph.cpp | 4 +- 19 files changed, 108 insertions(+), 101 deletions(-) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 1fae4c0b9..452ba523e 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -55,10 +55,10 @@ int main(int argc, char** argv) { LevenbergMarquardtParams params; // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType" // By default this parameter is set to OrderingType::COLAMD - params.orderingType = OrderingType::METIS; + params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); result.print("Final Result:\n"); return 0; -} \ No newline at end of file +} diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 26abfff85..3dd978ee3 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -589,7 +589,7 @@ void runStats() { cout << "Gathering statistics..." << endl; GaussianFactorGraph linear = *datasetMeasurements.linearize(initial); - GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear))); + GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear))); treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt); ofstream file; diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index c0c95ce88..5e261e200 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -54,10 +54,10 @@ namespace gtsam { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == OrderingType::METIS) - return eliminateSequential(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) + return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType); else - return eliminateSequential(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -92,10 +92,10 @@ namespace gtsam { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == OrderingType::METIS) - return eliminateMultifrontal(Ordering::METIS(asDerived()), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) + return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType); else - return eliminateMultifrontal(Ordering::COLAMD(*variableIndex), function, variableIndex, orderingType); + return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType); } } @@ -125,7 +125,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -163,7 +163,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -216,7 +216,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -275,7 +275,7 @@ namespace gtsam { boost::get(&variables) : boost::get&>(&variables); Ordering totalOrdering = - Ordering::COLAMDConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -301,7 +301,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index f0baf55a0..5fb5fbdb1 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -95,7 +95,7 @@ namespace gtsam { typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + typedef boost::optional OptionalOrderingType; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 645499b2c..7cb3d9817 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -46,7 +46,7 @@ namespace gtsam { const VariableIndex varIndex(factors); const FastSet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = - Ordering::COLAMDConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); + Ordering::colamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 654740163..6c72fe675 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -39,15 +39,15 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering Ordering::COLAMD(const VariableIndex& variableIndex) + Ordering Ordering::colamd(const VariableIndex& variableIndex) { // Call constrained version with all groups set to zero vector dummy_groups(variableIndex.size(), 0); - return Ordering::COLAMDConstrained(variableIndex, dummy_groups); + return Ordering::colamdConstrained(variableIndex, dummy_groups); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrained( + Ordering Ordering::colamdConstrained( const VariableIndex& variableIndex, std::vector& cmember) { gttic(Ordering_COLAMDConstrained); @@ -114,7 +114,7 @@ namespace gtsam { } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrainedLast( + Ordering Ordering::colamdConstrainedLast( const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder) { gttic(Ordering_COLAMDConstrainedLast); @@ -137,11 +137,11 @@ namespace gtsam { ++ group; } - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrainedFirst( + Ordering Ordering::colamdConstrainedFirst( const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder) { gttic(Ordering_COLAMDConstrainedFirst); @@ -171,11 +171,11 @@ namespace gtsam { if(c == none) c = group; - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex, + Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, const FastMap& groups) { gttic(Ordering_COLAMDConstrained); @@ -195,12 +195,12 @@ namespace gtsam { cmember[keyIndices.at(p.first)] = p.second; } - return Ordering::COLAMDConstrained(variableIndex, cmember); + return Ordering::colamdConstrained(variableIndex, cmember); } /* ************************************************************************* */ - Ordering Ordering::METIS(const MetisIndex& met) + Ordering Ordering::metis(const MetisIndex& met) { gttic(Ordering_METIS); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 3c5b41535..24c811841 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,15 +30,17 @@ namespace gtsam { - enum OrderingType { - COLAMD, METIS, CUSTOM - }; - class Ordering : public std::vector { protected: typedef std::vector Base; public: + + /// Type of ordering to use + enum OrderingType { + COLAMD, METIS, CUSTOM + }; + typedef Ordering This; ///< Typedef to this class typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class @@ -69,11 +71,11 @@ namespace gtsam { /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, /// it is faster to use COLAMD(const VariableIndex&) template - static Ordering COLAMD(const FactorGraph& graph) { - return COLAMD(VariableIndex(graph)); } + static Ordering colamd(const FactorGraph& graph) { + return colamd(VariableIndex(graph)); } /// Compute a fill-reducing ordering using COLAMD from a VariableIndex. - static GTSAM_EXPORT Ordering COLAMD(const VariableIndex& variableIndex); + static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// for note on performance). This internally builds a VariableIndex so if you already have a @@ -84,9 +86,9 @@ namespace gtsam { /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering COLAMDConstrainedLast(const FactorGraph& graph, + static Ordering colamdConstrainedLast(const FactorGraph& graph, const std::vector& constrainLast, bool forceOrder = false) { - return COLAMDConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } + return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// function constrains the variables in \c constrainLast to the end of the ordering, and orders @@ -94,7 +96,7 @@ namespace gtsam { /// variables in \c constrainLast will be ordered in the same order specified in the vector /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex, const std::vector& constrainLast, bool forceOrder = false); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details @@ -106,9 +108,9 @@ namespace gtsam { /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template - static Ordering COLAMDConstrainedFirst(const FactorGraph& graph, + static Ordering colamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { - return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } + return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// function constrains the variables in \c constrainFirst to the front of the ordering, and @@ -117,7 +119,7 @@ namespace gtsam { /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. - static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details @@ -130,9 +132,9 @@ namespace gtsam { /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the /// CCOLAMD documentation for more information. template - static Ordering COLAMDConstrained(const FactorGraph& graph, + static Ordering colamdConstrained(const FactorGraph& graph, const FastMap& groups) { - return COLAMDConstrained(VariableIndex(graph), groups); } + return colamdConstrained(VariableIndex(graph), groups); } /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this /// function, a group for each variable should be specified in \c groups, and each group of @@ -141,7 +143,7 @@ namespace gtsam { /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the /// supplied indices, see the CCOLAMD documentation for more information. - static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, + static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers @@ -158,12 +160,12 @@ namespace gtsam { static GTSAM_EXPORT void CSRFormat(std::vector& xadj, std::vector& adj, const FactorGraph& graph); /// Compute an ordering determined by METIS from a VariableIndex - static GTSAM_EXPORT Ordering METIS(const MetisIndex& met); + static GTSAM_EXPORT Ordering metis(const MetisIndex& met); template - static Ordering METIS(const FactorGraph& graph) + static Ordering metis(const FactorGraph& graph) { - return METIS(MetisIndex(graph)); + return metis(MetisIndex(graph)); } /// @} @@ -178,7 +180,7 @@ namespace gtsam { private: /// Internal COLAMD function - static GTSAM_EXPORT Ordering COLAMDConstrained( + static GTSAM_EXPORT Ordering colamdConstrained( const VariableIndex& variableIndex, std::vector& cmember); diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 7e1ccb838..57de00646 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include using namespace std; using namespace gtsam; @@ -40,17 +40,17 @@ TEST(Ordering, constrained_ordering) { sfg.push_factor(4,5); // unconstrained version - Ordering actUnconstrained = Ordering::COLAMD(sfg); + Ordering actUnconstrained = Ordering::colamd(sfg); Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5)); EXPECT(assert_equal(expUnconstrained, actUnconstrained)); // constrained version - push one set to the end - Ordering actConstrained = Ordering::COLAMDConstrainedLast(sfg, list_of(2)(4)); + Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4)); Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2)); EXPECT(assert_equal(expConstrained, actConstrained)); // constrained version - push one set to the start - Ordering actConstrained2 = Ordering::COLAMDConstrainedFirst(sfg, list_of(2)(4)); + Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); EXPECT(assert_equal(expConstrained2, actConstrained2)); } @@ -74,7 +74,7 @@ TEST(Ordering, grouped_constrained_ordering) { constraints[4] = 1; constraints[5] = 2; - Ordering actConstrained = Ordering::COLAMDConstrained(sfg, constraints); + Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints); Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5); EXPECT(assert_equal(expConstrained, actConstrained)); } @@ -109,17 +109,18 @@ TEST(Ordering, csr_format) { MetisIndex mi(sfg); - vector xadjExpected{ 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44}; - vector adjExpected{ 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, + vector xadjExpected, adjExpected; + xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; + adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, - 13, 8, 12, 14, 9, 13 }; + 13, 8, 12, 14, 9, 13 ; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); } -/* ************************************************************************* */ +/* ************************************************************************* */ TEST(Ordering, csr_format_2) { SymbolicFactorGraph sfg; @@ -132,16 +133,17 @@ TEST(Ordering, csr_format_2) { MetisIndex mi(sfg); - vector xadjExpected { 0, 1, 4, 6, 8, 10 }; - vector adjExpected { 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 4, 6, 8, 10; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); } -/* ************************************************************************* */ +/* ************************************************************************* */ TEST(Ordering, csr_format_3) { SymbolicFactorGraph sfg; @@ -154,40 +156,43 @@ TEST(Ordering, csr_format_3) { MetisIndex mi(sfg); - vector xadjExpected{ 0, 1, 4, 6, 8, 10 }; - vector adjExpected{ 1, 0, 2, 4, 1, 3, 2, 4, 1, 3 }; - size_t minKey = mi.minKey(); + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 4, 6, 8, 10; + adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; + size_t minKey = mi.minKey(); - vector adjAcutal = mi.adj(); + vector adjAcutal = mi.adj(); - // Normalize, subtract the smallest key - std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), std::bind2nd(std::minus(), minKey)); + // Normalize, subtract the smallest key + std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), + std::bind2nd(std::minus(), minKey)); EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); - EXPECT(adjExpected == adjAcutal); + EXPECT(adjExpected == adjAcutal); } /* ************************************************************************* */ TEST(Ordering, metis) { - + SymbolicFactorGraph sfg; sfg.push_factor(0); sfg.push_factor(0, 1); - sfg.push_factor(1, 2); + sfg.push_factor(1, 2); MetisIndex mi(sfg); - vector xadjExpected{ 0, 1, 3, 4 }; - vector adjExpected { 1, 0, 2, 1 }; + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 3, 4; + adjExpected += 1, 0, 2, 1; EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == mi.adj()); - Ordering metis = Ordering::METIS(sfg); + Ordering metis = Ordering::metis(sfg); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 082cc66c8..e148bd65d 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -96,7 +96,7 @@ void DoglegOptimizer::iterate(void) { /* ************************************************************************* */ DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); return params; } diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index 823f3a6ac..7a115e1c4 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -49,7 +49,7 @@ GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering( GaussNewtonParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); return params; } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6d6785b14..3b3d76285 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -341,7 +341,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe Ordering order; if(constrainKeys) { - order = Ordering::COLAMDConstrained(variableIndex_, *constrainKeys); + order = Ordering::colamdConstrained(variableIndex_, *constrainKeys); } else { @@ -351,11 +351,11 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe FastMap constraintGroups; BOOST_FOREACH(Key var, observedKeys) constraintGroups[var] = 1; - order = Ordering::COLAMDConstrained(variableIndex_, constraintGroups); + order = Ordering::colamdConstrained(variableIndex_, constraintGroups); } else { - order = Ordering::COLAMD(variableIndex_); + order = Ordering::colamd(variableIndex_); } } gttoc(ordering); @@ -481,7 +481,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::COLAMDConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 59acad3c3..473caa35e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -341,10 +341,10 @@ void LevenbergMarquardtOptimizer::iterate() { LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { if (!params.ordering){ - if (params.orderingType = OrderingType::METIS) - params.ordering = Ordering::METIS(graph); + if (params.orderingType == Ordering::METIS) + params.ordering = Ordering::metis(graph); else - params.ordering = Ordering::COLAMD(graph); + params.ordering = Ordering::colamd(graph); } return params; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 63a1a2218..fd14750ac 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -282,13 +282,13 @@ FastSet NonlinearFactorGraph::keys() const { /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { - return Ordering::COLAMD(*this); + return Ordering::colamd(*this); } /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap& constraints) const { - return Ordering::COLAMDConstrained(*this, constraints); + return Ordering::colamdConstrained(*this, constraints); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 79bc64414..5a163ffb9 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -109,10 +109,10 @@ void NonlinearOptimizerParams::print(const std::string& str) const { } switch (orderingType){ - case OrderingType::COLAMD: + case Ordering::COLAMD: std::cout << " ordering: COLAMD\n"; break; - case OrderingType::METIS: + case Ordering::METIS: std::cout << " ordering: METIS\n"; break; default: @@ -165,11 +165,11 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type) const{ +std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{ switch (type) { - case OrderingType::METIS: + case Ordering::METIS: return "METIS"; - case OrderingType::COLAMD: + case Ordering::COLAMD: return "COLAMD"; default: if (ordering) @@ -181,11 +181,11 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(OrderingType type) } /* ************************************************************************* */ -OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ +Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ if (type == "METIS") - return OrderingType::METIS; + return Ordering::METIS; if (type == "COLAMD") - return OrderingType::COLAMD; + return Ordering::COLAMD; throw std::invalid_argument( "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 2cb055465..10de6994f 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -43,12 +43,12 @@ public: double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) + Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY), orderingType(COLAMD) { - } + 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), + linearSolverType(MULTIFRONTAL_CHOLESKY) {} virtual ~NonlinearOptimizerParams() { } @@ -154,7 +154,7 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = OrderingType::CUSTOM; + this->orderingType = Ordering::CUSTOM; } std::string getOrderingType() const { @@ -171,9 +171,9 @@ private: LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(OrderingType type) const; + std::string orderingTypeTranslator(Ordering::OrderingType type) const; - OrderingType orderingTypeTranslator(const std::string& type) const; + Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index ed4b66616..05b0bb49e 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -191,7 +191,7 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); if(debug) { ordering_.print("New Ordering: "); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 3bb9c7e44..fcaae9626 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& keysT // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 if(keysToMove && keysToMove->size() > 0) { - ordering_ = Ordering::COLAMDConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(keysToMove->begin(), keysToMove->end())); }else{ - ordering_ = Ordering::COLAMD(factors_); + ordering_ = Ordering::colamd(factors_); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 8606538bf..0f6515056 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() { variableIndex_ = VariableIndex(factors_); FastList separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::COLAMDConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9c2eddcc3..f398a33fe 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING) { Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); - Ordering actual = Ordering::COLAMD(nlfg); + Ordering actual = Ordering::colamd(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); FastMap constraints; constraints[X(2)] = 1; - Ordering actualConstrained = Ordering::COLAMDConstrained(nlfg, constraints); + Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints); EXPECT(assert_equal(expectedConstrained, actualConstrained)); } From dde32f7fe2800b33794e52c598a3f7891715e9b4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 17 Nov 2014 19:10:27 -0500 Subject: [PATCH 52/89] templated SmartFactor classes on measurement dimension, ZDim --- gtsam/slam/JacobianFactorQ.h | 16 ++++---- gtsam/slam/JacobianFactorQR.h | 12 +++--- gtsam/slam/JacobianFactorSVD.h | 18 ++++----- gtsam/slam/JacobianSchurFactor.h | 4 +- gtsam/slam/SmartFactorBase.h | 38 ++++++++++--------- gtsam/slam/SmartProjectionFactor.h | 12 +++--- gtsam/slam/SmartStereoProjectionFactor.h | 4 +- .../tests/testRegularImplicitSchurFactor.cpp | 4 +- 8 files changed, 57 insertions(+), 51 deletions(-) diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index de13f7ef0..9b9383d7b 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -12,10 +12,10 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorQ: public JacobianSchurFactor { +template +class JacobianFactorQ: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; public: @@ -25,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -37,11 +37,11 @@ public: } /// Constructor - JacobianFactorQ(const std::vector >& Fblocks, + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { - size_t j = 0, m2 = E.rows(), m = m2 / 2; + JacobianSchurFactor() { + size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices @@ -51,7 +51,7 @@ public: QF.reserve(m); // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); } diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index a928106a8..ccd6e8756 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -12,10 +12,10 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorQR: public JacobianSchurFactor { +template +class JacobianFactorQR: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; public: @@ -25,14 +25,14 @@ public: JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + JacobianSchurFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { - gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, - b.segment<2>(2 * i), model); + gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, + b.segment(ZDim * i), model); i += 1; } //gfg.print("gfg"); diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 8bed77d0f..e0a5f4566 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -11,12 +11,12 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorSVD: public JacobianSchurFactor { +template +class JacobianFactorSVD: public JacobianSchurFactor { public: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 + typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; @@ -25,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -36,10 +36,10 @@ public: } /// Constructor - JacobianFactorSVD(const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - size_t numKeys = Enull.rows() / Z::Dim(); - size_t j = 0, m2 = Z::Dim()*numKeys-3; + JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + size_t numKeys = Enull.rows() / ZDim; + size_t j = 0, m2 = ZDim*numKeys-3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) @@ -48,7 +48,7 @@ public: std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, Z::Dim() * j++, m2, Z::Dim()) * it.second)); + QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index 2beecc264..5d28bbada 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -18,12 +18,12 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template +template class JacobianSchurFactor: public JacobianFactor { public: - typedef Eigen::Matrix Matrix2D; + typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; // Use eigen magic to access raw memory diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f7751422e..7c5a1175d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,7 +34,6 @@ #include #include #include -//#include namespace gtsam { /// Base class with no internal point, completely functional @@ -50,14 +49,16 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; + typedef Eigen::Matrix Matrix2; /// shorthand for base class type typedef NonlinearFactor Base; @@ -65,6 +66,7 @@ protected: /// shorthand for this class typedef SmartFactorBase This; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -262,7 +264,7 @@ public: // **************************************************************************************************** /// Compute F, E only (called below in both vanilla and SVD versions) /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector >& Fblocks, Matrix& E, + double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { size_t numKeys = this->keys_.size(); @@ -292,11 +294,11 @@ public: if (D == 6) { // optimize only camera pose Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); } else { - Hcam.block(0, 0) = Fi; // Z::Dim() x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras + Hcam.block(0, 0) = Fi; // Z::Dim() x 6 block for the cameras + Hcam.block(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); } - E.block(Z::dimension * i, 0) = Ei; + E.block(ZDim_t::value * i, 0) = Ei; subInsert(b, bi, Z::Dim() * i); } return f; @@ -304,7 +306,7 @@ public: // **************************************************************************************************** /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector >& Fblocks, Matrix& E, + double computeJacobians(std::vector& Fblocks, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -335,20 +337,20 @@ public: const double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector > Fblocks; + std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(Z::Dim() * numKeys, D * numKeys); for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras + F.block(ZDim_t::value * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras } return f; } // **************************************************************************************************** /// SVD version - double computeJacobiansSVD(std::vector >& Fblocks, Matrix& Enull, + double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -645,27 +647,27 @@ public: } // **************************************************************************************************** - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, b); } // **************************************************************************************************** boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector < KeyMatrix2D,Eigen::aligned_allocator > Fblocks; + std::vector Fblocks; Vector b; Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, Z> >(Fblocks, Enull, b); + return boost::make_shared< JacobianFactorSVD<6, ZDim_t::value> >(Fblocks, Enull, b); } private: diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index ee0636142..c4d0867dc 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -104,6 +104,8 @@ protected: /// shorthand for this class typedef SmartProjectionFactor This; + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + public: /// shorthand for a smart pointer to a factor @@ -418,16 +420,16 @@ public: } /// 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); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -435,7 +437,7 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor @@ -444,7 +446,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 9a73b84fe..4d2a69ddf 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -107,6 +107,8 @@ protected: /// shorthand for this class typedef SmartStereoProjectionFactor This; + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + public: /// shorthand for a smart pointer to a factor @@ -480,7 +482,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 0161d4fb6..d94304ed5 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -115,7 +115,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); { // error double expectedError = jf.error(xvalues); @@ -165,7 +165,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { } // Create JacobianFactorQR - JacobianFactorQR<6> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); { const SharedDiagonal model; VectorValues yActual = zero; From 600444eeb7ec11affe61e31ca9b48459e5461665 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 17 Nov 2014 19:53:10 -0500 Subject: [PATCH 53/89] absolute paths to includes --- gtsam/slam/SmartFactorBase.h | 8 ++++---- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam/slam/SmartProjectionPoseFactor.h | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 7c5a1175d..94f287225 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -19,10 +19,10 @@ #pragma once -#include "JacobianFactorQ.h" -#include "JacobianFactorSVD.h" -#include "RegularImplicitSchurFactor.h" -#include "RegularHessianFactor.h" +#include +#include +#include +#include #include #include // for Cheirality exception diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index c4d0867dc..8ade075af 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartFactorBase.h" +#include #include #include diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index f871ab3aa..0fd82414a 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartProjectionFactor.h" +#include namespace gtsam { /** From f3d42a84877719f49b8376bf697238c9cb1e98aa Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 17 Nov 2014 19:53:21 -0500 Subject: [PATCH 54/89] Move to unstable --- {gtsam => gtsam_unstable}/slam/SmartStereoProjectionFactor.h | 2 +- .../slam/SmartStereoProjectionPoseFactor.h | 2 +- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename {gtsam => gtsam_unstable}/slam/SmartStereoProjectionFactor.h (99%) rename {gtsam => gtsam_unstable}/slam/SmartStereoProjectionPoseFactor.h (99%) rename {gtsam => gtsam_unstable}/slam/tests/testSmartStereoProjectionPoseFactor.cpp (99%) diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h similarity index 99% rename from gtsam/slam/SmartStereoProjectionFactor.h rename to gtsam_unstable/slam/SmartStereoProjectionFactor.h index 4d2a69ddf..1c0d1bc37 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartFactorBase.h" +#include #include #include diff --git a/gtsam/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h similarity index 99% rename from gtsam/slam/SmartStereoProjectionPoseFactor.h rename to gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 9448f6498..1f2bd1942 100644 --- a/gtsam/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartStereoProjectionFactor.h" +#include namespace gtsam { /** diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp similarity index 99% rename from gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp rename to gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index e50616163..05260521e 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,7 +18,7 @@ * @date Sept 2013 */ -#include "../SmartStereoProjectionPoseFactor.h" +#include #include #include From 1c6dc8a77dff95f30e0276c745ff9bb58dada8e0 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 17 Nov 2014 20:09:19 -0500 Subject: [PATCH 55/89] uncomment/fix reprojectionError and computeEP methods --- gtsam/slam/SmartFactorBase.h | 84 ++++++++++++++++++------------------ 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 94f287225..599fc8f84 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -186,26 +186,26 @@ public: // **************************************************************************************************** // /// Calculate vector of re-projection errors, before applying noise model -// Vector reprojectionError(const Cameras& cameras, const Point3& point) const { -// -// Vector b = zero(2 * cameras.size()); -// -// size_t i = 0; -// BOOST_FOREACH(const Camera& camera, cameras) { -// const Z& zi = this->measured_.at(i); -// try { -// Z e(camera.project(point) - zi); -// b[2 * i] = e.x(); -// b[2 * i + 1] = e.y(); -// } catch (CheiralityException& e) { -// std::cout << "Cheirality exception " << std::endl; -// exit(EXIT_FAILURE); -// } -// i += 1; -// } -// -// return b; -// } + Vector reprojectionError(const Cameras& cameras, const Point3& point) const { + + Vector b = zero(ZDim_t::value * cameras.size()); + + size_t i = 0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + const Z& zi = this->measured_.at(i); + try { + Z e(camera.project(point) - zi); + b[ZDim_t::value * i] = e.x(); + b[ZDim_t::value * i + 1] = e.y(); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit(EXIT_FAILURE); + } + i += 1; + } + + return b; + } // **************************************************************************************************** /** @@ -238,28 +238,28 @@ public: // **************************************************************************************************** /// Assumes non-degenerate ! -// void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, -// const Point3& point) const { -// -// int numKeys = this->keys_.size(); -// E = zeros(2 * numKeys, 3); -// Vector b = zero(2 * numKeys); -// -// Matrix Ei(2, 3); -// for (size_t i = 0; i < this->measured_.size(); i++) { -// try { -// cameras[i].project(point, boost::none, Ei); -// } catch (CheiralityException& e) { -// std::cout << "Cheirality exception " << std::endl; -// exit(EXIT_FAILURE); -// } -// this->noise_.at(i)->WhitenSystem(Ei, b); -// E.block<2, 3>(2 * i, 0) = Ei; -// } -// -// // Matrix PointCov; -// PointCov.noalias() = (E.transpose() * E).inverse(); -// } + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, + const Point3& point) const { + + int numKeys = this->keys_.size(); + E = zeros(ZDim_t::value * numKeys, 3); + Vector b = zero(2 * numKeys); + + Matrix Ei(ZDim_t::value, 3); + for (size_t i = 0; i < this->measured_.size(); i++) { + try { + cameras[i].project(point, boost::none, Ei); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit(EXIT_FAILURE); + } + this->noise_.at(i)->WhitenSystem(Ei, b); + E.block(ZDim_t::value * i, 0) = Ei; + } + + // Matrix PointCov; + PointCov.noalias() = (E.transpose() * E).inverse(); + } // **************************************************************************************************** /// Compute F, E only (called below in both vanilla and SVD versions) From 6529b793ccd7c11061785db480240ad3b4086038 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 19 Nov 2014 12:51:12 -0500 Subject: [PATCH 56/89] Some fixes for feedback reported in pull request #39 --- gtsam/geometry/StereoPoint2.h | 4 +- gtsam/slam/JacobianFactorQ.h | 2 +- gtsam/slam/SmartFactorBase.h | 106 +++++++++--------- .../SmartStereoProjectionFactorExample.cpp | 2 +- 4 files changed, 59 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 3c92ca46f..694bf525b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -143,12 +143,12 @@ namespace gtsam { } /** convenient function to get a Point2 from the left image */ - inline Point2 point2() const { + Point2 point2() const { return Point2(uL_, v_); } /** convenient function to get a Point2 from the right image */ - inline Point2 const right(){ + Point2 right() const { return Point2(uR_, v_); } diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 9b9383d7b..923edddb7 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -49,7 +49,7 @@ public: typedef std::pair KeyMatrix; std::vector < KeyMatrix > QF; QF.reserve(m); - // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) + // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 599fc8f84..e0eac6b66 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -33,7 +33,6 @@ #include #include #include -#include namespace gtsam { /// Base class with no internal point, completely functional @@ -49,16 +48,16 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + static const int ZDim = traits::dimension::value; ///< Dimension trait of measurement type /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; + typedef Eigen::Matrix Matrix2; /// shorthand for base class type typedef NonlinearFactor Base; @@ -69,6 +68,8 @@ protected: public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor @@ -188,15 +189,15 @@ public: // /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - Vector b = zero(ZDim_t::value * cameras.size()); + Vector b = zero(ZDim * cameras.size()); size_t i = 0; BOOST_FOREACH(const CAMERA& camera, cameras) { const Z& zi = this->measured_.at(i); try { Z e(camera.project(point) - zi); - b[ZDim_t::value * i] = e.x(); - b[ZDim_t::value * i + 1] = e.y(); + b[ZDim * i] = e.x(); + b[ZDim * i + 1] = e.y(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); @@ -242,10 +243,10 @@ public: const Point3& point) const { int numKeys = this->keys_.size(); - E = zeros(ZDim_t::value * numKeys, 3); + E = zeros(ZDim * numKeys, 3); Vector b = zero(2 * numKeys); - Matrix Ei(ZDim_t::value, 3); + Matrix Ei(ZDim, 3); for (size_t i = 0; i < this->measured_.size(); i++) { try { cameras[i].project(point, boost::none, Ei); @@ -254,7 +255,7 @@ public: exit(EXIT_FAILURE); } this->noise_.at(i)->WhitenSystem(Ei, b); - E.block(ZDim_t::value * i, 0) = Ei; + E.block(ZDim * i, 0) = Ei; } // Matrix PointCov; @@ -268,11 +269,11 @@ public: Vector& b, const Cameras& cameras, const Point3& point) const { size_t numKeys = this->keys_.size(); - E = zeros(Z::Dim() * numKeys, 3); - b = zero(Z::Dim() * numKeys); + E = zeros(ZDim * numKeys, 3); + b = zero(ZDim * numKeys); double f = 0; - Matrix Fi(Z::Dim(), 6), Ei(Z::Dim(), 3), Hcali(Z::Dim(), D - 6), Hcam(Z::Dim(), D); + Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); for (size_t i = 0; i < this->measured_.size(); i++) { Vector bi; @@ -294,12 +295,12 @@ public: if (D == 6) { // optimize only camera pose Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); } else { - Hcam.block(0, 0) = Fi; // Z::Dim() x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras + Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras + Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); } - E.block(ZDim_t::value * i, 0) = Ei; - subInsert(b, bi, Z::Dim() * i); + E.block(ZDim * i, 0) = Ei; + subInsert(b, bi, ZDim * i); } return f; } @@ -340,10 +341,10 @@ public: std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); - F = zeros(Z::Dim() * numKeys, D * numKeys); + F = zeros(This::ZDim * numKeys, D * numKeys); for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(ZDim_t::value * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras + F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras } return f; } @@ -362,9 +363,9 @@ public: // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(Z::Dim() * numKeys, Z::Dim() * numKeys - 3); + // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, Z::Dim() * numKeys, Z::Dim() * numKeys - 3); // last Z::Dim()m-3 columns + Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns return f; } @@ -378,11 +379,11 @@ public: int numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(Z::Dim() * numKeys, D * numKeys); + F.resize(ZDim * numKeys, D * numKeys); F.setZero(); for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras + F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras return f; } @@ -443,9 +444,9 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(Z::Dim() * numKeys, D * numKeys); + Matrix F = zeros(ZDim * numKeys, D * numKeys); for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras + F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras Matrix H(D * numKeys, D * numKeys); Vector gs_vector; @@ -483,16 +484,16 @@ public: for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(Z::Dim() * i1, 0) * P; + const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment(Z::Dim() * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) + augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(Z::Dim() * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -500,7 +501,7 @@ public: // (DxD) = (Dx2) * ( (2x2) * (2xD) ) augmentedHessian(i1, i2) = -Fi1.transpose() - * (Ei1_P * E.block(Z::Dim() * i2, 0).transpose() * Fi2); + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras } @@ -527,16 +528,16 @@ public: // X X X X 14 const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(Z::Dim() * i1, 0) * P; + const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; { // for i1 = i2 // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment(Z::Dim() * i1) // F' * b - -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) + gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b + -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(Z::Dim() * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian @@ -545,7 +546,7 @@ public: // (DxD) = (Dx2) * ( (2x2) * (2xD) ) Gs.at(GsIndex) = -Fi1.transpose() - * (Ei1_P * E.block(Z::Dim() * i2, 0).transpose() * Fi2); + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras @@ -593,9 +594,9 @@ public: for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor const Matrix2D& Fi1 = Fblocks.at(i1).second; - const Matrix23 Ei1_P = E.block(Z::Dim() * i1, 0) * P; + const Matrix23 Ei1_P = E.block(ZDim * i1, 0) * P; - // D = (DxZ::Dim()) * (Z::Dim()) + // D = (DxZDim) * (ZDim) // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) // we should map those to a slot in the local (grouped) hessian (0,1,2,3,4) // Key cameraKey_i1 = this->keys_[i1]; @@ -605,15 +606,15 @@ public: // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() - + Fi1.transpose() * b.segment(Z::Dim() * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZ::Dim()) * (Z::Dim()x3) * (3*Z::Dim()m) * (Z::Dim()m x 1) + + Fi1.transpose() * b.segment(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xD) - (Z::Dim()x3) * (3xZ::Dim()) * (Z::Dim()xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); // add contribution of current factor augmentedHessian(aug_i1, aug_i1) = matrixBlock + - ( Fi1.transpose() * (Fi1 - Ei1_P * E.block(Z::Dim() * i1, 0).transpose() * Fi1) ); + ( Fi1.transpose() * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1) ); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -622,12 +623,12 @@ public: //Key cameraKey_i2 = this->keys_[i2]; DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - // (DxD) = (DxZ::Dim()) * ( (Z::Dim()xZ::Dim()) * (Z::Dim()xD) ) + // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // add contribution of current factor augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() * (Ei1_P * E.block(Z::Dim() * i2, 0).transpose() * Fi2); + - Fi1.transpose() * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras @@ -647,7 +648,7 @@ public: } // **************************************************************************************************** - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { std::vector Fblocks; @@ -656,7 +657,7 @@ public: Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, b); } // **************************************************************************************************** @@ -665,9 +666,9 @@ public: size_t numKeys = this->keys_.size(); std::vector Fblocks; Vector b; - Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); + Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, ZDim_t::value> >(Fblocks, Enull, b); + return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); } private: @@ -682,4 +683,7 @@ private: } }; +template +const int SmartFactorBase::ZDim; + } // \ namespace gtsam diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 841092ec9..f5e59b1b2 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include From dd255eb24c20d07aa3c58f75f1167f450a6215d7 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 19 Nov 2014 13:07:14 -0500 Subject: [PATCH 57/89] Remove landmark template parameter --- examples/SFMExample_SmartFactor.cpp | 3 +-- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 19 +++++++++++-------- gtsam/slam/SmartProjectionPoseFactor.h | 8 ++++---- .../tests/testSmartProjectionPoseFactor.cpp | 6 +++--- .../examples/SmartProjectionFactorExample.cpp | 2 +- 6 files changed, 21 insertions(+), 19 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b3c5ee5fe..eed389df9 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -61,8 +61,7 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef gtsam::SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e0eac6b66..8ff554104 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -48,7 +48,7 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits::dimension::value; ///< Dimension trait of measurement type + static const int ZDim = traits::dimension::value; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 8ade075af..75e4699d9 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -61,7 +61,7 @@ enum LinearizationMode { * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? */ -template +template class SmartProjectionFactor: public SmartFactorBase, D> { protected: @@ -102,9 +102,9 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; + typedef SmartProjectionFactor This; - typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + static const int ZDim = traits::dimension::value; ///< Measurement dimension public: @@ -420,16 +420,16 @@ public: } /// 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); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -437,7 +437,7 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor @@ -446,7 +446,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate @@ -709,4 +709,7 @@ private: } }; +template +const int SmartProjectionFactor::ZDim; + } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0fd82414a..3b2e2bcbc 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -48,10 +48,10 @@ protected: public: /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 4e4fde3e4..c2855f09b 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -60,8 +60,8 @@ 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 SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; void projectToMultipleCameras( SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, @@ -1202,7 +1202,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); + SmartProjectionPoseFactor factor1(rankTol, linThreshold); boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); factor1.add(measurement1, poseKey1, model, Kbundler); } diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 01c2ab3e1..c8a4e7123 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; From b19ed67545109307c3e0c4db769b6e249ce88ab7 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 21 Nov 2014 15:23:01 -0500 Subject: [PATCH 58/89] Work in progress on correcting bug with key casting to int32. Causes overflow on cast, causing bad array indexing in metis --- examples/StereoVOExample_large.cpp | 4 ++- gtsam/inference/MetisIndex-inl.h | 4 +-- gtsam/inference/MetisIndex.h | 8 +++--- gtsam/inference/Ordering.cpp | 43 +++++++++++++++++++++++++----- 4 files changed, 46 insertions(+), 13 deletions(-) diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index b9ab663d9..4d39a191a 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -103,7 +103,9 @@ int main(int argc, char** argv){ cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + LevenbergMarquardtParams params; + params.orderingType = OrderingType::METIS; + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl; diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index d9fb4cfd1..006ba075f 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -27,8 +27,8 @@ namespace gtsam { template void MetisIndex::augment(const FactorGraph& factors) { - std::map > adjMap; - std::map >::iterator adjMapIt; + std::map > adjMap; + std::map >::iterator adjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index b058b5564..476d34980 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -40,8 +40,8 @@ public: typedef boost::shared_ptr shared_ptr; private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector size_t nFactors_; // Number of factors in the original factor graph size_t nKeys_; // size_t minKey_; @@ -69,8 +69,8 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } size_t nValues() const { return nKeys_; } size_t minKey() const { return minKey_; } diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 654740163..e54ad3fde 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -204,12 +204,43 @@ namespace gtsam { { gttic(Ordering_METIS); - vector xadj = met.xadj(); - vector adj = met.adj(); - size_t minKey = met.minKey(); + vector xadj = met.xadj(); + vector adj = met.adj(); + Key minKey = met.minKey(); + + // TODO(Andrew): Debug + Key min = std::numeric_limits::max(); + for (int i = 0; i < adj.size(); i++) + { + if (adj[i] < min) + min = adj[i]; + } + + std::cout << "Min: " << min << " minkey: " << minKey << std::endl; // Normalize, subtract the smallest key - std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus(), minKey)); + //std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus(), minKey)); + for (vector::iterator it = adj.begin(); it != adj.end(); ++it) + *it = *it - minKey; + + // Cast the adjacency formats into idx_t (int32) + // NOTE: Keys can store quite large values and hence may overflow during conversion to int + // It's important that the normalization is performed first. + vector adj_idx; + for (vector::iterator it = adj.begin(); it != adj.end(); ++it) + adj_idx.push_back(static_cast(*it)); + + vector xadj_idx; + for (vector::iterator it = xadj.begin(); it != xadj.end(); ++it) + xadj_idx.push_back(static_cast(*it)); + + // TODO(Andrew): Debug + for (int i = 0; i < adj.size(); i++) + { + assert(adj[i] >= 0); + if (adj[i] < 0) + std::cout << adj[i] << std::endl; + } vector perm, iperm; @@ -222,7 +253,7 @@ namespace gtsam { int outputError; - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); + outputError = METIS_NodeND(&size, &xadj_idx[0], &adj_idx[0], NULL, NULL, &perm[0], &iperm[0]); Ordering result; if (outputError != METIS_OK) @@ -234,7 +265,7 @@ namespace gtsam { result.resize(size); for (size_t j = 0; j < size; ++j){ // We have to add the minKey value back to obtain the original key in the Values - result[j] = perm[j] + minKey; + result[j] = (Key)perm[j] + minKey; } return result; } From aebe40d19ff5669fd2f731188d7304c53722ba94 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Fri, 21 Nov 2014 20:50:30 -0500 Subject: [PATCH 59/89] Add the 4th input argument for calling PinholeCamera::project which does not take default arguments anymore. --- 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 8ff554104..ed60b1a4b 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -249,7 +249,7 @@ public: Matrix Ei(ZDim, 3); for (size_t i = 0; i < this->measured_.size(); i++) { try { - cameras[i].project(point, boost::none, Ei); + cameras[i].project(point, boost::none, Ei, boost::none); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); From 6cc7b17687d956dae9497d5e4c3591cdcd8279be Mon Sep 17 00:00:00 2001 From: HannesSommer Date: Sat, 22 Nov 2014 15:09:10 +0100 Subject: [PATCH 60/89] proposal to fix alignment in BAD (issue #154) --- gtsam_unstable/nonlinear/Expression-inl.h | 83 +++++++++++++------ gtsam_unstable/nonlinear/Expression.h | 8 +- .../nonlinear/tests/testExpressionFactor.cpp | 8 +- 3 files changed, 65 insertions(+), 34 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index a98ab349f..73d475a24 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -27,6 +27,7 @@ #include #include #include +#include // template meta-programming headers #include @@ -39,6 +40,26 @@ class ExpressionFactorBinaryTest; namespace gtsam { +const unsigned TraceAlignment = 16; + +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){ + // right now only word sized types are supported. + // Easy to extend if needed, + // by somehow inferring the unsigned integer of same size + BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + size_t & uiValue = reinterpret_cast(value); + size_t misAlignment = uiValue % requiredAlignment; + if(misAlignment) { + uiValue += requiredAlignment - misAlignment; + } + return value; +} +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment){ + return upAlign(value, requiredAlignment); +} + template class Expression; @@ -175,6 +196,11 @@ public: typedef ExecutionTrace type; }; +/// Storage type for the execution trace. +/// It enforces the proper alignment in a portable way. +/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -221,7 +247,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const = 0; + ExecutionTraceStorage* traceStorage) const = 0; }; //----------------------------------------------------------------------------- @@ -248,7 +274,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { return constant_; } }; @@ -292,7 +318,8 @@ public: /// Construct an execution trace for reverse AD virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, char* raw) const { + ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); } @@ -337,7 +364,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return values.at(key_); } @@ -432,7 +459,8 @@ struct FunctionalBase: ExpressionNode { } }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, char*& raw) const { + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { // base case: does not do anything } }; @@ -512,17 +540,18 @@ struct GenerateFunctionalNode: Argument, Base { }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, char*& raw) const { - Base::trace(values, record, raw); // recurse + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { + Base::trace(values, record, traceStorage); // recurse // Write an Expression execution trace in record->trace - // Iff Constant or Leaf, this will not write to raw, only to trace. - // Iff the expression is functional, write all Records in raw buffer + // Iff Constant or Leaf, this will not write to traceStorage, only to trace. + // Iff the expression is functional, write all Records in traceStorage buffer // Return value of type T is recorded in record->value record->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, raw); - // raw is never modified by traceExecution, but if traceExecution has + record->Record::This::trace, traceStorage); + // traceStorage is never modified by traceExecution, but if traceExecution has // written in the buffer, the next caller expects we advance the pointer - raw += This::expression->traceSize(); + traceStorage += This::expression->traceSize(); } }; @@ -587,15 +616,17 @@ struct FunctionalNode { }; /// Construct an execution trace for reverse AD - Record* trace(const Values& values, char* raw) const { + Record* trace(const Values& values, + ExecutionTraceStorage* traceStorage) const { + assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); // Create the record and advance the pointer - Record* record = new (raw) Record(); - raw = (char*) (record + 1); + Record* record = new (traceStorage) Record(); + traceStorage += upAligned(sizeof(Record)); // Record the traces for all arguments - // After this, the raw pointer is set to after what was written - Base::trace(values, record, raw); + // After this, the traceStorage pointer is set to after what was written + Base::trace(values, record, traceStorage); // Return the record for this function evaluation return record; @@ -622,7 +653,7 @@ private: UnaryExpression(Function f, const Expression& e1) : function_(f) { this->template reset(e1.root()); - ExpressionNode::traceSize_ = sizeof(Record) + e1.traceSize(); + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } friend class Expression ; @@ -636,9 +667,9 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { - Record* record = Base::trace(values, raw); + Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_(record->template value(), @@ -671,7 +702,7 @@ private: this->template reset(e1.root()); this->template reset(e2.root()); ExpressionNode::traceSize_ = // - sizeof(Record) + e1.traceSize() + e2.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } friend class Expression ; @@ -689,9 +720,9 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { - Record* record = Base::trace(values, raw); + Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_(record->template value(), @@ -727,7 +758,7 @@ private: this->template reset(e2.root()); this->template reset(e3.root()); ExpressionNode::traceSize_ = // - sizeof(Record) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize(); } friend class Expression ; @@ -745,9 +776,9 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { - Record* record = Base::trace(values, raw); + Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_( diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index e8bd8bbe7..5e1d425ed 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -118,8 +118,8 @@ public: /// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below! T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - return root_->traceExecution(values, trace, raw); + ExecutionTraceStorage* traceStorage) const { + return root_->traceExecution(values, trace, traceStorage); } /// Return value and derivatives, reverse AD version @@ -130,9 +130,9 @@ public: // with an execution trace, made up entirely of "Record" structs, see // the FunctionalNode class in expression-inl.h size_t size = traceSize(); - char raw[size]; + ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; - T value(traceExecution(values, trace, raw)); + T value(traceExecution(values, trace, traceStorage)); trace.startReverseAD(jacobians); return value; } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c63bfeb6f..e4448fbf7 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -162,9 +162,9 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla - char raw[size]; + ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; - Point2 value = binary.traceExecution(values, trace, raw); + Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -218,9 +218,9 @@ TEST(ExpressionFactor, Shallow) { size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - char raw[size]; + ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; - Point2 value = expression.traceExecution(values, trace, raw); + Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); From 7aa07a9e16a99625ca1954317837c779c97a2585 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 02:06:40 -0500 Subject: [PATCH 61/89] Reworked ordering to work with symbols, large keys, and non-zero indexed keys. No longer subtract min key to send to metis, but now creates bimap between keys and int values used in metis --- gtsam/inference/MetisIndex-inl.h | 46 +++++++++++------- gtsam/inference/MetisIndex.h | 27 +++++++---- gtsam/inference/Ordering.cpp | 64 ++++++-------------------- gtsam/inference/tests/testOrdering.cpp | 8 ++-- 4 files changed, 64 insertions(+), 81 deletions(-) diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 006ba075f..b9272990a 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -27,8 +27,8 @@ namespace gtsam { template void MetisIndex::augment(const FactorGraph& factors) { - std::map > adjMap; - std::map >::iterator adjMapIt; + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ @@ -37,31 +37,43 @@ void MetisIndex::augment(const FactorGraph& factors) // starting at index xadj[i] and ending at(but not including) // index xadj[i + 1](i.e., adjncy[xadj[i]] through // and including adjncy[xadj[i + 1] - 1]). + idx_t keyCounter = 0; + + // First: Record a copy of each key inside the factorgraph and create a + // key to integer mapping. This is referenced during the adjaceny step + for (size_t i = 0; i < factors.size(); i++){ + if (factors[i]) + BOOST_FOREACH(const Key& key, *factors[i]){ + keySet.insert(keySet.end(), key); // Keep a track of all unique keys + if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()){ + intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); + keyCounter++; + } + } + } + + // Create an adjacency mapping that stores the set of all adjacent keys for every key for (size_t i = 0; i < factors.size(); i++){ if (factors[i]){ - BOOST_FOREACH(const Key& k1, *factors[i]){ - BOOST_FOREACH(const Key& k2, *factors[i]){ - if (k1 != k2) - adjMap[k1].insert(adjMap[k1].end(), k2); // Insert at the end - } - keySet.insert(keySet.end(), k1); // Keep a track of all unique keySet - } + BOOST_FOREACH(const Key& k1, *factors[i]) + BOOST_FOREACH(const Key& k2, *factors[i]) + if (k1 != k2){ + // Store both in Key and idx_t format + int i = intKeyBMap_.left.at(k1); + int j = intKeyBMap_.left.at(k2); + iAdjMap[i].insert(iAdjMap[i].end(), j); + } } } // Number of keys referenced in this factor graph nKeys_ = keySet.size(); - - - // Starting with a nonzero key crashes METIS - // Find the smallest key in the graph - minKey_ = *keySet.begin(); // set is ordered xadj_.push_back(0);// Always set the first index to zero - for (adjMapIt = adjMap.begin(); adjMapIt != adjMap.end(); ++adjMapIt) { - std::vector temp; + for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { + std::vector temp; // Copy from the FastSet into a temporary vector - std::copy(adjMapIt->second.begin(), adjMapIt->second.end(), std::back_inserter(temp)); + std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), std::back_inserter(temp)); // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 476d34980..e3b7ea33d 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,12 +25,14 @@ #include #include #include +#include +#include namespace gtsam { /** * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built - * fromt a factor graph prior to elimination, and stores the list of factors + * from a factor graph prior to elimination, and stores the list of factors * that involve each variable. * \nosubgrouping */ @@ -38,13 +40,15 @@ class GTSAM_EXPORT MetisIndex { public: typedef boost::shared_ptr shared_ptr; + typedef boost::bimap bm_type; private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - size_t nFactors_; // Number of factors in the original factor graph - size_t nKeys_; // - size_t minKey_; + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; + boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship + size_t nFactors_; // Number of factors in the original factor graph + size_t nKeys_; public: /// @name Standard Constructors @@ -69,10 +73,13 @@ public: template void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } - size_t minKey() const { return minKey_; } + std::vector xadj() const { return xadj_; } + std::vector adj() const { return adj_; } + size_t nValues() const { return nKeys_; } + Key intToKey(idx_t value) const { + assert(value >= 0); + return intKeyBMap_.right.find(value)->second; + } /// @} }; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index dcc2bd00a..a069fd4f6 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -204,56 +204,20 @@ namespace gtsam { { gttic(Ordering_METIS); - vector xadj = met.xadj(); - vector adj = met.adj(); - Key minKey = met.minKey(); - - // TODO(Andrew): Debug - Key min = std::numeric_limits::max(); - for (int i = 0; i < adj.size(); i++) - { - if (adj[i] < min) - min = adj[i]; - } - - std::cout << "Min: " << min << " minkey: " << minKey << std::endl; - - // Normalize, subtract the smallest key - //std::transform(adj.begin(), adj.end(), adj.begin(), std::bind2nd(std::minus(), minKey)); - for (vector::iterator it = adj.begin(); it != adj.end(); ++it) - *it = *it - minKey; - - // Cast the adjacency formats into idx_t (int32) - // NOTE: Keys can store quite large values and hence may overflow during conversion to int - // It's important that the normalization is performed first. - vector adj_idx; - for (vector::iterator it = adj.begin(); it != adj.end(); ++it) - adj_idx.push_back(static_cast(*it)); - - vector xadj_idx; - for (vector::iterator it = xadj.begin(); it != xadj.end(); ++it) - xadj_idx.push_back(static_cast(*it)); - - // TODO(Andrew): Debug - for (int i = 0; i < adj.size(); i++) - { - assert(adj[i] >= 0); - if (adj[i] < 0) - std::cout << adj[i] << std::endl; - } - + vector xadj = met.xadj(); + vector adj = met.adj(); vector perm, iperm; - idx_t size = met.nValues(); - for (idx_t i = 0; i < size; i++) - { - perm.push_back(0); - iperm.push_back(0); - } + idx_t size = met.nValues(); + for (idx_t i = 0; i < size; i++) + { + perm.push_back(0); + iperm.push_back(0); + } int outputError; - outputError = METIS_NodeND(&size, &xadj_idx[0], &adj_idx[0], NULL, NULL, &perm[0], &iperm[0]); + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); Ordering result; if (outputError != METIS_OK) @@ -262,11 +226,11 @@ namespace gtsam { return result; } - result.resize(size); - for (size_t j = 0; j < size; ++j){ - // We have to add the minKey value back to obtain the original key in the Values - result[j] = (Key)perm[j] + minKey; - } + result.resize(size); + for (size_t j = 0; j < size; ++j){ + // We have to add the minKey value back to obtain the original key in the Values + result[j] = met.intToKey(perm[j]); + } return result; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 57de00646..5cd4c77b2 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -159,18 +159,18 @@ TEST(Ordering, csr_format_3) { vector xadjExpected, adjExpected; xadjExpected += 0, 1, 4, 6, 8, 10; adjExpected += 1, 0, 2, 4, 1, 3, 2, 4, 1, 3; - size_t minKey = mi.minKey(); + //size_t minKey = mi.minKey(); vector adjAcutal = mi.adj(); // Normalize, subtract the smallest key - std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), - std::bind2nd(std::minus(), minKey)); + //std::transform(adjAcutal.begin(), adjAcutal.end(), adjAcutal.begin(), + // std::bind2nd(std::minus(), minKey)); EXPECT(xadjExpected == mi.xadj()); EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected == adjAcutal); - + } /* ************************************************************************* */ From 6fd24118be3791aa0f8257c5ad143110661d2fb4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 24 Nov 2014 12:37:17 -0500 Subject: [PATCH 62/89] Fix gcc warnings --- gtsam/inference/MetisIndex-inl.h | 3 ++- gtsam/inference/Ordering.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index b9272990a..a8e9aef2f 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -42,7 +42,7 @@ void MetisIndex::augment(const FactorGraph& factors) // First: Record a copy of each key inside the factorgraph and create a // key to integer mapping. This is referenced during the adjaceny step for (size_t i = 0; i < factors.size(); i++){ - if (factors[i]) + if (factors[i]) { BOOST_FOREACH(const Key& key, *factors[i]){ keySet.insert(keySet.end(), key); // Keep a track of all unique keys if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()){ @@ -50,6 +50,7 @@ void MetisIndex::augment(const FactorGraph& factors) keyCounter++; } } + } } // Create an adjacency mapping that stores the set of all adjacent keys for every key diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index a069fd4f6..81f9ddb5c 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -227,7 +227,7 @@ namespace gtsam { } result.resize(size); - for (size_t j = 0; j < size; ++j){ + for (size_t j = 0; j < (size_t)size; ++j){ // We have to add the minKey value back to obtain the original key in the Values result[j] = met.intToKey(perm[j]); } From 8678754dbd2e7dded3212d3f1fc421e9968d93be Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 12:41:24 -0500 Subject: [PATCH 63/89] Ordering test for symbols --- gtsam/inference/tests/testOrdering.cpp | 37 ++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 5cd4c77b2..013f569e0 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -173,6 +173,43 @@ TEST(Ordering, csr_format_3) { } +/* ************************************************************************* */ +TEST(Ordering, csr_format_4) { + SymbolicFactorGraph sfg; + + sfg.push_factor(Symbol('x', 1)); + sfg.push_factor(Symbol('x', 1), Symbol('x', 2)); + sfg.push_factor(Symbol('x', 2), Symbol('x', 3)); + sfg.push_factor(Symbol('x', 3), Symbol('x', 4)); + sfg.push_factor(Symbol('x', 4), Symbol('x', 5)); + sfg.push_factor(Symbol('x', 5), Symbol('x', 6)); + + MetisIndex mi(sfg); + + vector xadjExpected, adjExpected; + xadjExpected += 0, 1, 3, 5, 7, 9, 10; + adjExpected += 1, 0, 2, 1, 3, 2, 4, 3, 5, 4; + + vector adjAcutal = mi.adj(); + vector xadjActual = mi.xadj(); + + EXPECT(xadjExpected == mi.xadj()); + EXPECT(adjExpected.size() == mi.adj().size()); + EXPECT(adjExpected == adjAcutal); + + Ordering metOrder = Ordering::metis(sfg); + + // Test different symbol types + sfg.push_factor(Symbol('l', 1)); + sfg.push_factor(Symbol('x', 1), Symbol('l', 1)); + sfg.push_factor(Symbol('x', 2), Symbol('l', 1)); + sfg.push_factor(Symbol('x', 3), Symbol('l', 1)); + sfg.push_factor(Symbol('x', 4), Symbol('l', 1)); + + Ordering metOrder2 = Ordering::metis(sfg); + +} + /* ************************************************************************* */ TEST(Ordering, metis) { From e8e502137b9ab4c0cff10bf6c6bbfa4cb4bb95e3 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 12:50:55 -0500 Subject: [PATCH 64/89] Update ordering --- examples/StereoVOExample_large.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 4d39a191a..e357745be 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv){ cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph LevenbergMarquardtParams params; - params.orderingType = OrderingType::METIS; + params.orderingType = Ordering::METIS; LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); From b76185cf5e2ad8136b3b47369bdfcfe74a50086e Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 13:02:27 -0500 Subject: [PATCH 65/89] Include metis.h correction --- gtsam/inference/MetisIndex.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index e3b7ea33d..684755895 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include namespace gtsam { From ce93030b0005df1506bef578bf2c183925c032ca Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 13:31:47 -0500 Subject: [PATCH 66/89] Correcting warnings on windows --- CMakeLists.txt | 2 ++ gtsam/3rdparty/metis-5.1.0/include/metis.h | 14 +++++++++++++- gtsam/CMakeLists.txt | 4 ++-- gtsam/inference/MetisIndex-inl.h | 2 +- 4 files changed, 18 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 951a0ec7e..b2e7aa3d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,6 +95,8 @@ if(MSVC) add_definitions(-DBOOST_ALL_NO_LIB) add_definitions(-DBOOST_ALL_DYN_LINK) endif() + # Also disable certain warnings + add_definitions(/wd4503) endif() find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) diff --git a/gtsam/3rdparty/metis-5.1.0/include/metis.h b/gtsam/3rdparty/metis-5.1.0/include/metis.h index dc5406ae5..2866a946b 100644 --- a/gtsam/3rdparty/metis-5.1.0/include/metis.h +++ b/gtsam/3rdparty/metis-5.1.0/include/metis.h @@ -65,17 +65,29 @@ #ifndef _GKLIB_H_ #ifdef COMPILER_MSC #include - typedef __int32 int32_t; typedef __int64 int64_t; #define PRId32 "I32d" #define PRId64 "I64d" #define SCNd32 "ld" #define SCNd64 "I64d" + +#ifndef INT32_MIN #define INT32_MIN ((int32_t)_I32_MIN) +#endif + +#ifndef INT32_MAX #define INT32_MAX _I32_MAX +#endif + +#ifndef INT64_MIN #define INT64_MIN ((int64_t)_I64_MIN) +#endif + +#ifndef INT64_MAX #define INT64_MAX _I64_MAX +#endif + #else #include #endif diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 2d5706f33..d08ded7e8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,7 +5,7 @@ set (gtsam_subdirs base geometry inference - symbolic + symbolic discrete linear nonlinear @@ -69,7 +69,7 @@ set(gtsam_srcs ${base_srcs} ${geometry_srcs} ${inference_srcs} - ${symbolic_srcs} + ${symbolic_srcs} ${discrete_srcs} ${linear_srcs} ${nonlinear_srcs} diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index a8e9aef2f..0be1c503c 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -78,7 +78,7 @@ void MetisIndex::augment(const FactorGraph& factors) // Insert each index's set in order by appending them to the end of adj_ adj_.insert(adj_.end(), temp.begin(), temp.end()); //adj_.push_back(temp); - xadj_.push_back(adj_.size()); + xadj_.push_back((idx_t)adj_.size()); } } From 96e649b1a49aabf4ca66cabafdfab4d9fc90fd24 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 13:33:36 -0500 Subject: [PATCH 67/89] Formatting --- gtsam/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d08ded7e8..ada7ae12d 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,7 +5,7 @@ set (gtsam_subdirs base geometry inference - symbolic + symbolic discrete linear nonlinear @@ -69,7 +69,7 @@ set(gtsam_srcs ${base_srcs} ${geometry_srcs} ${inference_srcs} - ${symbolic_srcs} + ${symbolic_srcs} ${discrete_srcs} ${linear_srcs} ${nonlinear_srcs} From c5cee669b130dd57f2626c900020b3cbaa4b995c Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 13:41:08 -0500 Subject: [PATCH 68/89] Formatting --- gtsam/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index ada7ae12d..b58a5b4b8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -10,7 +10,7 @@ set (gtsam_subdirs linear nonlinear slam - navigation + navigation ) set(gtsam_srcs) @@ -74,8 +74,8 @@ set(gtsam_srcs ${linear_srcs} ${nonlinear_srcs} ${slam_srcs} - ${navigation_srcs} - ${gtsam_core_headers} + ${navigation_srcs} + ${gtsam_core_headers} ) # Generate and install config and dllexport files From 9d3aa9c61820c37b8adba4dd8e6a9ba40bff24b5 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 13:43:29 -0500 Subject: [PATCH 69/89] Remove version info from metis folder name --- .../{metis-5.1.0 => metis}/BUILD-Windows.txt | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/BUILD.txt | 0 .../3rdparty/{metis-5.1.0 => metis}/CMakeLists.txt | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/Changelog | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/BUILD.txt | 0 .../{metis-5.1.0 => metis}/GKlib/CMakeLists.txt | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/GKlib.h | 0 .../{metis-5.1.0 => metis}/GKlib/GKlibSystem.cmake | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/Makefile | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/b64.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/blas.c | 0 .../GKlib/conf/check_thread_storage.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/csr.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/error.c | 0 .../{metis-5.1.0 => metis}/GKlib/evaluate.c | 0 .../{metis-5.1.0 => metis}/GKlib/fkvkselect.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/fs.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/getopt.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/gk_arch.h | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/gk_defs.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_externs.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_getopt.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_macros.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mkblas.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mkmemory.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mkpqueue.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mkpqueue2.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mkrandom.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mksort.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_mkutils.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_proto.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_struct.h | 0 .../{metis-5.1.0 => metis}/GKlib/gk_types.h | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/gkregex.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/gkregex.h | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/graph.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/htable.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/io.c | 0 .../{metis-5.1.0 => metis}/GKlib/itemsets.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/mcore.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/memory.c | 0 .../{metis-5.1.0 => metis}/GKlib/ms_inttypes.h | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/ms_stat.h | 0 .../{metis-5.1.0 => metis}/GKlib/ms_stdint.h | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/omp.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/pdb.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/pqueue.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/random.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/rw.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/seq.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/sort.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/string.c | 0 .../GKlib/test/CMakeLists.txt | 0 .../GKlib/test/Makefile.in.old | 0 .../{metis-5.1.0 => metis}/GKlib/test/Makefile.old | 0 .../{metis-5.1.0 => metis}/GKlib/test/fis.c | 0 .../{metis-5.1.0 => metis}/GKlib/test/gkgraph.c | 0 .../{metis-5.1.0 => metis}/GKlib/test/gksort.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/test/rw.c | 0 .../{metis-5.1.0 => metis}/GKlib/test/strings.c | 0 .../3rdparty/{metis-5.1.0 => metis}/GKlib/timers.c | 0 .../{metis-5.1.0 => metis}/GKlib/tokenizer.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/util.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/Install.txt | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/LICENSE.txt | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/Makefile | 0 .../{metis-5.1.0 => metis}/graphs/4elt.graph | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/README | 0 .../{metis-5.1.0 => metis}/graphs/copter2.graph | 0 .../{metis-5.1.0 => metis}/graphs/mdual.graph | 0 .../{metis-5.1.0 => metis}/graphs/metis.mesh | 0 .../{metis-5.1.0 => metis}/graphs/test.mgraph | 0 .../{metis-5.1.0 => metis}/include/CMakeLists.txt | 0 .../3rdparty/{metis-5.1.0 => metis}/include/metis.h | 0 .../{metis-5.1.0 => metis}/libmetis/CMakeLists.txt | 0 .../{metis-5.1.0 => metis}/libmetis/auxapi.c | 0 .../{metis-5.1.0 => metis}/libmetis/balance.c | 0 .../{metis-5.1.0 => metis}/libmetis/bucketsort.c | 0 .../{metis-5.1.0 => metis}/libmetis/checkgraph.c | 0 .../{metis-5.1.0 => metis}/libmetis/coarsen.c | 0 .../{metis-5.1.0 => metis}/libmetis/compress.c | 0 .../{metis-5.1.0 => metis}/libmetis/contig.c | 0 .../{metis-5.1.0 => metis}/libmetis/debug.c | 0 .../3rdparty/{metis-5.1.0 => metis}/libmetis/defs.h | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/fm.c | 0 .../{metis-5.1.0 => metis}/libmetis/fortran.c | 0 .../{metis-5.1.0 => metis}/libmetis/frename.c | 0 .../{metis-5.1.0 => metis}/libmetis/gklib.c | 0 .../{metis-5.1.0 => metis}/libmetis/gklib_defs.h | 0 .../{metis-5.1.0 => metis}/libmetis/gklib_rename.h | 0 .../{metis-5.1.0 => metis}/libmetis/graph.c | 0 .../{metis-5.1.0 => metis}/libmetis/initpart.c | 0 .../{metis-5.1.0 => metis}/libmetis/kmetis.c | 0 .../{metis-5.1.0 => metis}/libmetis/kwayfm.c | 0 .../{metis-5.1.0 => metis}/libmetis/kwayrefine.c | 0 .../{metis-5.1.0 => metis}/libmetis/macros.h | 0 .../{metis-5.1.0 => metis}/libmetis/mcutil.c | 0 .../3rdparty/{metis-5.1.0 => metis}/libmetis/mesh.c | 0 .../{metis-5.1.0 => metis}/libmetis/meshpart.c | 0 .../{metis-5.1.0 => metis}/libmetis/metislib.h | 0 .../{metis-5.1.0 => metis}/libmetis/minconn.c | 0 .../{metis-5.1.0 => metis}/libmetis/mincover.c | 0 .../3rdparty/{metis-5.1.0 => metis}/libmetis/mmd.c | 0 .../{metis-5.1.0 => metis}/libmetis/ometis.c | 0 .../{metis-5.1.0 => metis}/libmetis/options.c | 0 .../{metis-5.1.0 => metis}/libmetis/parmetis.c | 0 .../{metis-5.1.0 => metis}/libmetis/pmetis.c | 0 .../{metis-5.1.0 => metis}/libmetis/proto.h | 0 .../{metis-5.1.0 => metis}/libmetis/refine.c | 0 .../{metis-5.1.0 => metis}/libmetis/rename.h | 0 .../{metis-5.1.0 => metis}/libmetis/separator.c | 0 .../3rdparty/{metis-5.1.0 => metis}/libmetis/sfm.c | 0 .../{metis-5.1.0 => metis}/libmetis/srefine.c | 0 .../3rdparty/{metis-5.1.0 => metis}/libmetis/stat.c | 0 .../{metis-5.1.0 => metis}/libmetis/stdheaders.h | 0 .../{metis-5.1.0 => metis}/libmetis/struct.h | 0 .../{metis-5.1.0 => metis}/libmetis/timing.c | 0 .../3rdparty/{metis-5.1.0 => metis}/libmetis/util.c | 0 .../{metis-5.1.0 => metis}/libmetis/wspace.c | 0 .../{metis-5.1.0 => metis}/manual/manual.pdf | Bin .../{metis-5.1.0 => metis}/programs/CMakeLists.txt | 0 .../programs/cmdline_gpmetis.c | 0 .../programs/cmdline_m2gmetis.c | 0 .../programs/cmdline_mpmetis.c | 0 .../programs/cmdline_ndmetis.c | 0 .../{metis-5.1.0 => metis}/programs/cmpfillin.c | 0 .../3rdparty/{metis-5.1.0 => metis}/programs/defs.h | 0 .../{metis-5.1.0 => metis}/programs/gpmetis.c | 0 .../{metis-5.1.0 => metis}/programs/graphchk.c | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/programs/io.c | 0 .../{metis-5.1.0 => metis}/programs/m2gmetis.c | 0 .../{metis-5.1.0 => metis}/programs/metisbin.h | 0 .../{metis-5.1.0 => metis}/programs/mpmetis.c | 0 .../{metis-5.1.0 => metis}/programs/ndmetis.c | 0 .../{metis-5.1.0 => metis}/programs/proto.h | 0 .../{metis-5.1.0 => metis}/programs/smbfactor.c | 0 .../3rdparty/{metis-5.1.0 => metis}/programs/stat.c | 0 .../{metis-5.1.0 => metis}/programs/struct.h | 0 gtsam/3rdparty/{metis-5.1.0 => metis}/vsgen.bat | 0 139 files changed, 0 insertions(+), 0 deletions(-) rename gtsam/3rdparty/{metis-5.1.0 => metis}/BUILD-Windows.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/BUILD.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/CMakeLists.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/Changelog (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/BUILD.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/CMakeLists.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/GKlib.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/GKlibSystem.cmake (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/Makefile (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/b64.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/blas.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/conf/check_thread_storage.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/csr.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/error.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/evaluate.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/fkvkselect.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/fs.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/getopt.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_arch.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_defs.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_externs.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_getopt.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_macros.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mkblas.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mkmemory.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mkpqueue.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mkpqueue2.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mkrandom.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mksort.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_mkutils.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_proto.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_struct.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gk_types.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gkregex.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/gkregex.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/graph.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/htable.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/io.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/itemsets.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/mcore.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/memory.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/ms_inttypes.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/ms_stat.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/ms_stdint.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/omp.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/pdb.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/pqueue.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/random.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/rw.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/seq.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/sort.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/string.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/CMakeLists.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/Makefile.in.old (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/Makefile.old (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/fis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/gkgraph.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/gksort.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/rw.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/test/strings.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/timers.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/tokenizer.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/GKlib/util.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/Install.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/LICENSE.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/Makefile (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/4elt.graph (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/README (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/copter2.graph (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/mdual.graph (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/metis.mesh (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/graphs/test.mgraph (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/include/CMakeLists.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/include/metis.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/CMakeLists.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/auxapi.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/balance.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/bucketsort.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/checkgraph.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/coarsen.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/compress.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/contig.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/debug.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/defs.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/fm.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/fortran.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/frename.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/gklib.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/gklib_defs.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/gklib_rename.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/graph.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/initpart.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/kmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/kwayfm.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/kwayrefine.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/macros.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/mcutil.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/mesh.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/meshpart.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/metislib.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/minconn.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/mincover.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/mmd.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/ometis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/options.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/parmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/pmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/proto.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/refine.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/rename.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/separator.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/sfm.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/srefine.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/stat.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/stdheaders.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/struct.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/timing.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/util.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/libmetis/wspace.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/manual/manual.pdf (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/CMakeLists.txt (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/cmdline_gpmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/cmdline_m2gmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/cmdline_mpmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/cmdline_ndmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/cmpfillin.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/defs.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/gpmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/graphchk.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/io.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/m2gmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/metisbin.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/mpmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/ndmetis.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/proto.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/smbfactor.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/stat.c (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/programs/struct.h (100%) rename gtsam/3rdparty/{metis-5.1.0 => metis}/vsgen.bat (100%) diff --git a/gtsam/3rdparty/metis-5.1.0/BUILD-Windows.txt b/gtsam/3rdparty/metis/BUILD-Windows.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/BUILD-Windows.txt rename to gtsam/3rdparty/metis/BUILD-Windows.txt diff --git a/gtsam/3rdparty/metis-5.1.0/BUILD.txt b/gtsam/3rdparty/metis/BUILD.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/BUILD.txt rename to gtsam/3rdparty/metis/BUILD.txt diff --git a/gtsam/3rdparty/metis-5.1.0/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/CMakeLists.txt rename to gtsam/3rdparty/metis/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/Changelog b/gtsam/3rdparty/metis/Changelog similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/Changelog rename to gtsam/3rdparty/metis/Changelog diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/BUILD.txt b/gtsam/3rdparty/metis/GKlib/BUILD.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/BUILD.txt rename to gtsam/3rdparty/metis/GKlib/BUILD.txt diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/CMakeLists.txt b/gtsam/3rdparty/metis/GKlib/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/CMakeLists.txt rename to gtsam/3rdparty/metis/GKlib/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/GKlib.h b/gtsam/3rdparty/metis/GKlib/GKlib.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/GKlib.h rename to gtsam/3rdparty/metis/GKlib/GKlib.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/GKlibSystem.cmake b/gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/GKlibSystem.cmake rename to gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/Makefile b/gtsam/3rdparty/metis/GKlib/Makefile similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/Makefile rename to gtsam/3rdparty/metis/GKlib/Makefile diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/b64.c b/gtsam/3rdparty/metis/GKlib/b64.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/b64.c rename to gtsam/3rdparty/metis/GKlib/b64.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/blas.c b/gtsam/3rdparty/metis/GKlib/blas.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/blas.c rename to gtsam/3rdparty/metis/GKlib/blas.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/conf/check_thread_storage.c b/gtsam/3rdparty/metis/GKlib/conf/check_thread_storage.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/conf/check_thread_storage.c rename to gtsam/3rdparty/metis/GKlib/conf/check_thread_storage.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/csr.c b/gtsam/3rdparty/metis/GKlib/csr.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/csr.c rename to gtsam/3rdparty/metis/GKlib/csr.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/error.c b/gtsam/3rdparty/metis/GKlib/error.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/error.c rename to gtsam/3rdparty/metis/GKlib/error.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/evaluate.c b/gtsam/3rdparty/metis/GKlib/evaluate.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/evaluate.c rename to gtsam/3rdparty/metis/GKlib/evaluate.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/fkvkselect.c b/gtsam/3rdparty/metis/GKlib/fkvkselect.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/fkvkselect.c rename to gtsam/3rdparty/metis/GKlib/fkvkselect.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/fs.c b/gtsam/3rdparty/metis/GKlib/fs.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/fs.c rename to gtsam/3rdparty/metis/GKlib/fs.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/getopt.c b/gtsam/3rdparty/metis/GKlib/getopt.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/getopt.c rename to gtsam/3rdparty/metis/GKlib/getopt.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h b/gtsam/3rdparty/metis/GKlib/gk_arch.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h rename to gtsam/3rdparty/metis/GKlib/gk_arch.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_defs.h b/gtsam/3rdparty/metis/GKlib/gk_defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_defs.h rename to gtsam/3rdparty/metis/GKlib/gk_defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_externs.h b/gtsam/3rdparty/metis/GKlib/gk_externs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_externs.h rename to gtsam/3rdparty/metis/GKlib/gk_externs.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_getopt.h b/gtsam/3rdparty/metis/GKlib/gk_getopt.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_getopt.h rename to gtsam/3rdparty/metis/GKlib/gk_getopt.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_macros.h b/gtsam/3rdparty/metis/GKlib/gk_macros.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_macros.h rename to gtsam/3rdparty/metis/GKlib/gk_macros.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkblas.h b/gtsam/3rdparty/metis/GKlib/gk_mkblas.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkblas.h rename to gtsam/3rdparty/metis/GKlib/gk_mkblas.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkmemory.h b/gtsam/3rdparty/metis/GKlib/gk_mkmemory.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkmemory.h rename to gtsam/3rdparty/metis/GKlib/gk_mkmemory.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue.h b/gtsam/3rdparty/metis/GKlib/gk_mkpqueue.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue.h rename to gtsam/3rdparty/metis/GKlib/gk_mkpqueue.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue2.h b/gtsam/3rdparty/metis/GKlib/gk_mkpqueue2.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue2.h rename to gtsam/3rdparty/metis/GKlib/gk_mkpqueue2.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkrandom.h b/gtsam/3rdparty/metis/GKlib/gk_mkrandom.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkrandom.h rename to gtsam/3rdparty/metis/GKlib/gk_mkrandom.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mksort.h b/gtsam/3rdparty/metis/GKlib/gk_mksort.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mksort.h rename to gtsam/3rdparty/metis/GKlib/gk_mksort.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkutils.h b/gtsam/3rdparty/metis/GKlib/gk_mkutils.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkutils.h rename to gtsam/3rdparty/metis/GKlib/gk_mkutils.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_proto.h b/gtsam/3rdparty/metis/GKlib/gk_proto.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_proto.h rename to gtsam/3rdparty/metis/GKlib/gk_proto.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_struct.h b/gtsam/3rdparty/metis/GKlib/gk_struct.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_struct.h rename to gtsam/3rdparty/metis/GKlib/gk_struct.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gk_types.h b/gtsam/3rdparty/metis/GKlib/gk_types.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gk_types.h rename to gtsam/3rdparty/metis/GKlib/gk_types.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.c b/gtsam/3rdparty/metis/GKlib/gkregex.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.c rename to gtsam/3rdparty/metis/GKlib/gkregex.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.h b/gtsam/3rdparty/metis/GKlib/gkregex.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.h rename to gtsam/3rdparty/metis/GKlib/gkregex.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/graph.c b/gtsam/3rdparty/metis/GKlib/graph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/graph.c rename to gtsam/3rdparty/metis/GKlib/graph.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/htable.c b/gtsam/3rdparty/metis/GKlib/htable.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/htable.c rename to gtsam/3rdparty/metis/GKlib/htable.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/io.c b/gtsam/3rdparty/metis/GKlib/io.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/io.c rename to gtsam/3rdparty/metis/GKlib/io.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/itemsets.c b/gtsam/3rdparty/metis/GKlib/itemsets.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/itemsets.c rename to gtsam/3rdparty/metis/GKlib/itemsets.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/mcore.c b/gtsam/3rdparty/metis/GKlib/mcore.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/mcore.c rename to gtsam/3rdparty/metis/GKlib/mcore.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/memory.c b/gtsam/3rdparty/metis/GKlib/memory.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/memory.c rename to gtsam/3rdparty/metis/GKlib/memory.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_inttypes.h b/gtsam/3rdparty/metis/GKlib/ms_inttypes.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/ms_inttypes.h rename to gtsam/3rdparty/metis/GKlib/ms_inttypes.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stat.h b/gtsam/3rdparty/metis/GKlib/ms_stat.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/ms_stat.h rename to gtsam/3rdparty/metis/GKlib/ms_stat.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h b/gtsam/3rdparty/metis/GKlib/ms_stdint.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h rename to gtsam/3rdparty/metis/GKlib/ms_stdint.h diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/omp.c b/gtsam/3rdparty/metis/GKlib/omp.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/omp.c rename to gtsam/3rdparty/metis/GKlib/omp.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/pdb.c b/gtsam/3rdparty/metis/GKlib/pdb.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/pdb.c rename to gtsam/3rdparty/metis/GKlib/pdb.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/pqueue.c b/gtsam/3rdparty/metis/GKlib/pqueue.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/pqueue.c rename to gtsam/3rdparty/metis/GKlib/pqueue.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/random.c b/gtsam/3rdparty/metis/GKlib/random.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/random.c rename to gtsam/3rdparty/metis/GKlib/random.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/rw.c b/gtsam/3rdparty/metis/GKlib/rw.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/rw.c rename to gtsam/3rdparty/metis/GKlib/rw.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/seq.c b/gtsam/3rdparty/metis/GKlib/seq.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/seq.c rename to gtsam/3rdparty/metis/GKlib/seq.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/sort.c b/gtsam/3rdparty/metis/GKlib/sort.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/sort.c rename to gtsam/3rdparty/metis/GKlib/sort.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/string.c b/gtsam/3rdparty/metis/GKlib/string.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/string.c rename to gtsam/3rdparty/metis/GKlib/string.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/CMakeLists.txt b/gtsam/3rdparty/metis/GKlib/test/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/CMakeLists.txt rename to gtsam/3rdparty/metis/GKlib/test/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.in.old b/gtsam/3rdparty/metis/GKlib/test/Makefile.in.old similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.in.old rename to gtsam/3rdparty/metis/GKlib/test/Makefile.in.old diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.old b/gtsam/3rdparty/metis/GKlib/test/Makefile.old similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/Makefile.old rename to gtsam/3rdparty/metis/GKlib/test/Makefile.old diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/fis.c b/gtsam/3rdparty/metis/GKlib/test/fis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/fis.c rename to gtsam/3rdparty/metis/GKlib/test/fis.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/gkgraph.c b/gtsam/3rdparty/metis/GKlib/test/gkgraph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/gkgraph.c rename to gtsam/3rdparty/metis/GKlib/test/gkgraph.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/gksort.c b/gtsam/3rdparty/metis/GKlib/test/gksort.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/gksort.c rename to gtsam/3rdparty/metis/GKlib/test/gksort.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/rw.c b/gtsam/3rdparty/metis/GKlib/test/rw.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/rw.c rename to gtsam/3rdparty/metis/GKlib/test/rw.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/test/strings.c b/gtsam/3rdparty/metis/GKlib/test/strings.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/test/strings.c rename to gtsam/3rdparty/metis/GKlib/test/strings.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/timers.c b/gtsam/3rdparty/metis/GKlib/timers.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/timers.c rename to gtsam/3rdparty/metis/GKlib/timers.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/tokenizer.c b/gtsam/3rdparty/metis/GKlib/tokenizer.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/tokenizer.c rename to gtsam/3rdparty/metis/GKlib/tokenizer.c diff --git a/gtsam/3rdparty/metis-5.1.0/GKlib/util.c b/gtsam/3rdparty/metis/GKlib/util.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/GKlib/util.c rename to gtsam/3rdparty/metis/GKlib/util.c diff --git a/gtsam/3rdparty/metis-5.1.0/Install.txt b/gtsam/3rdparty/metis/Install.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/Install.txt rename to gtsam/3rdparty/metis/Install.txt diff --git a/gtsam/3rdparty/metis-5.1.0/LICENSE.txt b/gtsam/3rdparty/metis/LICENSE.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/LICENSE.txt rename to gtsam/3rdparty/metis/LICENSE.txt diff --git a/gtsam/3rdparty/metis-5.1.0/Makefile b/gtsam/3rdparty/metis/Makefile similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/Makefile rename to gtsam/3rdparty/metis/Makefile diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/4elt.graph b/gtsam/3rdparty/metis/graphs/4elt.graph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/4elt.graph rename to gtsam/3rdparty/metis/graphs/4elt.graph diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/README b/gtsam/3rdparty/metis/graphs/README similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/README rename to gtsam/3rdparty/metis/graphs/README diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/copter2.graph b/gtsam/3rdparty/metis/graphs/copter2.graph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/copter2.graph rename to gtsam/3rdparty/metis/graphs/copter2.graph diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/mdual.graph b/gtsam/3rdparty/metis/graphs/mdual.graph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/mdual.graph rename to gtsam/3rdparty/metis/graphs/mdual.graph diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/metis.mesh b/gtsam/3rdparty/metis/graphs/metis.mesh similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/metis.mesh rename to gtsam/3rdparty/metis/graphs/metis.mesh diff --git a/gtsam/3rdparty/metis-5.1.0/graphs/test.mgraph b/gtsam/3rdparty/metis/graphs/test.mgraph similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/graphs/test.mgraph rename to gtsam/3rdparty/metis/graphs/test.mgraph diff --git a/gtsam/3rdparty/metis-5.1.0/include/CMakeLists.txt b/gtsam/3rdparty/metis/include/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/include/CMakeLists.txt rename to gtsam/3rdparty/metis/include/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/include/metis.h b/gtsam/3rdparty/metis/include/metis.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/include/metis.h rename to gtsam/3rdparty/metis/include/metis.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt rename to gtsam/3rdparty/metis/libmetis/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/auxapi.c b/gtsam/3rdparty/metis/libmetis/auxapi.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/auxapi.c rename to gtsam/3rdparty/metis/libmetis/auxapi.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/balance.c b/gtsam/3rdparty/metis/libmetis/balance.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/balance.c rename to gtsam/3rdparty/metis/libmetis/balance.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/bucketsort.c b/gtsam/3rdparty/metis/libmetis/bucketsort.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/bucketsort.c rename to gtsam/3rdparty/metis/libmetis/bucketsort.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/checkgraph.c b/gtsam/3rdparty/metis/libmetis/checkgraph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/checkgraph.c rename to gtsam/3rdparty/metis/libmetis/checkgraph.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/coarsen.c b/gtsam/3rdparty/metis/libmetis/coarsen.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/coarsen.c rename to gtsam/3rdparty/metis/libmetis/coarsen.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/compress.c b/gtsam/3rdparty/metis/libmetis/compress.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/compress.c rename to gtsam/3rdparty/metis/libmetis/compress.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/contig.c b/gtsam/3rdparty/metis/libmetis/contig.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/contig.c rename to gtsam/3rdparty/metis/libmetis/contig.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/debug.c b/gtsam/3rdparty/metis/libmetis/debug.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/debug.c rename to gtsam/3rdparty/metis/libmetis/debug.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/defs.h b/gtsam/3rdparty/metis/libmetis/defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/defs.h rename to gtsam/3rdparty/metis/libmetis/defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/fm.c b/gtsam/3rdparty/metis/libmetis/fm.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/fm.c rename to gtsam/3rdparty/metis/libmetis/fm.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/fortran.c b/gtsam/3rdparty/metis/libmetis/fortran.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/fortran.c rename to gtsam/3rdparty/metis/libmetis/fortran.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/frename.c b/gtsam/3rdparty/metis/libmetis/frename.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/frename.c rename to gtsam/3rdparty/metis/libmetis/frename.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/gklib.c b/gtsam/3rdparty/metis/libmetis/gklib.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/gklib.c rename to gtsam/3rdparty/metis/libmetis/gklib.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/gklib_defs.h b/gtsam/3rdparty/metis/libmetis/gklib_defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/gklib_defs.h rename to gtsam/3rdparty/metis/libmetis/gklib_defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/gklib_rename.h b/gtsam/3rdparty/metis/libmetis/gklib_rename.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/gklib_rename.h rename to gtsam/3rdparty/metis/libmetis/gklib_rename.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/graph.c b/gtsam/3rdparty/metis/libmetis/graph.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/graph.c rename to gtsam/3rdparty/metis/libmetis/graph.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/initpart.c b/gtsam/3rdparty/metis/libmetis/initpart.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/initpart.c rename to gtsam/3rdparty/metis/libmetis/initpart.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/kmetis.c b/gtsam/3rdparty/metis/libmetis/kmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/kmetis.c rename to gtsam/3rdparty/metis/libmetis/kmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/kwayfm.c b/gtsam/3rdparty/metis/libmetis/kwayfm.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/kwayfm.c rename to gtsam/3rdparty/metis/libmetis/kwayfm.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/kwayrefine.c b/gtsam/3rdparty/metis/libmetis/kwayrefine.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/kwayrefine.c rename to gtsam/3rdparty/metis/libmetis/kwayrefine.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/macros.h b/gtsam/3rdparty/metis/libmetis/macros.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/macros.h rename to gtsam/3rdparty/metis/libmetis/macros.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mcutil.c b/gtsam/3rdparty/metis/libmetis/mcutil.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mcutil.c rename to gtsam/3rdparty/metis/libmetis/mcutil.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mesh.c b/gtsam/3rdparty/metis/libmetis/mesh.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mesh.c rename to gtsam/3rdparty/metis/libmetis/mesh.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/meshpart.c b/gtsam/3rdparty/metis/libmetis/meshpart.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/meshpart.c rename to gtsam/3rdparty/metis/libmetis/meshpart.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/metislib.h b/gtsam/3rdparty/metis/libmetis/metislib.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/metislib.h rename to gtsam/3rdparty/metis/libmetis/metislib.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/minconn.c b/gtsam/3rdparty/metis/libmetis/minconn.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/minconn.c rename to gtsam/3rdparty/metis/libmetis/minconn.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mincover.c b/gtsam/3rdparty/metis/libmetis/mincover.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mincover.c rename to gtsam/3rdparty/metis/libmetis/mincover.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/mmd.c b/gtsam/3rdparty/metis/libmetis/mmd.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/mmd.c rename to gtsam/3rdparty/metis/libmetis/mmd.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/ometis.c b/gtsam/3rdparty/metis/libmetis/ometis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/ometis.c rename to gtsam/3rdparty/metis/libmetis/ometis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/options.c b/gtsam/3rdparty/metis/libmetis/options.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/options.c rename to gtsam/3rdparty/metis/libmetis/options.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/parmetis.c b/gtsam/3rdparty/metis/libmetis/parmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/parmetis.c rename to gtsam/3rdparty/metis/libmetis/parmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/pmetis.c b/gtsam/3rdparty/metis/libmetis/pmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/pmetis.c rename to gtsam/3rdparty/metis/libmetis/pmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/proto.h b/gtsam/3rdparty/metis/libmetis/proto.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/proto.h rename to gtsam/3rdparty/metis/libmetis/proto.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/refine.c b/gtsam/3rdparty/metis/libmetis/refine.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/refine.c rename to gtsam/3rdparty/metis/libmetis/refine.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/rename.h b/gtsam/3rdparty/metis/libmetis/rename.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/rename.h rename to gtsam/3rdparty/metis/libmetis/rename.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/separator.c b/gtsam/3rdparty/metis/libmetis/separator.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/separator.c rename to gtsam/3rdparty/metis/libmetis/separator.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/sfm.c b/gtsam/3rdparty/metis/libmetis/sfm.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/sfm.c rename to gtsam/3rdparty/metis/libmetis/sfm.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/srefine.c b/gtsam/3rdparty/metis/libmetis/srefine.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/srefine.c rename to gtsam/3rdparty/metis/libmetis/srefine.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/stat.c b/gtsam/3rdparty/metis/libmetis/stat.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/stat.c rename to gtsam/3rdparty/metis/libmetis/stat.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/stdheaders.h b/gtsam/3rdparty/metis/libmetis/stdheaders.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/stdheaders.h rename to gtsam/3rdparty/metis/libmetis/stdheaders.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/struct.h b/gtsam/3rdparty/metis/libmetis/struct.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/struct.h rename to gtsam/3rdparty/metis/libmetis/struct.h diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/timing.c b/gtsam/3rdparty/metis/libmetis/timing.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/timing.c rename to gtsam/3rdparty/metis/libmetis/timing.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/util.c b/gtsam/3rdparty/metis/libmetis/util.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/util.c rename to gtsam/3rdparty/metis/libmetis/util.c diff --git a/gtsam/3rdparty/metis-5.1.0/libmetis/wspace.c b/gtsam/3rdparty/metis/libmetis/wspace.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/libmetis/wspace.c rename to gtsam/3rdparty/metis/libmetis/wspace.c diff --git a/gtsam/3rdparty/metis-5.1.0/manual/manual.pdf b/gtsam/3rdparty/metis/manual/manual.pdf similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/manual/manual.pdf rename to gtsam/3rdparty/metis/manual/manual.pdf diff --git a/gtsam/3rdparty/metis-5.1.0/programs/CMakeLists.txt b/gtsam/3rdparty/metis/programs/CMakeLists.txt similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/CMakeLists.txt rename to gtsam/3rdparty/metis/programs/CMakeLists.txt diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_gpmetis.c b/gtsam/3rdparty/metis/programs/cmdline_gpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_gpmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_gpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_m2gmetis.c b/gtsam/3rdparty/metis/programs/cmdline_m2gmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_m2gmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_m2gmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_mpmetis.c b/gtsam/3rdparty/metis/programs/cmdline_mpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_mpmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_mpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmdline_ndmetis.c b/gtsam/3rdparty/metis/programs/cmdline_ndmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmdline_ndmetis.c rename to gtsam/3rdparty/metis/programs/cmdline_ndmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/cmpfillin.c b/gtsam/3rdparty/metis/programs/cmpfillin.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/cmpfillin.c rename to gtsam/3rdparty/metis/programs/cmpfillin.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/defs.h b/gtsam/3rdparty/metis/programs/defs.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/defs.h rename to gtsam/3rdparty/metis/programs/defs.h diff --git a/gtsam/3rdparty/metis-5.1.0/programs/gpmetis.c b/gtsam/3rdparty/metis/programs/gpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/gpmetis.c rename to gtsam/3rdparty/metis/programs/gpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/graphchk.c b/gtsam/3rdparty/metis/programs/graphchk.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/graphchk.c rename to gtsam/3rdparty/metis/programs/graphchk.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/io.c b/gtsam/3rdparty/metis/programs/io.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/io.c rename to gtsam/3rdparty/metis/programs/io.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/m2gmetis.c b/gtsam/3rdparty/metis/programs/m2gmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/m2gmetis.c rename to gtsam/3rdparty/metis/programs/m2gmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/metisbin.h b/gtsam/3rdparty/metis/programs/metisbin.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/metisbin.h rename to gtsam/3rdparty/metis/programs/metisbin.h diff --git a/gtsam/3rdparty/metis-5.1.0/programs/mpmetis.c b/gtsam/3rdparty/metis/programs/mpmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/mpmetis.c rename to gtsam/3rdparty/metis/programs/mpmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/ndmetis.c b/gtsam/3rdparty/metis/programs/ndmetis.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/ndmetis.c rename to gtsam/3rdparty/metis/programs/ndmetis.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/proto.h b/gtsam/3rdparty/metis/programs/proto.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/proto.h rename to gtsam/3rdparty/metis/programs/proto.h diff --git a/gtsam/3rdparty/metis-5.1.0/programs/smbfactor.c b/gtsam/3rdparty/metis/programs/smbfactor.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/smbfactor.c rename to gtsam/3rdparty/metis/programs/smbfactor.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/stat.c b/gtsam/3rdparty/metis/programs/stat.c similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/stat.c rename to gtsam/3rdparty/metis/programs/stat.c diff --git a/gtsam/3rdparty/metis-5.1.0/programs/struct.h b/gtsam/3rdparty/metis/programs/struct.h similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/programs/struct.h rename to gtsam/3rdparty/metis/programs/struct.h diff --git a/gtsam/3rdparty/metis-5.1.0/vsgen.bat b/gtsam/3rdparty/metis/vsgen.bat similarity index 100% rename from gtsam/3rdparty/metis-5.1.0/vsgen.bat rename to gtsam/3rdparty/metis/vsgen.bat From 867acbef6cd508bfb58ce4fbb4e7bb6b57b72eb0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 24 Nov 2014 20:14:56 +0100 Subject: [PATCH 70/89] Fixed building wrapper. --- gtsam.h | 8 ++++---- gtsam_unstable/dynamics/FullIMUFactor.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 9a44c7aa2..0ceda6db5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2438,8 +2438,8 @@ virtual class ImuFactor : gtsam::NonlinearFactor { #include class AHRSFactorPreintegratedMeasurements { // Standard Constructor - AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); // Testable @@ -2468,8 +2468,8 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - const gtsam::imuBias::ConstantBias& bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, const gtsam::imuBias::ConstantBias& bias, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis) const; }; diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1c35fc9b8..1c2807e7a 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -75,7 +75,7 @@ public: // access const Vector3& gyro() const { return gyro_; } const Vector3& accel() const { return accel_; } - Vector6 z() const { return (Vector6() << accel_, gyro_); } + Vector6 z() const { return (Vector(6) << accel_, gyro_).finished(); } /** * Error evaluation with optional derivatives - calculates From b43248d9ab9006cd4c6a17a0a2033af58f097596 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 24 Nov 2014 15:44:12 -0500 Subject: [PATCH 71/89] fix ChartTesting header include issue in testRot3 --- gtsam/base/{chartTesting.h => ChartTesting.h} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename gtsam/base/{chartTesting.h => ChartTesting.h} (99%) diff --git a/gtsam/base/chartTesting.h b/gtsam/base/ChartTesting.h similarity index 99% rename from gtsam/base/chartTesting.h rename to gtsam/base/ChartTesting.h index d2f453521..f74982bce 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/ChartTesting.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file chartTesting.h + * @file ChartTesting.h * @brief * @date November, 2014 * @author Paul Furgale From 6c13834260f174507f0efe3cbadcbd1d330b76db Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 24 Nov 2014 15:51:07 -0500 Subject: [PATCH 72/89] change back to lower case, and fix another similar header issue --- gtsam/base/{ChartTesting.h => chartTesting.h} | 2 +- gtsam/geometry/tests/testRot3.cpp | 2 +- gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename gtsam/base/{ChartTesting.h => chartTesting.h} (99%) diff --git a/gtsam/base/ChartTesting.h b/gtsam/base/chartTesting.h similarity index 99% rename from gtsam/base/ChartTesting.h rename to gtsam/base/chartTesting.h index f74982bce..d2f453521 100644 --- a/gtsam/base/ChartTesting.h +++ b/gtsam/base/chartTesting.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file ChartTesting.h + * @file chartTesting.h * @brief * @date November, 2014 * @author Paul Furgale diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 0f066399d..1a4992052 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 4343e7085..b9d9896d3 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include #include From 2bc381dbb44c28a7820ffa2828f08645aa93648e Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Mon, 24 Nov 2014 17:12:41 -0500 Subject: [PATCH 73/89] Rename corrections --- CMakeLists.txt | 6 +++--- gtsam/3rdparty/CMakeLists.txt | 2 +- gtsam/inference/MetisIndex.h | 2 +- gtsam/inference/Ordering.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b2e7aa3d9..62bf4eb71 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -266,9 +266,9 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR}) include_directories(BEFORE gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include - gtsam/3rdparty/metis-5.1.0/include - gtsam/3rdparty/metis-5.1.0/libmetis - gtsam/3rdparty/metis-5.1.0/GKlib + gtsam/3rdparty/metis/include + gtsam/3rdparty/metis/libmetis + gtsam/3rdparty/metis/GKlib ${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR} # So we can include generated config header files CppUnitLite) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 38c084e25..64636d262 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -45,7 +45,7 @@ endif() option(GTSAM_BUILD_METIS "Build metis library" ON) option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) if(GTSAM_BUILD_METIS) - add_subdirectory(metis-5.1.0) + add_subdirectory(metis) endif(GTSAM_BUILD_METIS) ############ NOTE: When updating GeographicLib be sure to disable building their examples ############ and unit tests by commenting out their lines: diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 684755895..ff67a5e21 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include namespace gtsam { diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 81f9ddb5c..5ae25d543 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include using namespace std; From 00d1d8cead166d522399e3b1e3cedd47c589c8d9 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 24 Nov 2014 17:53:52 -0500 Subject: [PATCH 74/89] Remove option to build metis. Now required. --- gtsam/3rdparty/CMakeLists.txt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 64636d262..49d5a2fbb 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -42,11 +42,9 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) FILES_MATCHING PATTERN "*.h") endif() -option(GTSAM_BUILD_METIS "Build metis library" ON) option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_BUILD_METIS) - add_subdirectory(metis) -endif(GTSAM_BUILD_METIS) +add_subdirectory(metis) + ############ NOTE: When updating GeographicLib be sure to disable building their examples ############ and unit tests by commenting out their lines: # add_subdirectory (examples) From 3fed3c7cbb99bd783f5939560b3759f1ff3150a0 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 24 Nov 2014 17:54:50 -0500 Subject: [PATCH 75/89] match installed header location to source location --- gtsam/3rdparty/metis/metis.h | 13 +++++++++++++ gtsam/inference/MetisIndex.h | 2 +- 2 files changed, 14 insertions(+), 1 deletion(-) create mode 100644 gtsam/3rdparty/metis/metis.h diff --git a/gtsam/3rdparty/metis/metis.h b/gtsam/3rdparty/metis/metis.h new file mode 100644 index 000000000..a1ba08684 --- /dev/null +++ b/gtsam/3rdparty/metis/metis.h @@ -0,0 +1,13 @@ +/* + * metis.h + * Dummy header, not installed! + * Created on: Nov 24, 2014 + * Author: cbeall3 + */ + +#ifndef GTSAM_3RDPARTY_METIS_METIS_H_ +#define GTSAM_3RDPARTY_METIS_METIS_H_ + +#include + +#endif /* GTSAM_3RDPARTY_METIS_METIS_H_ */ diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index ff67a5e21..3eef4739c 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include namespace gtsam { From ebdcbc6d9ec48154375e1215a23145e09fa588bf Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 24 Nov 2014 18:15:56 -0500 Subject: [PATCH 76/89] Fix comma initializer --- examples/METISOrderingExample.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 452ba523e..eb6bdd49d 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -36,11 +36,11 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, priorMean, priorNoise)); Pose2 odometry(2.0, 0.0, 0.0); - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); graph.print("\nFactor Graph:\n"); // print From 97d60884679c139146f61b666e1f365326d33adf Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 24 Nov 2014 21:18:14 -0500 Subject: [PATCH 77/89] fix warning in issue 162 --- gtsam/geometry/Cal3Unified.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d65093847..f65b27780 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -63,6 +63,8 @@ public: double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} + virtual ~Cal3Unified() {} + /// @} /// @name Advanced Constructors /// @{ From 05c98ccafb54b152e3570f89541aff400127bd3d Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Tue, 25 Nov 2014 00:20:23 -0500 Subject: [PATCH 78/89] Add typedef CAMERA and restore a commented function of 'add' as a templated function. --- gtsam/slam/SmartFactorBase.h | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ed60b1a4b..d9e7b9819 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -76,6 +76,7 @@ public: typedef boost::shared_ptr shared_ptr; /// shorthand for a pinhole camera + typedef CAMERA Camera; typedef std::vector Cameras; /** @@ -134,13 +135,14 @@ public: * The noise is assumed to be the same for all measurements */ // **************************************************************************************************** -// void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) { -// 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); -// this->noise_.push_back(noise); -// } -// } + template + void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { + 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); + this->noise_.push_back(noise); + } + } /** return the measurements */ const std::vector& measured() const { From ce033f5594b6453cd5e527bedab9d2f9188823ea Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 09:57:31 +0100 Subject: [PATCH 79/89] Fixed many warnings with Clang, reformatted using BORG template. --- gtsam/inference/MetisIndex-inl.h | 90 +++++++++++----------- gtsam/inference/MetisIndex.h | 123 ++++++++++++++++++------------- 2 files changed, 115 insertions(+), 98 deletions(-) diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 0be1c503c..50bed2e3b 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -1,85 +1,85 @@ /* ---------------------------------------------------------------------------- -* 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) + * 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 + * See LICENSE for the license information -* -------------------------------------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** -* @file MetisIndex-inl.h -* @author Andrew Melim -* @date Oct. 10, 2014 -*/ + * @file MetisIndex-inl.h + * @author Andrew Melim + * @date Oct. 10, 2014 + */ #pragma once +#include #include #include namespace gtsam { - /* ************************************************************************* */ template -void MetisIndex::augment(const FactorGraph& factors) -{ +void MetisIndex::augment(const FactorGraph& factors) { std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first std::map >::iterator iAdjMapIt; - std::set keySet; - - /* ********** Convert to CSR format ********** */ - // Assuming that vertex numbering starts from 0 (C style), - // then the adjacency list of vertex i is stored in array adjncy - // starting at index xadj[i] and ending at(but not including) - // index xadj[i + 1](i.e., adjncy[xadj[i]] through - // and including adjncy[xadj[i + 1] - 1]). + std::set keySet; + + /* ********** Convert to CSR format ********** */ + // Assuming that vertex numbering starts from 0 (C style), + // then the adjacency list of vertex i is stored in array adjncy + // starting at index xadj[i] and ending at(but not including) + // index xadj[i + 1](i.e., adjncy[xadj[i]] through + // and including adjncy[xadj[i + 1] - 1]). idx_t keyCounter = 0; // First: Record a copy of each key inside the factorgraph and create a // key to integer mapping. This is referenced during the adjaceny step - for (size_t i = 0; i < factors.size(); i++){ + for (size_t i = 0; i < factors.size(); i++) { if (factors[i]) { - BOOST_FOREACH(const Key& key, *factors[i]){ + BOOST_FOREACH(const Key& key, *factors[i]) { keySet.insert(keySet.end(), key); // Keep a track of all unique keys - if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()){ + if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); keyCounter++; } } } } - + // Create an adjacency mapping that stores the set of all adjacent keys for every key - for (size_t i = 0; i < factors.size(); i++){ - if (factors[i]){ - BOOST_FOREACH(const Key& k1, *factors[i]) - BOOST_FOREACH(const Key& k2, *factors[i]) - if (k1 != k2){ + for (size_t i = 0; i < factors.size(); i++) { + if (factors[i]) { + BOOST_FOREACH(const Key& k1, *factors[i]) + BOOST_FOREACH(const Key& k2, *factors[i]) + if (k1 != k2) { // Store both in Key and idx_t format int i = intKeyBMap_.left.at(k1); int j = intKeyBMap_.left.at(k2); iAdjMap[i].insert(iAdjMap[i].end(), j); } - } - } + } + } - // Number of keys referenced in this factor graph - nKeys_ = keySet.size(); - - xadj_.push_back(0);// Always set the first index to zero + // Number of keys referenced in this factor graph + nKeys_ = keySet.size(); + + xadj_.push_back(0); // Always set the first index to zero for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { - std::vector temp; - // Copy from the FastSet into a temporary vector - std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), std::back_inserter(temp)); - // Insert each index's set in order by appending them to the end of adj_ - adj_.insert(adj_.end(), temp.begin(), temp.end()); - //adj_.push_back(temp); - xadj_.push_back((idx_t)adj_.size()); - } + std::vector temp; + // Copy from the FastSet into a temporary vector + std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), + std::back_inserter(temp)); + // Insert each index's set in order by appending them to the end of adj_ + adj_.insert(adj_.end(), temp.begin(), temp.end()); + //adj_.push_back(temp); + xadj_.push_back((idx_t) adj_.size()); + } } -} +} // \ gtsam diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 3eef4739c..51624e29e 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -1,89 +1,106 @@ /* ---------------------------------------------------------------------------- -* 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) + * 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 - -* -------------------------------------------------------------------------- */ + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ /** -* @file MetisIndex.h -* @author Andrew Melim -* @date Oct. 10, 2014 -*/ + * @file MetisIndex.h + * @author Andrew Melim + * @date Oct. 10, 2014 + */ #pragma once -#include -#include -#include -#include -#include #include #include +#include +#include +#include #include + +// Boost bimap generates many ugly warnings in CLANG +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wredeclared-class-member" +#endif #include +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + +#include namespace gtsam { /** - * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in - * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built - * from a factor graph prior to elimination, and stores the list of factors - * that involve each variable. - * \nosubgrouping - */ -class GTSAM_EXPORT MetisIndex -{ + * The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in + * METIS algorithms. Specifically, two vectors store the adjacency structure of the graph. It is built + * from a factor graph prior to elimination, and stores the list of factors + * that involve each variable. + * \nosubgrouping + */ +class GTSAM_EXPORT MetisIndex { public: - typedef boost::shared_ptr shared_ptr; - typedef boost::bimap bm_type; + typedef boost::shared_ptr shared_ptr; + typedef boost::bimap bm_type; private: - FastVector xadj_; // Index of node's adjacency list in adj - FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector - FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; + FastVector xadj_; // Index of node's adjacency list in adj + FastVector adj_; // Stores ajacency lists of all nodes, appended into a single vector + FastVector iadj_; // Integer keys for passing into metis. One to one mapping with adj_; boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship - size_t nFactors_; // Number of factors in the original factor graph - size_t nKeys_; + size_t nKeys_; public: - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /** Default constructor, creates empty MetisIndex */ - MetisIndex() : nFactors_(0), nKeys_(0) {} + /** Default constructor, creates empty MetisIndex */ + MetisIndex() : + nKeys_(0) { + } - template - MetisIndex(const FG& factorGraph) : nFactors_(0), nKeys_(0) { - augment(factorGraph); } + template + MetisIndex(const FG& factorGraph) : + nKeys_(0) { + augment(factorGraph); + } - ~MetisIndex(){} - /// @} - /// @name Advanced Interface - /// @{ + ~MetisIndex() { + } + /// @} + /// @name Advanced Interface + /// @{ - /** - * Augment the variable index with new factors. This can be used when - * solving problems incrementally. - */ - template - void augment(const FactorGraph& factors); + /** + * Augment the variable index with new factors. This can be used when + * solving problems incrementally. + */ + template + void augment(const FactorGraph& factors); - std::vector xadj() const { return xadj_; } - std::vector adj() const { return adj_; } - size_t nValues() const { return nKeys_; } + std::vector xadj() const { + return xadj_; + } + std::vector adj() const { + return adj_; + } + size_t nValues() const { + return nKeys_; + } Key intToKey(idx_t value) const { assert(value >= 0); return intKeyBMap_.right.find(value)->second; } - /// @} + /// @} }; -} +} // \ namesace gtsam #include From e0248c3ca77095ab376525d6e77fdcae381b512d Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 10:53:26 +0100 Subject: [PATCH 80/89] Created keysAndDims and safe version of values --- gtsam_unstable/nonlinear/Expression-inl.h | 8 +- gtsam_unstable/nonlinear/Expression.h | 104 ++++++++++++++---- gtsam_unstable/nonlinear/ExpressionFactor.h | 20 +--- .../nonlinear/tests/testExpression.cpp | 32 +++--- 4 files changed, 108 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 58f6de6b4..08dd18ee3 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -252,7 +252,7 @@ public: } /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { + virtual void dims(std::map& map) const { } // Return size needed for memory buffer in traceExecution @@ -324,7 +324,7 @@ public: } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + virtual void dims(std::map& map) const { // get dimension from the chart; only works for fixed dimension charts map[key_] = traits::dimension::value; } @@ -371,7 +371,7 @@ public: } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + virtual void dims(std::map& map) const { map[key_] = traits::dimension::value; } @@ -523,7 +523,7 @@ struct GenerateFunctionalNode: Argument, Base { } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + virtual void dims(std::map& map) const { Base::dims(map); This::expression->dims(map); } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 5e1d425ed..3b5026348 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -20,8 +20,12 @@ #pragma once #include "Expression-inl.h" +#include #include + #include +#include +#include namespace gtsam { @@ -31,6 +35,11 @@ namespace gtsam { template class Expression { +public: + + /// Define type so we can apply it as a meta-function + typedef Expression type; + private: // Paul's trick shared pointer, polymorphic root of entire expression tree @@ -55,7 +64,7 @@ public: // Construct a leaf expression, creating Symbol Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { + root_(new LeafExpression(Symbol(c, j))) { } /// Construct a nullary method expression @@ -87,8 +96,7 @@ public: template Expression(typename BinaryExpression::Function function, const Expression& expression1, const Expression& expression2) : - root_( - new BinaryExpression(function, expression1, expression2)) { + root_(new BinaryExpression(function, expression1, expression2)) { } /// Construct a ternary function expression @@ -101,14 +109,9 @@ public: expression2, expression3)) { } - /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } - - /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); + /// Return root + const boost::shared_ptr >& root() const { + return root_; } // Return size needed for memory buffer in traceExecution @@ -116,13 +119,77 @@ public: return root_->traceSize(); } - /// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below! + /// Return keys that play in this expression + std::set keys() const { + return root_->keys(); + } + + /// Return dimensions for each argument, as a map + void dims(std::map& map) const { + root_->dims(map); + } + + /// return keys and dimensions as vectors in same order + std::pair, FastVector > keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + FastVector keys(n); + boost::copy(map | boost::adaptors::map_keys, keys.begin()); + FastVector dims(n); + boost::copy(map | boost::adaptors::map_values, dims.begin()); + return make_pair(keys, dims); + } + + /** + * @brief Return value and optional derivatives, reverse AD version + * Notes: this is not terribly efficient, and H should have correct size. + */ + T value(const Values& values, boost::optional&> H = + boost::none) const { + + if (H) { + // Get keys and dimensions + FastVector keys; + FastVector dims; + boost::tie(keys, dims) = keysAndDims(); + + // H should be pre-allocated + assert(H->size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix + static const int Dim = traits::dimension::value; + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + JacobianMap jacobianMap(keys, Ab); + + // Call unsafe version + T result = value(values, jacobianMap); + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H->at(i) = Ab(i); + + return result; + } else { + // no derivatives needed, just return value + return root_->value(values); + } + } + +private: + + /// trace execution, very unsafe T traceExecution(const Values& values, ExecutionTrace& trace, ExecutionTraceStorage* traceStorage) const { return root_->traceExecution(values, trace, traceStorage); } - /// Return value and derivatives, reverse AD version + /** + * @brief Return value and derivatives, reverse AD version + * This fairly unsafe method needs a JacobianMap with correctly allocated + * and initialized VerticalBlockMatrix, hence is declared private. + */ T value(const Values& values, JacobianMap& jacobians) const { // The following piece of code is absolutely crucial for performance. // We allocate a block of memory on the stack, which can be done at runtime @@ -137,17 +204,6 @@ public: return value; } - /// Return value - T value(const Values& values) const { - return root_->value(values); - } - - const boost::shared_ptr >& root() const { - return root_; - } - - /// Define type so we can apply it as a meta-function - typedef Expression type; }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 22a2aefeb..22c2f527f 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -22,8 +22,6 @@ #include #include #include -#include -#include #include namespace gtsam { @@ -36,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor { T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled - std::vector dimensions_; ///< dimensions of the Jacobian matrices + FastVector dimensions_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -54,17 +52,9 @@ public: "ExpressionFactor was created with a NoiseModel of incorrect dimension."); noiseModel_ = noiseModel; - // Get dimensions of Jacobian matrices + // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now - std::map map; - expression_.dims(map); - size_t n = map.size(); - - keys_.resize(n); - boost::copy(map | boost::adaptors::map_keys, keys_.begin()); - - dimensions_.resize(n); - boost::copy(map | boost::adaptors::map_values, dimensions_.begin()); + boost::tie(keys_,dimensions_) = expression_.keysAndDims(); // Add sizes to know how much memory to allocate on stack in linearize augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1); @@ -126,13 +116,13 @@ public: // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap map(keys_, Ab); + JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, map); // <<< Reverse AD happens here ! + T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! Ab(size()).col(0) = -chart.local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 4e032b9f2..d02a67b2d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -114,22 +114,29 @@ TEST(Expression, NullaryMethod) { Values values; values.insert(67, Point3(3, 4, 5)); - // Pre-allocate JacobianMap - FastVector keys; - keys.push_back(67); - FastVector dims; - dims.push_back(3); - VerticalBlockMatrix Ab(dims, 1); - JacobianMap map(keys, Ab); + // Check dims as map + std::map map; + norm.dims(map); + LONGS_EQUAL(1,map.size()); - // Get value and Jacobian - double actual = norm.value(values, map); + // Get and check keys and dims + FastVector keys; + FastVector dims; + boost::tie(keys, dims) = norm.keysAndDims(); + LONGS_EQUAL(1,keys.size()); + LONGS_EQUAL(1,dims.size()); + LONGS_EQUAL(67,keys[0]); + LONGS_EQUAL(3,dims[0]); + + // Get value and Jacobians + std::vector H(1); + double actual = norm.value(values, H); // Check all EXPECT(actual == sqrt(50)); Matrix expected(1, 3); expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); - EXPECT(assert_equal(expected,Ab(0))); + EXPECT(assert_equal(expected,H[0])); } /* ************************************************************************* */ // Binary(Leaf,Leaf) @@ -159,7 +166,7 @@ TEST(Expression, BinaryKeys) { /* ************************************************************************* */ // dimensions TEST(Expression, BinaryDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3); + map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); EXPECT(actual==expected); } @@ -190,8 +197,7 @@ TEST(Expression, TreeKeys) { /* ************************************************************************* */ // dimensions TEST(Expression, TreeDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3)(3, - 5); + map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); EXPECT(actual==expected); } From 07e5475b6b47b173e101213fc9bc39c910c2def0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 11:02:54 +0100 Subject: [PATCH 81/89] Making friends... --- gtsam_unstable/nonlinear/Expression.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 3b5026348..40b55c410 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -27,8 +27,13 @@ #include #include +class ExpressionFactorShallowTest; + namespace gtsam { +// Forward declare +template class ExpressionFactor; + /** * Expression class that supports automatic differentiation */ @@ -204,6 +209,10 @@ private: return value; } + // be very selective on who can access these private methods: + friend class ExpressionFactor; + friend class ::ExpressionFactorShallowTest; + }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator From 2c35cda71f7b9750f8a8d9547c14d6e8e4b075ae Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 11:23:38 +0100 Subject: [PATCH 82/89] Yet another indirection makes public code a bit cleaner. --- gtsam_unstable/nonlinear/Expression.h | 63 +++++++++++++++------------ 1 file changed, 35 insertions(+), 28 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 40b55c410..8f1514a22 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -45,6 +45,8 @@ public: /// Define type so we can apply it as a meta-function typedef Expression type; + typedef std::pair, FastVector > KeysAndDims; + private: // Paul's trick shared pointer, polymorphic root of entire expression tree @@ -135,7 +137,7 @@ public: } /// return keys and dimensions as vectors in same order - std::pair, FastVector > keysAndDims() const { + KeysAndDims keysAndDims() const { std::map map; dims(map); size_t n = map.size(); @@ -153,37 +155,42 @@ public: T value(const Values& values, boost::optional&> H = boost::none) const { - if (H) { - // Get keys and dimensions - FastVector keys; - FastVector dims; - boost::tie(keys, dims) = keysAndDims(); - - // H should be pre-allocated - assert(H->size()==keys.size()); - - // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension::value; - VerticalBlockMatrix Ab(dims, Dim); - Ab.matrix().setZero(); - JacobianMap jacobianMap(keys, Ab); - - // Call unsafe version - T result = value(values, jacobianMap); - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(keys.size()); i++) - H->at(i) = Ab(i); - - return result; - } else { + if (H) + // Call provate version that returns derivatives in H + return value(values, keysAndDims(), *H); + else // no derivatives needed, just return value return root_->value(values); - } } private: + /// private version that takes keys and dimensions, returns derivatives + T value(const Values& values, const KeysAndDims& keysAndDims, + std::vector& H) const { + + const FastVector& keys = keysAndDims.first; + const FastVector& dims = keysAndDims.second; + + // H should be pre-allocated + assert(H->size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix + static const int Dim = traits::dimension::value; + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + JacobianMap jacobianMap(keys, Ab); + + // Call unsafe version + T result = value(values, jacobianMap); + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); + + return result; + } + /// trace execution, very unsafe T traceExecution(const Values& values, ExecutionTrace& trace, ExecutionTraceStorage* traceStorage) const { @@ -192,7 +199,7 @@ private: /** * @brief Return value and derivatives, reverse AD version - * This fairly unsafe method needs a JacobianMap with correctly allocated + * This very unsafe method needs a JacobianMap with correctly allocated * and initialized VerticalBlockMatrix, hence is declared private. */ T value(const Values& values, JacobianMap& jacobians) const { @@ -210,7 +217,7 @@ private: } // be very selective on who can access these private methods: - friend class ExpressionFactor; + friend class ExpressionFactor ; friend class ::ExpressionFactorShallowTest; }; From 2ced73ebe189ef12ea2e6ebfcd763d920098f37e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 11:29:50 +0100 Subject: [PATCH 83/89] We now use safe version in unwhitenedError --- gtsam_unstable/nonlinear/ExpressionFactor.h | 41 +++++++-------------- 1 file changed, 14 insertions(+), 27 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 22c2f527f..61eb94c89 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -34,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor { T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled - FastVector dimensions_; ///< dimensions of the Jacobian matrices + FastVector dims_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -54,13 +54,13 @@ public: // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now - boost::tie(keys_,dimensions_) = expression_.keysAndDims(); + boost::tie(keys_, dims_) = expression_.keysAndDims(); // Add sizes to know how much memory to allocate on stack in linearize - augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1); + augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1); #ifdef DEBUG_ExpressionFactor - BOOST_FOREACH(size_t d, dimensions_) + BOOST_FOREACH(size_t d, dims_) std::cout << d << " "; std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; #endif @@ -76,32 +76,15 @@ public: // TODO(PTF) Is this a place for custom charts? DefaultChart chart; if (H) { - // H should be pre-allocated - assert(H->size()==size()); - - VerticalBlockMatrix Ab(dimensions_, Dim); - - // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_, Ab); - Ab.matrix().setZero(); - - // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, map); // <<< Reverse AD happens here ! - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(size()); i++) - H->at(i) = Ab(i); - + const T value = expression_.value(x, std::make_pair(keys_, dims_), *H); return chart.local(measurement_, value); } else { - const T& value = expression_.value(x); + const T value = expression_.value(x); return chart.local(measurement_, value); } } virtual boost::shared_ptr linearize(const Values& x) const { - // TODO(PTF) Is this a place for custom charts? - DefaultChart chart; // Only linearize if the factor is active if (!active(x)) return boost::shared_ptr(); @@ -110,9 +93,9 @@ public: // In case noise model is constrained, we need to provide a noise model bool constrained = noiseModel_->is_constrained(); boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained ? new JacobianFactor(keys_, dims_, Dim, boost::static_pointer_cast(noiseModel_)->unit()) : - new JacobianFactor(keys_, dimensions_, Dim)); + new JacobianFactor(keys_, dims_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); @@ -121,13 +104,17 @@ public: // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); - // Evaluate error to get Jacobians and RHS vector b + // Get value and Jacobians, writing directly into JacobianFactor T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + + // Evaluate error and set RHS vector b + // TODO(PTF) Is this a place for custom charts? + DefaultChart chart; Ab(size()).col(0) = -chart.local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(),dummy); + noiseModel_->WhitenSystem(Ab.matrix(), dummy); return factor; } From df91cf7fadb1e9a8bba3bbd6710e8651f13640d6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 12:36:52 +0100 Subject: [PATCH 84/89] Made vaguely unsafe keysAndDims private (as it relies on keys and dimensions being in same order), as to not tempt people to use it. --- gtsam_unstable/nonlinear/Expression.h | 27 +++++++++---------- .../nonlinear/tests/testExpression.cpp | 9 ------- .../nonlinear/tests/testExpressionFactor.cpp | 11 ++++++++ 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 8f1514a22..4684184b8 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -45,8 +45,6 @@ public: /// Define type so we can apply it as a meta-function typedef Expression type; - typedef std::pair, FastVector > KeysAndDims; - private: // Paul's trick shared pointer, polymorphic root of entire expression tree @@ -136,18 +134,6 @@ public: root_->dims(map); } - /// return keys and dimensions as vectors in same order - KeysAndDims keysAndDims() const { - std::map map; - dims(map); - size_t n = map.size(); - FastVector keys(n); - boost::copy(map | boost::adaptors::map_keys, keys.begin()); - FastVector dims(n); - boost::copy(map | boost::adaptors::map_values, dims.begin()); - return make_pair(keys, dims); - } - /** * @brief Return value and optional derivatives, reverse AD version * Notes: this is not terribly efficient, and H should have correct size. @@ -165,6 +151,19 @@ public: private: + /// Vaguely unsafe keys and dimensions in same order + typedef std::pair, FastVector > KeysAndDims; + KeysAndDims keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + FastVector keys(n); + boost::copy(map | boost::adaptors::map_keys, keys.begin()); + FastVector dims(n); + boost::copy(map | boost::adaptors::map_values, dims.begin()); + return make_pair(keys, dims); + } + /// private version that takes keys and dimensions, returns derivatives T value(const Values& values, const KeysAndDims& keysAndDims, std::vector& H) const { diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index d02a67b2d..6156d103c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -119,15 +119,6 @@ TEST(Expression, NullaryMethod) { norm.dims(map); LONGS_EQUAL(1,map.size()); - // Get and check keys and dims - FastVector keys; - FastVector dims; - boost::tie(keys, dims) = norm.keysAndDims(); - LONGS_EQUAL(1,keys.size()); - LONGS_EQUAL(1,dims.size()); - LONGS_EQUAL(67,keys[0]); - LONGS_EQUAL(3,dims[0]); - // Get value and Jacobians std::vector H(1); double actual = norm.value(values, H); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b9d9896d3..d146ea945 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -202,6 +202,17 @@ TEST(ExpressionFactor, Shallow) { // Construct expression, concise evrsion Point2_ expression = project(transform_to(x_, p_)); + // Get and check keys and dims + FastVector keys; + FastVector dims; + boost::tie(keys, dims) = expression.keysAndDims(); + LONGS_EQUAL(2,keys.size()); + LONGS_EQUAL(2,dims.size()); + LONGS_EQUAL(1,keys[0]); + LONGS_EQUAL(2,keys[1]); + LONGS_EQUAL(6,dims[0]); + LONGS_EQUAL(3,dims[1]); + // traceExecution of shallow tree typedef UnaryExpression Unary; typedef BinaryExpression Binary; From dc4c0b54c0f0169175fc9b2963edc316e8c299f3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 16:13:30 +0100 Subject: [PATCH 85/89] Addressed code review by @hannessommer --- gtsam_unstable/nonlinear/Expression.h | 26 ++++++++++----------- gtsam_unstable/nonlinear/ExpressionFactor.h | 2 +- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 4684184b8..68a79ed6b 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -137,14 +137,16 @@ public: /** * @brief Return value and optional derivatives, reverse AD version * Notes: this is not terribly efficient, and H should have correct size. + * The order of the Jacobians is same as keys in either keys() or dims() */ T value(const Values& values, boost::optional&> H = boost::none) const { - if (H) - // Call provate version that returns derivatives in H - return value(values, keysAndDims(), *H); - else + if (H) { + // Call private version that returns derivatives in H + KeysAndDims pair = keysAndDims(); + return value(values, pair.first, pair.second, *H); + } else // no derivatives needed, just return value return root_->value(values); } @@ -157,19 +159,15 @@ private: std::map map; dims(map); size_t n = map.size(); - FastVector keys(n); - boost::copy(map | boost::adaptors::map_keys, keys.begin()); - FastVector dims(n); - boost::copy(map | boost::adaptors::map_values, dims.begin()); - return make_pair(keys, dims); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; } /// private version that takes keys and dimensions, returns derivatives - T value(const Values& values, const KeysAndDims& keysAndDims, - std::vector& H) const { - - const FastVector& keys = keysAndDims.first; - const FastVector& dims = keysAndDims.second; + T value(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const { // H should be pre-allocated assert(H->size()==keys.size()); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 61eb94c89..069fb5e2e 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -76,7 +76,7 @@ public: // TODO(PTF) Is this a place for custom charts? DefaultChart chart; if (H) { - const T value = expression_.value(x, std::make_pair(keys_, dims_), *H); + const T value = expression_.value(x, keys_, dims_, *H); return chart.local(measurement_, value); } else { const T value = expression_.value(x); From ee63fb0bb43818846344b8ac7104f61ad5ac2160 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 25 Nov 2014 15:44:59 -0500 Subject: [PATCH 86/89] Remove debug cmake messages --- CMakeLists.txt | 1 - gtsam/CMakeLists.txt | 2 -- 2 files changed, 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b6e59cc57..308f7f72e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -330,7 +330,6 @@ endif(GTSAM_BUILD_UNSTABLE) # Install config and export files GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") -message("GTSAM export: ${GTSAM_EXPORTED_TARGETS}") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b58a5b4b8..b77d936dc 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -92,8 +92,6 @@ set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSIO set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM Version: ${gtsam_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") -message("GTSAM Additional: ${GTSAM_ADDITIONAL_LIBRARIES}") -message("GTSAM Exports: ${GTSAM_EXPORTED_TARGETS}") # build shared and static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) From 41197f1ec786d486ff0a139709da742033379f7b Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 25 Nov 2014 15:51:49 -0500 Subject: [PATCH 87/89] Move warning suppression --- CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 308f7f72e..0888a394e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,8 +101,6 @@ if(MSVC) add_definitions(-DBOOST_ALL_NO_LIB) add_definitions(-DBOOST_ALL_DYN_LINK) endif() - # Also disable certain warnings - add_definitions(/wd4503) endif() find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) @@ -281,7 +279,7 @@ include_directories(BEFORE if(MSVC) add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) - add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings + add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings endif() # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. From a0452bdac9070bdc178ed134bdd2a4c9211239dd Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 25 Nov 2014 15:55:59 -0500 Subject: [PATCH 88/89] Minor format --- gtsam/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b77d936dc..0ebd6c07d 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -10,7 +10,7 @@ set (gtsam_subdirs linear nonlinear slam - navigation + navigation ) set(gtsam_srcs) @@ -74,8 +74,8 @@ set(gtsam_srcs ${linear_srcs} ${nonlinear_srcs} ${slam_srcs} - ${navigation_srcs} - ${gtsam_core_headers} + ${navigation_srcs} + ${gtsam_core_headers} ) # Generate and install config and dllexport files From e842c9adbc0dd00cda8e342dff4b71d082677749 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 26 Nov 2014 09:34:37 +0100 Subject: [PATCH 89/89] Fixed issue #172 --- gtsam.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index 0ceda6db5..d63563028 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2272,7 +2272,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include -template +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, @@ -2288,7 +2288,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include