diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h new file mode 100644 index 000000000..139e194b4 --- /dev/null +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -0,0 +1,383 @@ +/** + * @file ImplicitSchurFactor.h + * @brief A new type of linear factor (GaussianFactor), which is subclass of JacobiaFactor + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * ImplicitSchurFactor + */ +template // +class ImplicitSchurFactor: public GaussianFactor { + +public: + typedef ImplicitSchurFactor This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +protected: + + typedef Eigen::Matrix Matrix2D; ///< type of an F block + typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix MatrixDD; ///< camera hessian + typedef std::pair KeyMatrix2D; ///< named F block + + std::vector Fblocks_; ///< All 2*D F blocks (one for each camera) + Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) + Matrix E_; ///< The 2m*3 E Jacobian with respect to the point + Vector b_; ///< 2m-dimensional RHS vector + +public: + + /// Constructor + ImplicitSchurFactor() { + } + + /// Construct from blcoks of F, E, inv(E'*E), and RHS vector b + ImplicitSchurFactor(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b) : + Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { + initKeys(); + } + + /// initialize keys from Fblocks + void initKeys() { + keys_.reserve(Fblocks_.size()); + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) + keys_.push_back(it.first); + } + + /// Destructor + virtual ~ImplicitSchurFactor() { + } + + // Write access, only use for construction! + + inline std::vector& Fblocks() { + return Fblocks_; + } + + inline Matrix3& PointCovariance() { + return PointCovariance_; + } + + inline Matrix& E() { + return E_; + } + + inline Vector& b() { + return b_; + } + + /// Get matrix P + inline const Matrix& getPointCovariance() const { + return PointCovariance_; + } + + /// print + void print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << " ImplicitSchurFactor " << std::endl; + Factor::print(s); + std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; + std::cout << " E_ \n" << E_ << std::endl; + std::cout << " b_ \n" << b_.transpose() << std::endl; + } + + /// equals + bool equals(const GaussianFactor& lf, double tol) const { + if (!dynamic_cast(&lf)) + return false; + else { + return false; + } + } + + /// Degrees of freedom of camera + virtual DenseIndex getDim(const_iterator variable) const { + return D; + } + + virtual Matrix augmentedJacobian() const { + throw std::runtime_error( + "ImplicitSchurFactor::augmentedJacobian non implemented"); + return Matrix(); + } + virtual std::pair jacobian() const { + throw std::runtime_error("ImplicitSchurFactor::jacobian non implemented"); + return std::make_pair(Matrix(), Vector()); + } + virtual Matrix augmentedInformation() const { + throw std::runtime_error( + "ImplicitSchurFactor::augmentedInformation non implemented"); + return Matrix(); + } + virtual Matrix information() const { + throw std::runtime_error( + "ImplicitSchurFactor::information non implemented"); + return Matrix(); + } + + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const { + // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); + VectorValues d; + + for (size_t pos = 0; pos < size(); ++pos) { // for each camera + Key j = keys_[pos]; + + // Calculate Fj'*Ej for the current camera (observing a single point) + // D x 3 = (D x 2) * (2 x 3) + const Matrix2D& Fj = Fblocks_[pos].second; + Eigen::Matrix FtE = Fj.transpose() + * E_.block<2, 3>(2 * pos, 0); + + Eigen::Matrix dj; + for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + // Vector column_k_Fj = Fj.col(k); + dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); + // Vector column_k_FtE = FtE.row(k); + // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) + dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); + } + d.insert(j, dj); + } + return d; + } + + /** + * @brief add the contribution of this factor to the diagonal of the hessian + * d(output) = d(input) + deltaHessianFactor + */ + void hessianDiagonal(double* d) const { + // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor + Key j = keys_[pos]; + + // Calculate Fj'*Ej for the current camera (observing a single point) + // D x 3 = (D x 2) * (2 x 3) + const Matrix2D& Fj = Fblocks_[pos].second; + Eigen::Matrix FtE = Fj.transpose() + * E_.block<2, 3>(2 * pos, 0); + + DVector dj; + for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian + dj(k) = Fj.col(k).squaredNorm(); + // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) + dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose(); + } + DMap(d + D * j) += dj; + } + } + + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const { + std::map blocks; + for (size_t pos = 0; pos < size(); ++pos) { + Key j = keys_[pos]; + const Matrix2D& Fj = Fblocks_[pos].second; + // F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) + Eigen::Matrix FtE = Fj.transpose() + * E_.block<2, 3>(2 * pos, 0); + blocks[j] = Fj.transpose() * Fj + - FtE * PointCovariance_ * FtE.transpose(); + // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( +// static const Eigen::Matrix I2 = eye(2); +// Eigen::Matrix Q = // +// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); +// blocks[j] = Fj.transpose() * Q * Fj; + } + return blocks; + } + + virtual GaussianFactor::shared_ptr clone() const { + return boost::make_shared >(Fblocks_, + PointCovariance_, E_, b_); + throw std::runtime_error("ImplicitSchurFactor::clone non implemented"); + } + virtual bool empty() const { + return false; + } + + virtual GaussianFactor::shared_ptr negate() const { + return boost::make_shared >(Fblocks_, + PointCovariance_, E_, b_); + throw std::runtime_error("ImplicitSchurFactor::negate non implemented"); + } + + // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing + static + void multiplyHessianAdd(const Matrix& F, const Matrix& E, + const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) { + Vector e1 = F * x; + Vector d1 = E.transpose() * e1; + Vector d2 = PointCovariance * d1; + Vector e2 = E * d2; + Vector e3 = alpha * (e1 - e2); + y += F.transpose() * e3; + } + + typedef std::vector Error2s; + + /** + * @brief Calculate corrected error Q*e = (I - E*P*E')*e + */ + void projectError(const Error2s& e1, Error2s& e2) const { + + // d1 = E.transpose() * e1 = (3*2m)*2m + Vector3 d1; + d1.setZero(); + for (size_t k = 0; k < size(); k++) + d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k]; + + // d2 = E.transpose() * e1 = (3*2m)*2m + Vector3 d2 = PointCovariance_ * d1; + + // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] + for (size_t k = 0; k < size(); k++) + e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; + } + + /// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) + virtual double error(const VectorValues& x) const { + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x - b = (2m*dm)*dm + for (size_t k = 0; k < size(); ++k) + e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); + projectError(e1, e2); + + double result = 0; + for (size_t k = 0; k < size(); ++k) + result += dot(e2[k], e2[k]); + return 0.5 * result; + } + + /// Scratch space for multiplyHessianAdd + mutable Error2s e1, e2; + + /** + * @brief double* Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ + void multiplyHessianAdd(double alpha, const double* x, double* y) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x = (2m*dm)*dm + size_t k = 0; + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { + Key key = it.first; + e1[k++] = it.second * ConstDMap(x + D * key); + } + + projectError(e1, e2); + + // y += F.transpose()*e2 = (2d*2m)*2m + k = 0; + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { + Key key = it.first; + DMap(y + D * key) += it.second.transpose() * alpha * e2[k++]; + } + } + + void multiplyHessianAdd(double alpha, const double* x, double* y, + std::vector keys) const { + } + ; + + /** + * @brief Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x + */ + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + + // resize does not do malloc if correct size + e1.resize(size()); + e2.resize(size()); + + // e1 = F * x = (2m*dm)*dm + for (size_t k = 0; k < size(); ++k) + e1[k] = Fblocks_[k].second * x.at(keys_[k]); + + projectError(e1, e2); + + // y += F.transpose()*e2 = (2d*2m)*2m + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + static const Vector empty; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + // Create the value as a zero vector if it does not exist. + if (it.second) + yi = Vector::Zero(Fblocks_[k].second.cols()); + yi += Fblocks_[k].second.transpose() * alpha * e2[k]; + } + } + + /** + * @brief Dummy version to measure overhead of key access + */ + void multiplyHessianDummy(double alpha, const VectorValues& x, + VectorValues& y) const { + + BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { + static const Vector empty; + Key key = Fi.first; + std::pair it = y.tryInsert(key, empty); + Vector& yi = it.first->second; + yi = x.at(key); + } + } + + /** + * Calculate gradient, which is -F'Q*b, see paper + */ + VectorValues gradientAtZero() const { + // calculate Q*b + e1.resize(size()); + e2.resize(size()); + for (size_t k = 0; k < size(); k++) + e1[k] = b_.segment < 2 > (2 * k); + projectError(e1, e2); + + // g = F.transpose()*e2 + VectorValues g; + for (size_t k = 0; k < size(); ++k) { + Key key = keys_[k]; + g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); + } + + // return it + return g; + } + +}; +// ImplicitSchurFactor + +} + diff --git a/gtsam_unstable/slam/JacobianFactorQ.h b/gtsam_unstable/slam/JacobianFactorQ.h new file mode 100644 index 000000000..fdbe0e231 --- /dev/null +++ b/gtsam_unstable/slam/JacobianFactorQ.h @@ -0,0 +1,47 @@ +/* + * @file JacobianFactorQ.h + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once + +#include "JacobianSchurFactor.h" + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianFactorQ: public JacobianSchurFactor { + + typedef JacobianSchurFactor Base; + +public: + + /// Default constructor + JacobianFactorQ() { + } + + /// Constructor + 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; + // Calculate projector Q + Matrix Q = eye(m2) - E * P * E.transpose(); + // Calculate pre-computed Jacobian matrices + // TODO: can we do better ? + 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) + BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + // Which is then passed to the normal JacobianFactor constructor + JacobianFactor::fillTerms(QF, Q * b, model); + } +}; + +} diff --git a/gtsam_unstable/slam/JacobianSchurFactor.h b/gtsam_unstable/slam/JacobianSchurFactor.h new file mode 100644 index 000000000..2beecc264 --- /dev/null +++ b/gtsam_unstable/slam/JacobianSchurFactor.h @@ -0,0 +1,93 @@ +/* + * @file JacobianSchurFactor.h + * @brief Jacobianfactor that combines and eliminates points + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianSchurFactor: public JacobianFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhiten(Ax) : Ax; + } + + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const + { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); + if (empty()) return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; pos +#include +#include + +namespace gtsam { + +template +class RegularHessianFactor: public HessianFactor { + +private: + + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix VectorD; + +public: + + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the + * quadratic term (the Hessian matrix) provided in row-order, gs the pieces + * of the linear vector term, and f the constant term. + */ + RegularHessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + HessianFactor(js, Gs, gs, f) { + } + + /** Constructor with an arbitrary number of keys and with the augmented information matrix + * specified as a block matrix. */ + template + RegularHessianFactor(const KEYS& keys, + const SymmetricBlockMatrix& augmentedInformation) : + HessianFactor(keys, augmentedInformation) { + } + + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); + } + + // Scratch space for multiplyHessianAdd + typedef Eigen::Matrix DVector; + mutable std::vector y; + + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { + // Create a vector of temporary y values, corresponding to rows i + y.resize(size()); + BOOST_FOREACH(DVector & yi, y) + yi.setZero(); + + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + DVector xj(D); + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + Key key = keys_[j]; + const double* xj = x + key * D; + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + Key key = keys_[i]; + DMap(yvalues + key * D) += alpha * y[i]; + } + } + +}; + +} + diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h new file mode 100644 index 000000000..2c81fb5f5 --- /dev/null +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -0,0 +1,518 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartFactorBase.h + * @brief Base class to create smart factors on poses or cameras + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include "JacobianFactorQ.h" +#include "ImplicitSchurFactor.h" +#include "RegularHessianFactor.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { +/// Base class with no internal point, completely functional +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 noise_; ///< noise model used + ///< (important that the order is the same as the keys that we use to create the factor) + + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + + /// Definitions for blocks of F + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' + typedef std::pair KeyMatrix2D; // Fblocks + typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix Matrix2; + + /// shorthand for base class type + typedef NonlinearFactor Base; + + /// shorthand for this class + typedef SmartFactorBase This; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a pinhole camera + typedef PinholeCamera Camera; + typedef std::vector Cameras; + + /** + * Constructor + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartFactorBase(boost::optional body_P_sensor = boost::none) : + body_P_sensor_(body_P_sensor) { + } + + /** Virtual destructor */ + virtual ~SmartFactorBase() { + } + + /** + * add a new measurement and pose key + * @param measured_i is the 2m dimensional projection of a single landmark + * @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, + const SharedNoiseModel& noise_i) { + this->measured_.push_back(measured_i); + this->keys_.push_back(poseKey_i); + this->noise_.push_back(noise_i); + } + + /** + * 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, + std::vector& noises) { + for (size_t i = 0; i < measurements.size(); i++) { + this->measured_.push_back(measurements.at(i)); + this->keys_.push_back(poseKeys.at(i)); + this->noise_.push_back(noises.at(i)); + } + } + + /** + * 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, + const SharedNoiseModel& noise) { + for (size_t i = 0; i < measurements.size(); i++) { + this->measured_.push_back(measurements.at(i)); + this->keys_.push_back(poseKeys.at(i)); + this->noise_.push_back(noise); + } + } + + /** + * Adds an entire SfM_track (collection of cameras observing a single point). + * The noise is assumed to be the same for all measurements + */ + // **************************************************************************************************** + void add(const SfM_Track& trackToAdd, const SharedNoiseModel& noise) { + for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { + this->measured_.push_back(trackToAdd.measurements[k].second); + this->keys_.push_back(trackToAdd.measurements[k].first); + this->noise_.push_back(noise); + } + } + + /** return the measurements */ + const Vector& measured() const { + return measured_; + } + + /** return the noise model */ + const SharedNoiseModel& noise() const { + return noise_; + } + + /** + * 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 << "SmartFactorBase, z = \n"; + for (size_t k = 0; k < measured_.size(); ++k) { + std::cout << "measurement, p = " << measured_[k] << "\t"; + noise_[k]->print("noise model = "); + } + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + + bool areMeasurementsEqual = true; + for (size_t i = 0; i < measured_.size(); i++) { + if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false) + areMeasurementsEqual = false; + break; + } + return e && Base::equals(p, tol) && areMeasurementsEqual + && ((!body_P_sensor_ && !e->body_P_sensor_) + || (body_P_sensor_ && e->body_P_sensor_ + && body_P_sensor_->equals(*e->body_P_sensor_))); + } + + // **************************************************************************************************** + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionError(const Cameras& cameras, const Point3& point) const { + + Vector b = zero(2 * cameras.size()); + + size_t i = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + try { + Point2 e(camera.project(point) - zi); + b[2 * i] = e.x(); + b[2 * i + 1] = e.y(); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + i += 1; + } + + return b; + } + + // **************************************************************************************************** + /** + * Calculate 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. + * This is different from reprojectionError(cameras,point) as each point is whitened + */ + double totalReprojectionError(const Cameras& cameras, + const Point3& point) const { + + double overallError = 0; + + size_t i = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + try { + Point2 reprojectionError(camera.project(point) - zi); + overallError += 0.5 + * this->noise_.at(i)->distance(reprojectionError.vector()); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + i += 1; + } + return overallError; + } + + // **************************************************************************************************** + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, + const Point3& point) const { + + int numKeys = this->keys_.size(); + E = zeros(2 * numKeys, 3); + Vector b = zero(2 * numKeys); + + Matrix Ei(2, 3); + for (size_t i = 0; i < this->measured_.size(); i++) { + try { + cameras[i].project(point, boost::none, Ei); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + this->noise_.at(i)->WhitenSystem(Ei, b); + E.block<2, 3>(2 * i, 0) = Ei; + } + + // Matrix PointCov; + PointCov.noalias() = (E.transpose() * E).inverse(); + } + + // **************************************************************************************************** + /// Compute F, E only (called below in both vanilla and SVD versions) + /// Given a Point3, assumes dimensionality is 3 + double computeJacobians(std::vector& Fblocks, Matrix& E, + Vector& b, const Cameras& cameras, const Point3& point) const { + + int numKeys = this->keys_.size(); + E = zeros(2 * numKeys, 3); + b = zero(2 * numKeys); + double f = 0; + + Matrix Fi(2, 6), Ei(2, 3), Hcali(2, D - 6), Hcam(2, D); + for (size_t i = 0; i < this->measured_.size(); i++) { + + Vector bi; + try { + bi = + -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + } catch (CheiralityException& e) { + std::cout << "Cheirality exception " << std::endl; + exit (EXIT_FAILURE); + } + this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); + + f += bi.squaredNorm(); + 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 + Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); + } + E.block<2, 3>(2 * i, 0) = Ei; + subInsert(b, bi, 2 * i); + } + return f; + } + + // **************************************************************************************************** + /// Version that computes PointCov, with optional lambda parameter + double computeJacobians(std::vector& Fblocks, Matrix& E, + Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, + double lambda = 0.0, bool diagonalDamping = false) const { + + double f = computeJacobians(Fblocks, E, b, cameras, point); + + // Point covariance inv(E'*E) + Matrix3 EtE = E.transpose() * E; + Matrix3 DMatrix = eye(E.cols()); // damping matrix + + if (diagonalDamping) { // diagonal of the hessian + DMatrix(0, 0) = EtE(0, 0); + DMatrix(1, 1) = EtE(1, 1); + DMatrix(2, 2) = EtE(2, 2); + } + + PointCov.noalias() = (EtE + lambda * DMatrix).inverse(); + + return f; + } + + // **************************************************************************************************** + // TODO, there should not be a Matrix version, really + double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, + const Cameras& cameras, const Point3& point, + const double lambda = 0.0) const { + + int numKeys = this->keys_.size(); + std::vector Fblocks; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, + lambda); + F = zeros(2 * 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 + } + return f; + } + + // **************************************************************************************************** + /// SVD version + double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, + Vector& b, const Cameras& cameras, const Point3& point, + double lambda = 0.0, bool diagonalDamping = false) const { + + Matrix E; + Matrix3 PointCov; // useless + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + + // Do SVD on A + Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); + Vector s = svd.singularValues(); + // Enull = zeros(2 * numKeys, 2 * numKeys - 3); + int numKeys = this->keys_.size(); + Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns + + return f; + } + + // **************************************************************************************************** + /// Matrix version of SVD + // TODO, there should not be a Matrix version, really + double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, + const Cameras& cameras, const Point3& point) const { + + int numKeys = this->keys_.size(); + std::vector Fblocks; + double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); + F.resize(2 * 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 + + return f; + } + + // **************************************************************************************************** + /// linearize returns a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Cameras& cameras, const Point3& point, const double lambda = 0.0, + bool diagonalDamping = false) const { + + int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + // Create structures for Hessian Factors + std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); + std::vector < Vector > gs(numKeys); + + sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); + // schurComplement(Fblocks, E, PointCov, b, Gs, gs); + + //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); + //std::vector < Vector > gs2(gs.begin(), gs.end()); + + return boost::make_shared < RegularHessianFactor + > (this->keys_, Gs, gs, f); + } + + // **************************************************************************************************** + void schurComplement(const std::vector& Fblocks, const Matrix& E, + const Matrix& PointCov, const Vector& b, + /*output ->*/std::vector& Gs, std::vector& gs) const { + // Schur complement trick + // Gs = F' * F - F' * E * inv(E'*E) * E' * F + // gs = F' * (b - E * inv(E'*E) * E' * b) + // This version uses full matrices + + int numKeys = this->keys_.size(); + + /// Compute Full F ???? + Matrix F = zeros(2 * 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 + + 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)))); + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * D; + gs.at(i1) = gs_vector.segment < D > (i1D); + for (DenseIndex i2 = 0; i2 < numKeys; i2++) { + if (i2 >= i1) { + Gs.at(GsCount2) = H.block(i1D, i2 * D); + GsCount2++; + } + } + } + } + + // **************************************************************************************************** + void sparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& PointCov, const Vector& b, + /*output ->*/std::vector& Gs, std::vector& gs) const { + // Schur complement trick + // Gs = F' * F - F' * E * inv(E'*E) * E' * F + // gs = F' * (b - E * inv(E'*E) * E' * b) + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); + + // Blockwise Schur complement + int GsCount = 0; + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera + const Matrix2D& Fi1 = Fblocks.at(i1).second; + // D = (Dx2) * (2) + gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + + for (size_t i2 = 0; i2 < numKeys; i2++) { + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + // Compute (Ei1 * PointCov * Ei2') + // (2x2) = (2x3) * (3x3) * (3x2) + Matrix2 E_invEtE_Et = E.block<2, 3>(2 * i1, 0) * PointCov * (E.block<2, 3>(2 * i2, 0)).transpose(); + + // D = (Dx2) * (2x2) * (2) + gs.at(i1) -= Fi1.transpose() * ( E_invEtE_Et * b.segment < 2 > (2 * i2) ); + + if (i2 == i1) { // diagonal entries + // (DxD) = (Dx2) * ( (2xD) - (2x2) * (2xD) ) + Gs.at(GsCount) = Fi1.transpose() * (Fi1 - E_invEtE_Et * Fi2); + GsCount++; + } + if (i2 > i1) { // off diagonal + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + Gs.at(GsCount) = - Fi1.transpose() * ( E_invEtE_Et * Fi2 ); + GsCount++; + } + } + } + } + + // **************************************************************************************************** + boost::shared_ptr > createImplicitSchurFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0, + bool diagonalDamping = false) const { + typename boost::shared_ptr > f( + new ImplicitSchurFactor()); + computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), + cameras, point, lambda, diagonalDamping); + f->initKeys(); + return f; + } + + // **************************************************************************************************** + boost::shared_ptr > createJacobianQFactor( + const Cameras& cameras, const Point3& point, double lambda = 0.0, + bool diagonalDamping = false) const { + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + return boost::make_shared < JacobianFactorQ > (Fblocks, E, PointCov, b); + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h new file mode 100644 index 000000000..a1b85e2c3 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -0,0 +1,663 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionFactor.h + * @brief Base class to create smart factors on poses or cameras + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include "SmartFactorBase.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gtsam { + +/** + * Structure for storing some state memory, used to speed up optimization + * @addtogroup SLAM + */ +class SmartProjectionFactorState { + +protected: + +public: + + SmartProjectionFactorState() { + } + // Hessian representation (after Schur complement) + bool calculatedHessian; + Matrix H; + Vector gs_vector; + std::vector Gs; + std::vector gs; + double f; +}; + +/** + * SmartProjectionFactor: triangulates point + * TODO: why LANDMARK parameter? + */ +template +class SmartProjectionFactor: 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; + + /// shorthand for this class + typedef SmartProjectionFactor This; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// shorthand for a pinhole camera + typedef PinholeCamera Camera; + typedef std::vector Cameras; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) + * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, + * otherwise the factor is simply neglected + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param body_P_sensor is the transform from body to sensor frame (default identity) + */ + SmartProjectionFactor(const double rankTol, const double linThreshold, + const bool manageDegeneracy, const bool enableEPI, + boost::optional body_P_sensor = boost::none, + SmartFactorStatePtr state = SmartFactorStatePtr( + new SmartProjectionFactorState())) : + 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) { + } + + /** Virtual destructor */ + virtual ~SmartProjectionFactor() { + } + + /** + * 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 << "SmartProjectionFactor, z = \n"; + std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; + std::cout << "degenerate_ = " << degenerate_ << std::endl; + std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; + Base::print("", keyFormatter); + } + + /// Check if the new linearization point_ is the same as the one used for previous triangulation + bool decideIfTriangulate(const Cameras& cameras) const { + // several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate + // Note that this is not yet "selecting linearization", that will come later, and we only check if the + // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_ + + size_t m = cameras.size(); + + bool retriangulate = false; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesTriangulation_.empty() + || cameras.size() != cameraPosesTriangulation_.size()) + retriangulate = true; + + if (!retriangulate) { + for (size_t i = 0; i < cameras.size(); i++) { + if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], + retriangulationThreshold_)) { + retriangulate = true; // at least two poses are different, hence we retriangulate + break; + } + } + } + + if (retriangulate) { // we store the current poses used for triangulation + cameraPosesTriangulation_.clear(); + cameraPosesTriangulation_.reserve(m); + for (size_t i = 0; i < m; i++) + // cameraPosesTriangulation_[i] = cameras[i].pose(); + cameraPosesTriangulation_.push_back(cameras[i].pose()); + } + + return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation + } + + /// This function checks if the new linearization point_ is 'close' to the previous one used for linearization + bool decideIfLinearize(const Cameras& cameras) const { + // "selective linearization" + // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose + // (we only care about the "rigidity" of the poses, not about their absolute pose) + + if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize + return true; + + // if we do not have a previous linearization point_ or the new linearization point_ includes more poses + if (cameraPosesLinearization_.empty() + || (cameras.size() != cameraPosesLinearization_.size())) + return true; + + Pose3 firstCameraPose, firstCameraPoseOld; + for (size_t i = 0; i < cameras.size(); i++) { + + if (i == 0) { // we store the initial pose, this is useful for selective re-linearization + firstCameraPose = cameras[i].pose(); + firstCameraPoseOld = cameraPosesLinearization_[i]; + continue; + } + + // we compare the poses in the frame of the first pose + Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose()); + Pose3 localCameraPoseOld = firstCameraPoseOld.between( + cameraPosesLinearization_[i]); + if (!localCameraPose.equals(localCameraPoseOld, + this->linearizationThreshold_)) + return true; // at least two "relative" poses are different, hence we re-linearize + } + return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize + } + + /// triangulateSafe + size_t triangulateSafe(const Values& values) const { + return triangulateSafe(this->cameras(values)); + } + + /// triangulateSafe + size_t triangulateSafe(const Cameras& cameras) const { + + size_t m = cameras.size(); + if (m < 2) { // if we have a single pose the corresponding factor is uninformative + degenerate_ = true; + return m; + } + bool retriangulate = decideIfTriangulate(cameras); + + if (retriangulate) { + // We triangulate the 3D position of the landmark + try { + // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + point_ = triangulatePoint3(cameras, this->measured_, + rankTolerance_, enableEPI_); + degenerate_ = false; + cheiralityException_ = false; + } catch (TriangulationUnderconstrainedException& e) { + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + degenerate_ = true; + cheiralityException_ = false; + } catch (TriangulationCheiralityException& e) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint + 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; + int numKeys = this->keys_.size(); + // Create structures for Hessian Factors + std::vector < Key > js; + std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); + std::vector < Vector > gs(numKeys); + + if (this->measured_.size() != cameras.size()) { + std::cout + << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" + << std::endl; + exit(1); + } + + this->triangulateSafe(cameras); + + if (numKeys < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // std::cout << "In linearize: exception" << std::endl; + BOOST_FOREACH(gtsam::Matrix& m, Gs) + m = zeros(D, D); + BOOST_FOREACH(Vector& v, gs) + v = zero(D); + return boost::make_shared >(this->keys_, Gs, gs, + 0.0); + } + + // instead, if we want to manage the exception.. + if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors + this->degenerate_ = true; + } + + bool doLinearize = this->decideIfLinearize(cameras); + + if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize + for (size_t i = 0; i < cameras.size(); i++) + this->cameraPosesLinearization_[i] = cameras[i].pose(); + + if (!doLinearize) { // return the previous Hessian factor + std::cout << "=============================" << std::endl; + std::cout << "doLinearize " << doLinearize << std::endl; + std::cout << "this->linearizationThreshold_ " + << this->linearizationThreshold_ << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + std::cout + << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled" + << std::endl; + exit(1); + return boost::make_shared >(this->keys_, + this->state_->Gs, this->state_->gs, this->state_->f); + } + + // ================================================================== + Matrix F, E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + + // Schur complement trick + // Frank says: should be possible to do this more efficiently? + // And we care, as in grouped factors this is called repeatedly + Matrix H(D * numKeys, D * numKeys); + Vector gs_vector; + + H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() + * (b - (E * (PointCov * (E.transpose() * b)))); + if (isDebug) + std::cout << "gs_vector size " << gs_vector.size() << std::endl; + + // Populate Gs and gs + int GsCount2 = 0; + for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera + DenseIndex i1D = i1 * D; + gs.at(i1) = gs_vector.segment < D > (i1D); + for (DenseIndex i2 = 0; i2 < 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::shared_ptr >(); + } + + /// 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::shared_ptr >(); + } + + /// 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 << "SmartProjectionFactor: this is not ready" << std::endl; + std::cout << "this->cheiralityException_ " << this->cheiralityException_ + << std::endl; + std::cout << "this->degenerate_ " << this->degenerate_ << std::endl; + } + return true; + } + + /// Takes values + bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeEP(E, PointCov, myCameras); + return nonDegenerate; + } + + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { + return Base::computeEP(E, PointCov, cameras, point_); + } + + /// Version that takes values, and creates the point + bool computeJacobians(std::vector& Fblocks, + Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + return nonDegenerate; + } + + /// Compute F, E only (called below in both vanilla and SVD versions) + /// Assumes the point has been computed + /// Note E can be 2m*3 or 2m*2, in case point is degenerate + double computeJacobians(std::vector& Fblocks, + Matrix& E, Vector& b, const Cameras& cameras) const { + + if (this->degenerate_) { + std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl; + std::cout << "point " << point_ << std::endl; + std::cout + << "SmartProjectionFactor: Management of degeneracy is disabled - not ready to be used" + << std::endl; + if (D > 6) { + std::cout + << "Management of degeneracy is not yet ready when one also optimizes for the calibration " + << std::endl; + } + + int numKeys = this->keys_.size(); + E = zeros(2 * numKeys, 2); + b = zero(2 * numKeys); + double f = 0; + for (size_t i = 0; i < this->measured_.size(); i++) { + if (i == 0) { // first pose + this->point_ = cameras[i].backprojectPointAtInfinity( + this->measured_.at(i)); + // 3D parametrization of point at infinity: [px py 1] + } + Matrix Fi, Ei; + Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) + - this->measured_.at(i)).vector(); + + this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); + f += bi.squaredNorm(); + Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); + E.block < 2, 2 > (2 * i, 0) = Ei; + subInsert(b, bi, 2 * i); + } + return f; + } else { + // nondegenerate: just return Base version + return Base::computeJacobians(Fblocks, E, b, cameras, point_); + } // end else + } + + /// Version that computes PointCov, with optional lambda parameter + double computeJacobians(std::vector& Fblocks, + Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, + const double lambda = 0.0) const { + + double f = computeJacobians(Fblocks, E, b, cameras); + + // Point covariance inv(E'*E) + PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); + + return f; + } + + /// takes values + bool computeJacobiansSVD(std::vector& Fblocks, + Matrix& Enull, Vector& b, const Values& values) const { + typename Base::Cameras myCameras; + double good = computeCamerasAndTriangulate(values, myCameras); + if (good) + computeJacobiansSVD(Fblocks, Enull, b, myCameras); + return true; + } + + /// SVD version + double computeJacobiansSVD(std::vector& Fblocks, + Matrix& Enull, Vector& b, const Cameras& cameras) const { + return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_); + } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + // TODO should there be a lambda? + double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobiansSVD(F, Enull, b, cameras, point_); + } + + /// Returns Matrix, TODO: maybe should not exist -> not sparse ! + double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, + const Cameras& cameras, const double lambda) const { + return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + } + + /// Calculate vector of re-projection errors, before applying noise model + /// Assumes triangulation was done and degeneracy handled + Vector reprojectionError(const Cameras& cameras) const { + return Base::reprojectionError(cameras, point_); + } + + /// Calculate vector of re-projection errors, before applying noise model + Vector reprojectionError(const Values& values) const { + Cameras myCameras; + bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); + if (nonDegenerate) + return reprojectionError(myCameras); + else + return zero(myCameras.size() * 2); + } + + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. + * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model + * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. + */ + double totalReprojectionError(const Cameras& cameras, + boost::optional externalPoint = boost::none) const { + + size_t nrCameras; + if (externalPoint) { + nrCameras = this->keys_.size(); + point_ = *externalPoint; + degenerate_ = false; + cheiralityException_ = false; + } else { + nrCameras = this->triangulateSafe(cameras); + } + + if (nrCameras < 2 + || (!this->manageDegeneracy_ + && (this->cheiralityException_ || this->degenerate_))) { + // if we don't want to manage the exceptions we discard the factor + // std::cout << "In error evaluation: exception" << std::endl; + return 0.0; + } + + if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors + std::cout + << "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!" + << std::endl; + this->degenerate_ = true; + } + + if (this->degenerate_) { + // return 0.0; // TODO: this maybe should be zero? + std::cout + << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!" + << std::endl; + size_t i = 0; + double overallError = 0; + BOOST_FOREACH(const Camera& camera, cameras) { + const Point2& zi = this->measured_.at(i); + if (i == 0) // first pose + this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity + Point2 reprojectionError( + camera.projectPointAtInfinity(this->point_) - zi); + overallError += 0.5 + * this->noise_.at(i)->distance(reprojectionError.vector()); + i += 1; + } + return overallError; + } else { + // Just use version in base class + return Base::totalReprojectionError(cameras, point_); + } + } + + /// Cameras are computed in derived class + virtual Cameras cameras(const Values& values) const = 0; + + /** return the landmark */ + boost::optional point() const { + return point_; + } + + /** COMPUTE the landmark */ + boost::optional point(const Values& values) const { + triangulateSafe(values); + return point_; + } + + /** return the degenerate state */ + inline bool isDegenerate() const { + return (cheiralityException_ || degenerate_); + } + + /** return the cheirality status flag */ + inline bool isPointBehindCamera() const { + return cheiralityException_; + } + /** return chirality verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h new file mode 100644 index 000000000..879e5ab80 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -0,0 +1,177 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionPoseFactor.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 "SmartProjectionFactor.h" + +namespace gtsam { + +/** + * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * @addtogroup SLAM + */ +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { +protected: + + // Known calibration + std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + +public: + + /// shorthand for base class type + typedef SmartProjectionFactor Base; + + /// shorthand for this class + typedef SmartProjectionPoseFactor 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) + */ + SmartProjectionPoseFactor(const double rankTol = 1, + const double linThreshold = -1, const bool manageDegeneracy = false, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + + /** Virtual destructor */ + virtual ~SmartProjectionPoseFactor() {} + + /** + * add a new measurement and pose key + * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param poseKey is the index corresponding to the camera observing the same landmark + * @param noise_i is the measurement noise + * @param K_i is the (known) camera calibration + */ + void add(const Point2 measured_i, const Key poseKey_i, + const SharedNoiseModel noise_i, + const boost::shared_ptr K_i) { + Base::add(measured_i, poseKey_i, noise_i); + K_all_.push_back(K_i); + } + + /** + * add a new measurements and pose keys + * Variant of the previous one in which we include a set of measurements + */ + 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)); + } + } + + /** + * add a new measurements and pose keys + * Variant of the previous one in which we include a set of measurements with the same noise and calibration + */ + 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 << "SmartProjectionPoseFactor, 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 + 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 returns a Hessian factor contraining the poses + */ + virtual boost::shared_ptr linearize( + const Values& values) const { + return this->createHessianFactor(cameras(values)); + } + + /** + * 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 boost::shared_ptr 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/testSmartProjectionHessianPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp new file mode 100644 index 000000000..9a3fe7f62 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp @@ -0,0 +1,1053 @@ +/* ---------------------------------------------------------------------------- + + * 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 TestSmartProjectionPoseFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @date Sept 2013 + */ + +#include "../SmartProjectionPoseFactor.h" + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost::assign; +using namespace gtsam; + +static bool isDebugTest = false; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; + +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + +static double rankTol = 1.0; +static double linThreshold = -1.0; +static bool manageDegeneracy = true; +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +Symbol x1('X', 1); +Symbol x2('X', 2); +Symbol x3('X', 3); + +static Key poseKey1(x1); +static Point2 measurement1(323.0, 240.0); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; + +void projectToMultipleCameras( + SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, + vector& measurements_cam){ + + Point2 cam1_uv1 = cam1.project(landmark); + Point2 cam2_uv1 = cam2.project(landmark); + Point2 cam3_uv1 = cam3.project(landmark); + measurements_cam.push_back(cam1_uv1); + measurements_cam.push_back(cam2_uv1); + measurements_cam.push_back(cam3_uv1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor) { + SmartFactor::shared_ptr factor1(new SmartFactor()); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor2) { + SmartFactor factor1(rankTol, linThreshold); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor3) { + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, Constructor4) { + SmartFactor factor1(rankTol, linThreshold); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { + bool manageDegeneracy = true; + bool enableEPI = false; + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + factor1.add(measurement1, poseKey1, model, K); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, 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( SmartProjectionPoseFactor, noiseless ){ + // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K2); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 level_uv = level_camera.project(landmark); + Point2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartFactor factor1; + factor1.add(level_uv, x1, model, K); + factor1.add(level_uv_right, x2, model, K); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // test vector of errors + //Vector actual = factor1.unwhitenedError(values); + //EXPECT(assert_equal(zero(4),actual,1e-8)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noisy ){ + // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera level_camera(level_pose, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera level_camera_right(level_pose_right, *K2); + + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 pixelError(0.2,0.2); + Point2 level_uv = level_camera.project(landmark) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark); + + Values values; + values.insert(x1, level_pose); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + values.insert(x2, level_pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor1(new SmartFactor()); + factor1->add(level_uv, x1, model, K); + factor1->add(level_uv_right, x2, model, K); + + double actualError1= factor1->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor()); + vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + std::vector< SharedNoiseModel > noises; + noises.push_back(model); + noises.push_back(model); + + std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + Ks.push_back(K); + Ks.push_back(K); + + std::vector views; + views.push_back(x1); + views.push_back(x2); + + factor2->add(measurements, views, noises, Ks); + + double actualError2= factor2->error(values); + + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, model, K2); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, model, K2); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, model, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + 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( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor()); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor()); + smartFactor3->add(measurements_cam3, views, model, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ + // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // 1. Project three landmarks into three cameras and triangulate + graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + + graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + + graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3* noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + LevenbergMarquardtOptimizer optimizer(graph, values, params); + Values result = optimizer.optimize(); + + if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, CheckHessian){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + double rankTol = 10; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); + smartFactor2->add(measurements_cam2, views, model, K); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); + smartFactor3->add(measurements_cam3, views, model, K); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + + Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize(values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); + + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + + // Check Information vector + // cout << AugInformationMatrix.size() << endl; + Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + 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_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + 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( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ + // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + 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_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + 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( SmartProjectionPoseFactor, Hessian ){ + // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor()); + smartFactor1->add(measurements_cam1,views, model, K2); + + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + + boost::shared_ptr hessianFactor = smartFactor1->linearize(values); + if(isDebugTest) hessianFactor->print("Hessian factor \n"); + + // compute triangulation from linearization point + // compute reprojection errors (sum squared) + // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +} + + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotation ){ + // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose3, *K); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); + smartFactorInstance->add(measurements_cam1, views, model, K); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + // hessianFactor->print("Hessian factor \n"); + + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); + + boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + // hessianFactorRot->print("Hessian factor \n"); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); + + boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ + // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + SimpleCamera cam2(pose2, *K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + SimpleCamera cam3(pose3, *K2); + + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactor(new SmartFactor()); + smartFactor->add(measurements_cam1, views, model, K2); + + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3); + + boost::shared_ptr hessianFactor = smartFactor->linearize(values); + if(isDebugTest) hessianFactor->print("Hessian factor \n"); + + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(pose1)); + rotValues.insert(x2, poseDrift.compose(pose2)); + rotValues.insert(x3, poseDrift.compose(pose3)); + + boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); + if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(pose1)); + tranValues.insert(x2, poseDrift2.compose(pose2)); + tranValues.insert(x3, poseDrift2.compose(pose3)); + + boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { + SmartProjectionPoseFactor factor1(rankTol, linThreshold); + boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + factor1.add(measurement1, poseKey1, model, Kbundler); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3Bundler ){ + // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + PinholeCamera cam1(pose1, *Kbundler); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + PinholeCamera cam2(pose2, *Kbundler); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + PinholeCamera cam3(pose3, *Kbundler); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); + smartFactor1->add(measurements_cam1, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); + smartFactor2->add(measurements_cam2, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); + smartFactor3->add(measurements_cam3, views, model, Kbundler); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, noisePrior)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); + if(isDebugTest) tictoc_print_(); + } + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ + + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + PinholeCamera cam1(pose1, *Kbundler); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + PinholeCamera cam2(pose2, *Kbundler); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + PinholeCamera cam3(pose3, *Kbundler); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + measurements_cam1.push_back(cam3_uv1); + + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2.push_back(cam1_uv2); + measurements_cam2.push_back(cam2_uv2); + measurements_cam2.push_back(cam3_uv2); + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3.push_back(cam1_uv3); + measurements_cam3.push_back(cam2_uv3); + measurements_cam3.push_back(cam3_uv3); + + double rankTol = 10; + + SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + smartFactor1->add(measurements_cam1, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + smartFactor2->add(measurements_cam2, views, model, Kbundler); + + SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + smartFactor3->add(measurements_cam3, views, model, Kbundler); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); + Point3 positionPrior = gtsam::Point3(0,0,1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3*noise_pose); + if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionPoseFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); + std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + // EXPECT(assert_equal(pose3,result.at(x3))); + if(isDebugTest) tictoc_print_(); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + +