diff --git a/.cproject b/.cproject index 1dcc51dfe..d42d2b0e0 100644 --- a/.cproject +++ b/.cproject @@ -2777,6 +2777,14 @@ true true + + make + -j4 + testCustomChartExpression.run + true + true + true + make -j2 diff --git a/CMakeLists.txt b/CMakeLists.txt index b6e59cc57..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. @@ -330,7 +328,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/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 9e9c74edc..b999e6600 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.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 diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b58a5b4b8..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 @@ -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) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 30136d9f2..f2c4566bb 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), delta); } -// The benefit of this method is that it does not need to know what types are involved -// to evaluate the factor. If all the machinery of gtsam is working correctly, we should -// get the correct numerical derivatives out the other side. -template -JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor, - const Values& values, - double fd_step) { - Eigen::VectorXd e = factor.unwhitenedError(values); - const size_t rows = e.size(); - - std::map jacobians; - typename FactorType::const_iterator key_it = factor.begin(); - VectorValues dX = values.zeroVectors(); - for(; key_it != factor.end(); ++key_it) { - size_t key = *key_it; - // Compute central differences using the values struct. - const size_t cols = dX.dim(key); - Matrix J = Matrix::Zero(rows, cols); - for(size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = fd_step; - dX[key] = dx; - Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.unwhitenedError(eval_values); - dx[col] = -fd_step; - dX[key] = dx; - eval_values = values.retract(dX); - Eigen::VectorXd right = factor.unwhitenedError(eval_values); - J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); - } - jacobians[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - -template -void testFactorJacobians(TestResult& result_, - const std::string& name_, - const FactorType& f, - const gtsam::Values& values, - double fd_step, - double tolerance) { - // Check linearization - JacobianFactor expected = computeNumericalDerivativeJacobianFactor(f, values, fd_step); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - - typedef std::pair Jacobian; - Jacobian evalJ = jf->jacobianUnweighted(); - Jacobian estJ = expected.jacobianUnweighted(); - EXPECT(assert_equal(evalJ.first, estJ.first, tolerance)); - EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); - EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); -} - } // namespace gtsam -/// \brief Check the Jacobians produced by a factor against finite differences. -/// \param factor The factor to test. -/// \param values Values filled in for testing the Jacobians. -/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians -/// \param tolerance The numerical tolerance to use when comparing Jacobians. -#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ - { gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } - 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/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f48c188aa..b9e03c01d 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 9c326a8d2..60ea7693d 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]); diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index f599a2dea..cd6f09507 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -19,10 +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() << ')'; + return os; +} + +} // namespace gtsam diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index d62a3f067..694bf525b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -88,7 +88,7 @@ namespace gtsam { StereoPoint2 operator-(const StereoPoint2& b) const { return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } - + /// @} /// @name Manifold /// @{ @@ -143,15 +143,18 @@ namespace gtsam { } /** convenient function to get a Point2 from the left image */ - inline Point2 point2(){ + Point2 point2() const { return Point2(uL_, v_); } /** convenient function to get a Point2 from the right image */ - inline Point2 right(){ + Point2 right() const { return Point2(uR_, v_); } + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p); + private: /// @} diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4595a70ed..e983ccb17 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -302,8 +302,8 @@ namespace gtsam { } // overloaded insert with chart initializer - template - void Values::insert(Key j, const ValueType& val, ChartInit chart) { + template + void Values::insert(Key j, const ValueType& val, Chart chart) { insert(j, static_cast(ChartValue(val, chart))); } @@ -320,8 +320,8 @@ namespace gtsam { } // update with chart initializer, /todo: perhaps there is a way to init chart from old value... - template - void Values::update(Key j, const ValueType& val, ChartInit chart) { + template + void Values::update(Key j, const ValueType& val, Chart chart) { update(j, static_cast(ChartValue(val, chart))); } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e4a27849d..d00baa0d9 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -263,8 +263,8 @@ namespace gtsam { void insert(Key j, const ValueType& val); /// overloaded insert version that also specifies a chart initializer - template - void insert(Key j, const ValueType& val, ChartInit chart); + template + void insert(Key j, const ValueType& val, Chart chart); /** insert that mimics the STL map insert - if the value already exists, the map is not modified @@ -288,8 +288,8 @@ namespace gtsam { void update(Key j, const T& val); /// overloaded insert version that also specifies a chart initializer - template - void update(Key j, const T& val, ChartInit chart); + template + void update(Key j, const T& val, Chart chart); /** update the current available values without adding new ones */ void update(const Values& values); diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..923edddb7 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; @@ -40,8 +40,8 @@ public: 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 @@ -49,9 +49,9 @@ 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, 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 e28185038..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; + 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; @@ -37,9 +37,9 @@ 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; + 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, 2 * j++, m2, 2) * 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 8ae26e7cd..d9e7b9819 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -19,16 +19,16 @@ #pragma once -#include "JacobianFactorQ.h" -#include "JacobianFactorSVD.h" -#include "RegularImplicitSchurFactor.h" -#include "RegularHessianFactor.h" +#include +#include +#include +#include #include -#include +#include // for Cheirality exception +#include #include #include -#include #include #include @@ -36,40 +36,48 @@ 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) + static const int ZDim = traits::dimension::value; ///< Measurement dimension + /// 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: + + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// 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; + typedef CAMERA Camera; + typedef std::vector Cameras; /** * Constructor @@ -89,7 +97,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 +108,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 +121,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,7 +135,8 @@ public: * The noise is assumed to be the same for all measurements */ // **************************************************************************************************** - void add(const SfM_Track& trackToAdd, const SharedNoiseModel& 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); @@ -136,7 +145,7 @@ public: } /** return the measurements */ - const std::vector& measured() const { + const std::vector& measured() const { return measured_; } @@ -179,18 +188,18 @@ public: } // **************************************************************************************************** - /// Calculate vector of re-projection errors, before applying noise model +// /// 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()); + Vector b = zero(ZDim * cameras.size()); size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); + BOOST_FOREACH(const CAMERA& camera, cameras) { + const Z& zi = this->measured_.at(i); try { - Point2 e(camera.project(point) - zi); - b[2 * i] = e.x(); - b[2 * i + 1] = e.y(); + Z e(camera.project(point) - zi); + b[ZDim * i] = e.x(); + b[ZDim * i + 1] = e.y(); } catch (CheiralityException& e) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); @@ -215,10 +224,10 @@ public: double overallError = 0; size_t i = 0; - BOOST_FOREACH(const Camera& camera, cameras) { - const Point2& zi = this->measured_.at(i); + BOOST_FOREACH(const CAMERA& camera, cameras) { + 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&) { @@ -236,19 +245,19 @@ public: const Point3& point) const { int numKeys = this->keys_.size(); - E = zeros(2 * numKeys, 3); + E = zeros(ZDim * numKeys, 3); Vector b = zero(2 * numKeys); - Matrix Ei(2, 3); + 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); } this->noise_.at(i)->WhitenSystem(Ei, b); - E.block<2, 3>(2 * i, 0) = Ei; + E.block(ZDim * i, 0) = Ei; } // Matrix PointCov; @@ -262,11 +271,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(ZDim * numKeys, 3); + b = zero(ZDim * numKeys); double f = 0; - Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, 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; @@ -288,12 +297,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; // 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<2, 3>(2 * i, 0) = Ei; - subInsert(b, bi, 2 * i); + E.block(ZDim * i, 0) = Ei; + subInsert(b, bi, ZDim * i); } return f; } @@ -334,10 +343,10 @@ public: std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); - F = zeros(2 * numKeys, D * numKeys); + F = zeros(This::ZDim * 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(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras } return f; } @@ -356,9 +365,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(ZDim * numKeys, ZDim * 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, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns return f; } @@ -372,11 +381,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(ZDim * 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(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras return f; } @@ -437,9 +446,9 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(2 * numKeys, D * numKeys); + Matrix F = zeros(ZDim * 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(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras Matrix H(D * numKeys, D * numKeys); Vector gs_vector; @@ -477,16 +486,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(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<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(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) augmentedHessian(i1, i1) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * 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 @@ -494,7 +503,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(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras } @@ -521,16 +530,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(ZDim * 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(ZDim * i1) // F' * b + -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1); GsIndex++; } // upper triangular part of the hessian @@ -539,7 +548,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(ZDim * i2, 0).transpose() * Fi2); GsIndex++; } } // end of for over cameras @@ -587,9 +596,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(ZDim * i1, 0) * P; - // D = (Dx2) * (2) + // 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]; @@ -599,15 +608,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(ZDim * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + // (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<2, 3>(2 * 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 @@ -616,12 +625,12 @@ public: //Key cameraKey_i2 = this->keys_[i2]; DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]]; - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + // (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<2, 3>(2 * i2, 0).transpose() * Fi2); + - Fi1.transpose() * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras @@ -641,7 +650,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; @@ -650,18 +659,18 @@ 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); } // **************************************************************************************************** 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 Fblocks; Vector b; - Matrix Enull(2*numKeys, 2*numKeys-3); + Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b); + return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); } private: @@ -676,4 +685,7 @@ private: } }; +template +const int SmartFactorBase::ZDim; + } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index bfd73d9fb..75e4699d9 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartFactorBase.h" +#include #include #include @@ -61,8 +61,8 @@ enum LinearizationMode { * SmartProjectionFactor: triangulates point * TODO: why LANDMARK parameter? */ -template -class SmartProjectionFactor: public SmartFactorBase { +template +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 @@ -102,7 +102,9 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; + typedef SmartProjectionFactor This; + + static const int ZDim = traits::dimension::value; ///< Measurement dimension public: @@ -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 @@ -707,4 +709,7 @@ private: } }; +template +const int SmartProjectionFactor::ZDim; + } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index f871ab3aa..3b2e2bcbc 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -19,7 +19,7 @@ #pragma once -#include "SmartProjectionFactor.h" +#include namespace gtsam { /** @@ -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/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index e96c8101c..29eaf2ac1 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -114,7 +114,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); @@ -164,7 +164,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; 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; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp new file mode 100644 index 000000000..f5e59b1b2 --- /dev/null +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -0,0 +1,153 @@ +/* ---------------------------------------------------------------------------- + +* 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; + + 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, init_pose3Out; + + 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"); + + //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]; + } + if(add_initial_noise){ + m(1,3) += (pose_id % 10)/10.0; + } + 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; + + // 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"); + + 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; +} 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..68a79ed6b 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -20,17 +20,31 @@ #pragma once #include "Expression-inl.h" +#include #include + #include +#include +#include + +class ExpressionFactorShallowTest; namespace gtsam { +// Forward declare +template class ExpressionFactor; + /** * Expression class that supports automatic differentiation */ 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 +69,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 +101,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 +114,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 +124,81 @@ 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); + } + + /** + * @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 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); + } + +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(); + 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 FastVector& keys, + const FastVector& dims, std::vector& H) const { + + // 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 { return root_->traceExecution(values, trace, traceStorage); } - /// Return value and derivatives, reverse AD version + /** + * @brief Return value and derivatives, reverse AD version + * 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 { // 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 +213,10 @@ public: return value; } - /// Return value - T value(const Values& values) const { - return root_->value(values); - } + // be very selective on who can access these private methods: + friend class ExpressionFactor ; + friend class ::ExpressionFactorShallowTest; - 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..181ac49b4 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 dims_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -54,23 +52,15 @@ 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_, 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 @@ -83,35 +73,17 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - // 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, 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(); @@ -120,24 +92,28 @@ 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(); - 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 ! + // 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; } diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h index ae2c1e7e5..92c8f71e8 100644 --- a/gtsam_unstable/nonlinear/expressionTesting.h +++ b/gtsam_unstable/nonlinear/expressionTesting.h @@ -30,19 +30,88 @@ namespace gtsam { +/** + * Linearize a nonlinear factor using numerical differentiation + * The benefit of this method is that it does not need to know what types are + * involved to evaluate the factor. If all the machinery of gtsam is working + * correctly, we should get the correct numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a map of Jacobians + std::map jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for (size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians[key] = J; + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +namespace internal { +// CPPUnitLite-style test for linearization of a factor +void testFactorJacobians(TestResult& result_, const std::string& name_, + const NoiseModelFactor& factor, const gtsam::Values& values, double delta, + double tolerance) { + + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, delta); + + // Create actual value by linearize + boost::shared_ptr actual = // + boost::dynamic_pointer_cast(factor.linearize(values)); + + // Check cast result and then equality + CHECK(actual); + EXPECT( assert_equal(expected, *actual, tolerance)); +} +} + +/// \brief Check the Jacobians produced by a factor against finite differences. +/// \param factor The factor to test. +/// \param values Values filled in for testing the Jacobians. +/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ + { gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } + +namespace internal { +// CPPUnitLite-style test for linearization of an ExpressionFactor template -void testExpressionJacobians(TestResult& result_, - const std::string& name_, - const gtsam::Expression& expression, - const gtsam::Values& values, - double nd_step, - double tolerance) { +void testExpressionJacobians(TestResult& result_, const std::string& name_, + const gtsam::Expression& expression, const gtsam::Values& values, + double nd_step, double tolerance) { // Create factor size_t size = traits::dimension::value; - ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); + ExpressionFactor f(noiseModel::Unit::Create(size), + expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} // namespace gtsam +} +} // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. /// \param expression The expression to test. @@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_, /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ - { gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } + { gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp new file mode 100644 index 000000000..bc2055c55 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testCustomChartExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include + +#include + +using namespace gtsam; + +// A simple prototype custom chart that does two things: +// 1. it reduces the dimension of the variable being estimated +// 2. it scales the input to retract. +// +// The Jacobian of this chart is: +// [ 2 0 ] +// [ 0 3 ] +// [ 0 0 ] +struct ProjectionChart { + typedef Eigen::Vector3d type; + typedef Eigen::Vector2d vector; + static vector local(const type& origin, const type& other) { + return (other - origin).head<2>(); + } + static type retract(const type& origin, const vector& d) { + return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0); + } + static int getDimension(const type& origin) { + return 2; + } +}; + +namespace gtsam { +namespace traits { +template<> struct is_chart : public boost::true_type {}; +template<> struct dimension : public boost::integral_constant {}; +} // namespace traits +} // namespace gtsam + +TEST(ExpressionCustomChart, projection) { + // Create expression + Expression point(1); + + Eigen::Vector3d pval(1.0, 2.0, 3.0); + Values standard; + standard.insert(1, pval); + + Values custom; + custom.insert(1, pval, ProjectionChart()); + + Eigen::Vector3d pstandard = point.value(standard); + Eigen::Vector3d pcustom = point.value(custom); + + // The values should be the same. + EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10)); + + + EXPECT_LONGS_EQUAL(3, standard.at(1).dim()); + VectorValues vstandard = standard.zeroVectors(); + EXPECT_LONGS_EQUAL(3, vstandard.at(1).size()); + + + EXPECT_LONGS_EQUAL(2, custom.at(1).dim()); + VectorValues vcustom = custom.zeroVectors(); + EXPECT_LONGS_EQUAL(2, vcustom.at(1).size()); + + ExpressionFactor f(noiseModel::Unit::Create(pval.size()), pval, point); + + boost::shared_ptr gfstandard = f.linearize(standard); + boost::shared_ptr jfstandard = // + boost::dynamic_pointer_cast(gfstandard); + + typedef std::pair Jacobian; + Jacobian Jstandard = jfstandard->jacobianUnweighted(); + EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10)); + + boost::shared_ptr gfcustom = f.linearize(custom); + boost::shared_ptr jfcustom = // + boost::dynamic_pointer_cast(gfcustom); + + Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2); + expectedJacobian(0,0) = 2.0; + expectedJacobian(1,1) = 3.0; + Jacobian Jcustom = jfcustom->jacobianUnweighted(); + EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10)); + + // Amazingly, the finite differences get the expected Jacobian right. + const double fd_step = 1e-5; + const double tolerance = 1e-5; + EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 4e032b9f2..6156d103c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -114,22 +114,20 @@ 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 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 +157,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 +188,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); } 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; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h new file mode 100644 index 000000000..1c0d1bc37 --- /dev/null +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -0,0 +1,748 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +#include +#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: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + 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; + + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a StereoCamera // TODO: Get rid of this? + typedef StereoCamera Camera; + + /// Vector of cameras + typedef 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; + std::cout << "linearizationThreshold_ = " << linearizationThreshold_ << 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; + + //TODO: Chris will replace this with something else for stereo +// point_ = triangulatePoint3(cameras, this->measured_, +// rankTolerance_, enableEPI_); + + // // // Temporary hack to use monocular triangulation + std::vector mono_measurements; + BOOST_FOREACH(const StereoPoint2& sp, this->measured_) { + mono_measurements.push_back(sp.point2()); + } + + std::vector > mono_cameras; + BOOST_FOREACH(const 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); + + 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 StereoPoint2& zi = this->measured_.at(i); + try { + StereoPoint2 reprojectionError(camera.project(point_) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + cheiralityException_ = true; + } + i += 1; + } + //std::cout << "totalReprojError error: " << totalReprojError << std::endl; + // we discard smart factors that have large reprojection error + if(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 (isDebug) std::cout << "point_ = " << point_ << std::endl; + + if (numKeys < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + if (isDebug) std::cout << "In linearize: exception" << std::endl; + 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; + 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(); + + 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; + if (isDebug) std::cout << "H:\n" << H << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * 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_) { + 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_); + } // 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() * 3); + } + + /** + * 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 StereoPoint2& zi = this->measured_.at(i); +// if (i == 0) // first pose +// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity +// StereoPoint2 reprojectionError( +// camera.projectPointAtInfinity(this->point_) - zi); +// overallError += 0.5 +// * this->noise_.at(i)->distance(reprojectionError.vector()); +// i += 1; +// } +// return overallError; + } else { + // Just use version in base class + return Base::totalReprojectionError(cameras, point_); + } + } + + /// 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_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h new file mode 100644 index 000000000..1f2bd1942 --- /dev/null +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -0,0 +1,218 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +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: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// 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 StereoPoint2 measured_i, const Key poseKey_i, + const SharedNoiseModel noise_i, + const boost::shared_ptr K_i) { + Base::add(measured_i, poseKey_i, noise_i); + K_all_.push_back(K_i); + } + + /** + * 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 : + throw("JacobianQ not working yet!"); +// return this->createJacobianQFactor(cameras(values), 0.0); + break; + default: + return this->createHessianFactor(cameras(values)); + break; + } + } + + /** + * error calculates the error of the factor. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** return the calibration object */ + inline const std::vector > calibration() const { + return K_all_; + } + +private: + + /// Serialization function + 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_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp new file mode 100644 index 000000000..05260521e --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -0,0 +1,1038 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +static bool isDebugTest = true; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static double b = 1; + +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); +static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + +static double rankTol = 1.0; +static double linThreshold = -1.0; +static bool manageDegeneracy = true; +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(3)); + +// 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 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 SmartFactorBundler; + +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); + StereoPoint2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); + + return measurements_cam; +} + +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, Constructor) { + fprintf(stderr,"Test 1 Complete"); + 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)); + 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, K2); + factor1.add(level_uv_right, x2, model, K2); + + 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); + + // 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); + + 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, 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); + + // 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 ){ +// +// 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); + + // 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 ){ +// // 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 = 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) ); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +