From 44ae90f52377a4242972c83dbb89fdc196ab1685 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Mon, 14 Oct 2013 22:33:15 +0000 Subject: [PATCH] added SmartHessianProjection factor: allows different calibrations and noises for the cameras --- .../slam/SmartProjectionHessianFactor.h | 599 +++++++++++++ .../testSmartProjectionHessianFactor.cpp | 844 ++++++++++++++++++ 2 files changed, 1443 insertions(+) create mode 100644 gtsam_unstable/slam/SmartProjectionHessianFactor.h create mode 100644 gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp diff --git a/gtsam_unstable/slam/SmartProjectionHessianFactor.h b/gtsam_unstable/slam/SmartProjectionHessianFactor.h new file mode 100644 index 000000000..5e175e307 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionHessianFactor.h @@ -0,0 +1,599 @@ +/* ---------------------------------------------------------------------------- + + * 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 ProjectionFactor.h + * @brief Basic bearing factor from 2D measurement + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +//#include + +static bool isDebug=false; + +namespace gtsam { + + // default threshold for selective relinearization + static double defaultLinThreshold = -1; // 1e-7; // 0.01 + // default threshold for retriangulation + static double defaultTriangThreshold = 1e-5; + // default threshold for rank deficient triangulation + static double defaultRankTolerance = 1; // this value may be scenario-dependent and has to be larger in presence of larger noise + // if set to true will use the rotation-only version for degenerate cases + static bool manageDegeneracy = true; + + /** + * Structure for storing some state memory, used to speed up optimization + * @addtogroup SLAM + */ + class SmartProjectionHessianFactorState { + public: + + static int lastID; + int ID; + + SmartProjectionHessianFactorState() { + ID = lastID++; + calculatedHessian = false; + } + + // Linearization point + Values values; + std::vector cameraPosesLinearization; + + // Triangulation at current linearization point + Point3 point; + std::vector cameraPosesTriangulation; + bool degenerate; + bool cheiralityException; + + // Overall reprojection error + double overallError; + std::vector cameraPosesError; + + // Hessian representation (after Schur complement) + bool calculatedHessian; + Matrix H; + Vector gs_vector; + std::vector Gs; + std::vector gs; + double f; + + // C = Hl'Hl + // Cinv = inv(Hl'Hl) + // Matrix3 Cinv; + // E = Hx'Hl + // w = Hl'b + }; + + int SmartProjectionHessianFactorState::lastID = 0; + + /** + * The calibration is known here. + * @addtogroup SLAM + */ + template + class SmartProjectionHessianFactor: 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< SharedNoiseModel > noise_; ///< noise model used + ///< (important that the order is the same as the keys that we use to create the factor) + std::vector< boost::shared_ptr > K_all_; ///< shared pointer to calibration object (one for each camera) + + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + + double rankTolerance; ///< threshold to decide whether triangulation is degenerate + + double linearizationThreshold; ///< threshold to decide whether to re-linearize + + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for each camera) + + boost::shared_ptr state_; + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + public: + + /// shorthand for base class type + typedef NonlinearFactor Base; + + /// shorthand for this class + typedef SmartProjectionHessianFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for smart projection factor state variable + typedef boost::shared_ptr SmartFactorStatePtr; + + /** + * Constructor + * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark + * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements) + * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views) + * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartProjectionHessianFactor( + const double rankTol = defaultRankTolerance, + const double linThreshold = defaultLinThreshold, + boost::optional body_P_sensor = boost::none, + SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionHessianFactorState())) : + retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol), + linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor), + state_(state), throwCheirality_(false), verboseCheirality_(false) {} + + + /** Virtual destructor */ + virtual ~SmartProjectionHessianFactor() {} + + /** + * 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 the index corresponding to the camera observing the same landmark + */ + void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, + const boost::shared_ptr K_i) { + measured_.push_back(measured_i); + keys_.push_back(poseKey_i); + noise_.push_back(noise_i); + K_all_.push_back(K_i); + } + + void add(std::vector< Point2 > measurements, std::vector< Key > poseKeys, std::vector< SharedNoiseModel > noises, + std::vector< boost::shared_ptr > Ks) { + for(size_t i = 0; i < measurements.size(); i++) { + measured_.push_back(measurements.at(i)); + keys_.push_back(poseKeys.at(i)); + noise_.push_back(noises.at(i)); + K_all_.push_back(Ks.at(i)); + } + } + + void add(std::vector< Point2 > measurements, std::vector< Key > poseKeys, const SharedNoiseModel noise, + const boost::shared_ptr K) { + for(size_t i = 0; i < measurements.size(); i++) { + measured_.push_back(measurements.at(i)); + keys_.push_back(poseKeys.at(i)); + noise_.push_back(noise); + K_all_.push_back(K); + } + } + + // This function checks if the new linearization point is the same as the one used for previous triangulation + // (if not, a new triangulation is needed) + static bool decideIfTriangulate(std::vector cameraPoses, std::vector oldPoses, double retriangulationThreshold) { + // 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 + + // if we do not have a previous linearization point or the new linearization point includes more poses + if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size())) + return true; + + for(size_t i = 0; i < cameraPoses.size(); i++) { + if (!cameraPoses[i].equals(oldPoses[i], retriangulationThreshold)) { + return true; // at least two poses are different, hence we retriangulate + } + } + return false; // 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 + // (if not, a new linearization is needed) + static bool decideIfLinearize(std::vector cameraPoses, std::vector oldPoses, double linearizationThreshold) { + // "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 we do not have a previous linearization point or the new linearization point includes more poses + if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size())) + return true; + + Pose3 firstCameraPose; + Pose3 firstCameraPoseOld; + + for(size_t i = 0; i < cameraPoses.size(); i++) { + + if(i==0){ // we store the initial pose, this is useful for selective re-linearization + firstCameraPose = cameraPoses[i]; + firstCameraPoseOld = oldPoses[i]; + continue; + } + + // we compare the poses in the frame of the first pose + Pose3 localCameraPose = firstCameraPose.between(cameraPoses[i]); + Pose3 localCameraPoseOld = firstCameraPoseOld.between(oldPoses[i]); + + if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold)) { + return true; // at least two "relative" poses are different, hence we re-linerize + } + } + return false; // if we arrive to this point all poses are the same and we don't need re-linerize + } + + + /** + * 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 << "SmartProjectionHessianFactor, z = \n "; + BOOST_FOREACH(const Point2& p, measured_) { + std::cout << "measurement, p = "<< p << std::endl; + } + BOOST_FOREACH(const SharedNoiseModel& noise_i, noise_) { + noise_i->print("noise model = "); + } + if(this->body_P_sensor_){ + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + bool areMeasurementsEqual = true; + for(size_t i = 0; i < measured_.size(); i++) { + if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + areMeasurementsEqual = false; + break; + } + + return e + && Base::equals(p, tol) + && areMeasurementsEqual + //&& this->K_->equals(*e->K_all_, tol); + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + /// get the dimension of the factor (number of rows on linearization) + virtual size_t dim() const { + return 6*keys_.size(); + } + + /// linearize returns a Hessianfactor that is an approximation of error(p) + virtual boost::shared_ptr linearize(const Values& values) const { + + bool blockwise = false; // the full matrix version in faster + int dim_landmark = 3; // for degenerate instances this will become 2 (direction-only information) + + // Create structures for Hessian Factors + unsigned int numKeys = keys_.size(); + if(isDebug) {std::cout<< " numKeys = "<< numKeys< js; + std::vector Gs(numKeys*(numKeys+1)/2); + std::vector gs(numKeys); + double f=0; + + // Collect all poses (Cameras) + std::vector cameraPoses; + BOOST_FOREACH(const Key& k, keys_) { + Pose3 cameraPose; + if(body_P_sensor_) { cameraPose = values.at(k).compose(*body_P_sensor_);} + else { cameraPose = values.at(k);} + cameraPoses.push_back(cameraPose); + } + + if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative + state_->degenerate = true; + BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); + BOOST_FOREACH(Vector& v, gs) v = zero(6); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed + } + + bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold); + + if(retriangulate) {// we store the current poses used for triangulation + state_->cameraPosesTriangulation = cameraPoses; + } + + if (retriangulate) { + // We triangulate the 3D position of the landmark + try { + state_->point = triangulatePoint3(cameraPoses, measured_, *K_all_.at(0), rankTolerance); + state_->degenerate = false; + state_->cheiralityException = false; + } catch( TriangulationUnderconstrainedException& e) { + // 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 + //std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity + state_->degenerate = true; + state_->cheiralityException = false; + } catch( TriangulationCheiralityException& e) { + // 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 + //std::cout << e.what() << std::end; + state_->cheiralityException = true; + } + } + + if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){ + // std::cout << "In linearize: exception" << std::endl; + BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); + BOOST_FOREACH(Vector& v, gs) v = zero(6); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); + } + + if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors + state_->degenerate = true; + dim_landmark = 2; + } + + bool doLinearize; + if (linearizationThreshold >= 0){//by convention if linearizationThreshold is negative we always relinearize + // std::cout << "Temporary disabled" << std::endl; + doLinearize = decideIfLinearize(cameraPoses, state_->cameraPosesLinearization, linearizationThreshold); + } + else{ + doLinearize = true; + } + + if (doLinearize) { + state_->cameraPosesLinearization = cameraPoses; + } + + if(!doLinearize){ // return the previous Hessian factor + // std::cout << "Using stored factors :) " << std::endl; + return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f)); + } + + if (blockwise == false){ // version with full matrix multiplication + // ========================================================================================================== + Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys); + Matrix Hl2 = zeros(2 * numKeys, dim_landmark); + Vector b2 = zero(2 * numKeys); + + if(state_->degenerate){ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_all_.at(i)); + if(i==0){ // first pose + state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); + // 3D parametrization of point at infinity: [px py 1] + // std::cout << "point_ " << state_->point<< std::endl; + } + Matrix Hxi, Hli; + Vector bi = -( camera.projectPointAtInfinity(state_->point,Hxi,Hli) - measured_.at(i) ).vector(); + // std::cout << "Hxi \n" << Hxi<< std::endl; + // std::cout << "Hli \n" << Hli<< std::endl; + + noise_.at(i)-> WhitenSystem(Hxi, Hli, bi); + f += bi.squaredNorm(); + + Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; + Hl2.block( 2*i, 0, 2, 2 ) = Hli; + + subInsert(b2,bi,2*i); + } + // std::cout << "Hx2 \n" << Hx2<< std::endl; + // std::cout << "Hl2 \n" << Hl2<< std::endl; + } + else{ + + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_all_.at(i)); + Matrix Hxi, Hli; + + Vector bi; + try { + bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector(); + } catch ( CheiralityException& e) { + std::cout << "Cheirality exception " << state_->ID << std::endl; + exit(EXIT_FAILURE); + } + noise_.at(i)-> WhitenSystem(Hxi, Hli, bi); + f += bi.squaredNorm(); + + Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; + Hl2.block( 2*i, 0, 2, 3 ) = Hli; + + subInsert(b2,bi,2*i); + } + + } + + // Shur complement trick + Matrix H(6 * numKeys, 6 * numKeys); + Matrix C2; + Vector gs_vector; + + C2.noalias() = (Hl2.transpose() * Hl2).inverse(); + H.noalias() = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2)))); + gs_vector.noalias() = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2)))); + + // Populate Gs and gs + int GsCount2 = 0; + for(size_t i1 = 0; i1 < numKeys; i1++) { + gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6); + + for(size_t i2 = 0; i2 < numKeys; i2++) { + if (i2 >= i1) { + Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6); + GsCount2++; + } + } + } + } + + // ========================================================================================================== + if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables + state_->calculatedHessian = true; + state_->Gs = Gs; + state_->gs = gs; + state_->f = f; + } + + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); + } + + /** + * 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. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + double overallError=0; + + // Collect all poses (Cameras) + std::vector cameraPoses; + BOOST_FOREACH(const Key& k, keys_) { + Pose3 cameraPose; + if(body_P_sensor_) { cameraPose = values.at(k).compose(*body_P_sensor_);} + else { cameraPose = values.at(k);} + cameraPoses.push_back(cameraPose); + + if(0&& isDebug) {cameraPose.print("cameraPose = "); } + } + + if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative + return 0.0; + } + + bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold); + + if(retriangulate) {// we store the current poses used for triangulation + state_->cameraPosesTriangulation = cameraPoses; + } + + if (retriangulate) { + // We triangulate the 3D position of the landmark + try { + state_->point = triangulatePoint3(cameraPoses, measured_, *K_all_.at(0), rankTolerance); + state_->degenerate = false; + state_->cheiralityException = false; + } catch( TriangulationUnderconstrainedException& e) { + // 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 + //std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity + state_->degenerate = true; + state_->cheiralityException = false; + } catch( TriangulationCheiralityException& e) { + // 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 + //std::cout << e.what() << std::end; + state_->cheiralityException = true; + } + } + + if (!manageDegeneracy && (state_->cheiralityException || state_->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 (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors + state_->degenerate = true; + } + + if(state_->degenerate){ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_all_.at(i)); + if(i==0){ // first pose + state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity + } + Point2 reprojectionError(camera.projectPointAtInfinity(state_->point) - measured_.at(i)); + overallError += 0.5 * noise_.at(i)->distance( reprojectionError.vector() ); + //overallError += reprojectionError.vector().norm(); + } + return overallError; + } + else{ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_all_.at(i)); + + try { + Point2 reprojectionError(camera.project(state_->point) - measured_.at(i)); + //std::cout << "Reprojection error: " << reprojectionError << std::endl; + overallError += 0.5 * noise_.at(i)->distance( reprojectionError.vector() ); + //overallError += reprojectionError.vector().norm(); + } catch ( CheiralityException& e) { + std::cout << "Cheirality exception " << state_->ID << std::endl; + exit(EXIT_FAILURE); + } + } + return overallError; + } + } else { // else of active flag + return 0.0; + } + } + + /** return the measurements */ + const Vector& measured() const { + return measured_; + } + + /** return the noise model */ + const SharedNoiseModel& noise() const { + return noise_; + } + + /** return the landmark */ + boost::optional point() const { + return state_->point; + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_all_; + } + + /** return 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(measured_); + ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + + }; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp new file mode 100644 index 000000000..1361033b3 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp @@ -0,0 +1,844 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSmartProjectionHessianFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Frank Dellaert + * @date Nov 2009 + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 double rankTol = 1.0; +static double linThreshold = -1.0; +// 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 +Symbol x1('X', 1); +Symbol x2('X', 2); +Symbol x3('X', 3); + +static Key poseKey1(x1); +static Key poseKey2(x2); +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 SmartProjectionHessianFactor SmartFactor; + +/* ************************************************************************* */ +TEST( SmartProjectionHessianFactor, Constructor) { + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +TEST( SmartProjectionHessianFactor, Constructor2) { + SmartFactor factor1(rankTol, linThreshold); +} + +/* ************************************************************************* */ +TEST( SmartProjectionHessianFactor, Constructor3) { + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionHessianFactor, Constructor4) { + SmartFactor factor1(rankTol, linThreshold); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionHessianFactor, ConstructorWithTransform) { + SmartFactor factor1(rankTol, linThreshold, body_P_sensor1); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionHessianFactor, 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( SmartProjectionHessianFactor, noiseless ){ + // cout << " ************************ SmartProjectionHessianFactor: 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::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, x1, model, K); + factor1->add(level_uv_right, x2, model, K); + + double actualError = factor1->error(values); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionHessianFactor, noisy ){ + // cout << " ************************ SmartProjectionHessianFactor: 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( SmartProjectionHessianFactor, 3poses_smart_projection_factor ){ + // cout << " ************************ SmartProjectionHessianFactor: 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 + 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); + + 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_(SmartProjectionHessianFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionHessianFactor); + 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( SmartProjectionHessianFactor, 3poses_iterative_smart_projection_factor ){ + // cout << " ************************ SmartProjectionHessianFactor: 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 + 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); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(cam1_uv1, views[0], model, K); + smartFactor1->add(cam2_uv1, views[1], model, K); + smartFactor1->add(cam3_uv1, views[2], model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(cam1_uv2, views[0], model, K); + smartFactor2->add(cam2_uv2, views[1], model, K); + smartFactor2->add(cam3_uv2, views[2], model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(cam1_uv3, views[0], model, K); + smartFactor3->add(cam2_uv3, views[1], model, K); + smartFactor3->add(cam3_uv3, views[2], 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_(SmartProjectionHessianFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionHessianFactor); + 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( SmartProjectionHessianFactor, 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( SmartProjectionHessianFactor, 3poses_2land_rotation_only_smart_projection_factor ){ + // cout << " ************************ SmartProjectionHessianFactor: 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 + 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); + + double rankTol = 50; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); + smartFactor1->add(measurements_cam1, views, model, K2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); + 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_(SmartProjectionHessianFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionHessianFactor); + 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( SmartProjectionHessianFactor, 3poses_rotation_only_smart_projection_factor ){ + // cout << " ************************ SmartProjectionHessianFactor: 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 + 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; + + 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); + + 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_(SmartProjectionHessianFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionHessianFactor); + 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( SmartProjectionHessianFactor, Hessian ){ + // cout << " ************************ SmartProjectionHessianFactor: 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( SmartProjectionHessianFactor, HessianWithRotation ){ + // cout << " ************************ SmartProjectionHessianFactor: 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; + + // 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); + + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); + smartFactorInstance->add(cam1_uv1, views[0], model, K); + smartFactorInstance->add(cam2_uv1, views[1], model, K); + smartFactorInstance->add(cam3_uv1, views[2], 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( SmartProjectionHessianFactor, HessianWithRotationDegenerate ){ + // cout << " ************************ SmartProjectionHessianFactor: 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; + + // 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); + + SmartFactor::shared_ptr smartFactor(new SmartFactor()); + smartFactor->add(cam1_uv1, views[0], model, K2); + smartFactor->add(cam2_uv1, views[1], model, K2); + smartFactor->add(cam3_uv1, views[2], 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) ); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +