SmartProjectionFactorBase.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/geometry/triangulation.h>
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
 
#include <optional>
#include <vector>
 
namespace gtsam {

template <class CAMERA>
class SmartProjectionFactorBase : public SmartFactorBase<CAMERA> {
 private:
  typedef SmartFactorBase<CAMERA> Base;
  typedef SmartProjectionFactorBase<CAMERA> This;
 
 protected:
  SmartProjectionParams params_;

  mutable TriangulationResult result_;  
  mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3>>
      cameraPosesTriangulation_;  
 
 public:
  typedef std::shared_ptr<This> shared_ptr;

  typedef CAMERA Camera;
  typedef CameraSet<CAMERA> Cameras;

  using HessianFactorType = RegularHessianFactor<Base::Dim>;
  using JacobianFactorType = JacobianFactorQ<Base::Dim, 2>;
  using SharedHessianFactor = std::shared_ptr<HessianFactorType>;
  using SharedJacobianFactor = std::shared_ptr<JacobianFactorType>;

  SmartProjectionFactorBase() {}

  SmartProjectionFactorBase(
      const SharedNoiseModel& sharedNoiseModel,
      const SmartProjectionParams& params = SmartProjectionParams())
      : Base(sharedNoiseModel),
        params_(params),
        result_(TriangulationResult::Degenerate()) {}

  ~SmartProjectionFactorBase() override {}

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << "SmartProjectionFactor\n";
    std::cout << "linearizationMode: " << params_.linearizationMode
              << std::endl;
    std::cout << "triangulationParameters:\n"
              << params_.triangulation << std::endl;
    std::cout << "result:\n" << result_ << std::endl;
    Base::print("", keyFormatter);
  }

  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&p);
    return e && params_.linearizationMode == e->params_.linearizationMode &&
           Base::equals(p, tol);
  }

  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;
 
    // Definitely true 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;
 
    // Otherwise, check poses against cache.
    if (!retriangulate) {
      for (size_t i = 0; i < cameras.size(); i++) {
        if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
                                      params_.retriangulationThreshold)) {
          retriangulate =
              true;  // at least two poses are different, hence we retriangulate
          break;
        }
      }
    }
 
    // Store the current poses used for triangulation if we will re-triangulate.
    if (retriangulate) {
      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;
  }

  TriangulationResult triangulateSafe(const Cameras& cameras) const {
    size_t m = cameras.size();
    if (m < 2)  // if we have a single pose the corresponding factor is
                // uninformative
      return TriangulationResult::Degenerate();
 
    bool retriangulate = decideIfTriangulate(cameras);
    if (retriangulate)
      result_ = gtsam::triangulateSafe(cameras, this->measured_,
                                       params_.triangulation);
    return result_;
  }

  bool triangulateForLinearize(const Cameras& cameras) const {
    triangulateSafe(cameras);  // imperative, might reset result_
    return bool(result_);
  }

  std::shared_ptr<RegularHessianFactor<Base::Dim>> createHessianFactor(
      const Cameras& cameras, const double _lambda = 0.0,
      bool diagonalDamping = false) const {
    size_t numKeys = this->keys_.size();
    // Create structures for Hessian Factors
    KeyVector js;
    std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
    std::vector<Vector> gs(numKeys);
 
    if (this->measured_.size() != cameras.size())
      throw std::runtime_error(
          "SmartProjectionHessianFactor: this->measured_"
          ".size() inconsistent with input");
 
    triangulateSafe(cameras);
 
    if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
      // failed: return"empty" Hessian
      for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
      for (Vector& v : gs) v = Vector::Zero(Base::Dim);
      return std::make_shared<RegularHessianFactor<Base::Dim>>(this->keys_, Gs,
                                                               gs, 0.0);
    }
 
    // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
    typename Base::FBlocks Fs;
    Matrix E;
    Vector b;
    computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
 
    // Whiten using noise model
    Base::whitenJacobians(Fs, E, b);
 
    // build augmented hessian
    SymmetricBlockMatrix augmentedHessian =  //
        Cameras::SchurComplement(Fs, E, b, _lambda, diagonalDamping);
 
    return std::make_shared<RegularHessianFactor<Base::Dim>>(this->keys_,
                                                             augmentedHessian);
  }
 
  // Create RegularImplicitSchurFactor factor.
  std::shared_ptr<RegularImplicitSchurFactor<CAMERA>>
  createRegularImplicitSchurFactor(const Cameras& cameras,
                                   double _lambda) const {
    if (triangulateForLinearize(cameras))
      return Base::createRegularImplicitSchurFactor(cameras, *result_, _lambda);
    else
      // failed: return empty
      return std::shared_ptr<RegularImplicitSchurFactor<CAMERA>>();
  }

  std::shared_ptr<JacobianFactorQ<Base::Dim, 2>> createJacobianQFactor(
      const Cameras& cameras, double _lambda) const {
    if (triangulateForLinearize(cameras))
      return Base::createJacobianQFactor(cameras, *result_, _lambda);
    else
      // failed: return empty
      return std::make_shared<JacobianFactorQ<Base::Dim, 2>>(this->keys_);
  }

  std::shared_ptr<JacobianFactorQ<Base::Dim, 2>> createJacobianQFactor(
      const Values& values, double _lambda) const {
    return createJacobianQFactor(this->cameras(values), _lambda);
  }

  std::shared_ptr<JacobianFactor> createJacobianSVDFactor(
      const Cameras& cameras, double _lambda) const {
    if (triangulateForLinearize(cameras))
      return Base::createJacobianSVDFactor(cameras, *result_, _lambda);
    else
      // failed: return empty
      return std::make_shared<JacobianFactorSVD<Base::Dim, 2>>(this->keys_);
  }

  virtual std::shared_ptr<RegularHessianFactor<Base::Dim>> linearizeToHessian(
      const Values& values, double _lambda = 0.0) const {
    return createHessianFactor(this->cameras(values), _lambda);
  }

  virtual std::shared_ptr<RegularImplicitSchurFactor<CAMERA>>
  linearizeToImplicit(const Values& values, double _lambda = 0.0) const {
    return createRegularImplicitSchurFactor(this->cameras(values), _lambda);
  }

  virtual std::shared_ptr<JacobianFactorQ<Base::Dim, 2>> linearizeToJacobian(
      const Values& values, double _lambda = 0.0) const {
    return createJacobianQFactor(this->cameras(values), _lambda);
  }

  std::shared_ptr<GaussianFactor> linearizeDamped(
      const Cameras& cameras, const double _lambda = 0.0) const {
    // depending on flag set on construction we may linearize to different
    // linear factors
    switch (params_.linearizationMode) {
      case HESSIAN:
        return createHessianFactor(cameras, _lambda);
      case IMPLICIT_SCHUR:
        return createRegularImplicitSchurFactor(cameras, _lambda);
      case JACOBIAN_SVD:
        return createJacobianSVDFactor(cameras, _lambda);
      case JACOBIAN_Q:
        return createJacobianQFactor(cameras, _lambda);
      default:
        throw std::runtime_error("SmartFactorlinearize: unknown mode");
    }
  }

  std::shared_ptr<GaussianFactor> linearizeDamped(
      const Values& values, const double _lambda = 0.0) const {
    // depending on flag set on construction we may linearize to different
    // linear factors
    Cameras cameras = this->cameras(values);
    return linearizeDamped(cameras, _lambda);
  }

  std::shared_ptr<GaussianFactor> linearize(
      const Values& values) const override {
    return linearizeDamped(values);
  }

  bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
    bool nonDegenerate = triangulateForLinearize(cameras);
    if (nonDegenerate) {
      cameras.project2(*result_, nullptr, &E);
    }
    return nonDegenerate;
  }

  bool triangulateAndComputeE(Matrix& E, const Values& values) const {
    Cameras cameras = this->cameras(values);
    return triangulateAndComputeE(E, cameras);
  }

  void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
                                             Matrix& E, Vector& b,
                                             const Cameras& cameras) const {
    if (!result_) {
      // Handle degeneracy
      // TODO check flag whether we should do this
      Unit3 backProjected =
          cameras[0].backprojectPointAtInfinity(this->measured_.at(0));
      Base::computeJacobians(Fs, E, b, cameras, backProjected);
    } else {
      // valid result: just return Base version
      Base::computeJacobians(Fs, E, b, cameras, *result_);
    }
  }

  bool triangulateAndComputeJacobians(typename Base::FBlocks& Fs, Matrix& E,
                                      Vector& b, const Values& values) const {
    Cameras cameras = this->cameras(values);
    bool nonDegenerate = triangulateForLinearize(cameras);
    if (nonDegenerate) computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
    return nonDegenerate;
  }

  bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks& Fs,
                                         Matrix& Enull, Vector& b,
                                         const Values& values) const {
    Cameras cameras = this->cameras(values);
    bool nonDegenerate = triangulateForLinearize(cameras);
    if (nonDegenerate)
      Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
    return nonDegenerate;
  }

  Vector reprojectionErrorAfterTriangulation(const Values& values) const {
    Cameras cameras = this->cameras(values);
    bool nonDegenerate = triangulateForLinearize(cameras);
    if (nonDegenerate)
      return Base::unwhitenedError(cameras, *result_);
    else
      return Vector::Zero(cameras.size() * 2);
  }

  double totalReprojectionError(
      const Cameras& cameras, std::optional<Point3> externalPoint = {}) const {
    if (externalPoint)
      result_ = TriangulationResult(*externalPoint);
    else
      result_ = triangulateSafe(cameras);
 
    if (result_)
      // All good, just use version in base class
      return Base::totalReprojectionError(cameras, *result_);
    else if (params_.degeneracyMode == HANDLE_INFINITY) {
      // Otherwise, manage the exceptions with rotation-only factors
      Unit3 backprojected =
          cameras.front().backprojectPointAtInfinity(this->measured_.at(0));
      return Base::totalReprojectionError(cameras, backprojected);
    } else
      // if we don't want to manage the exceptions we discard the factor
      return 0.0;
  }

  double error(const Values& values) const override {
    if (this->active(values)) {
      return totalReprojectionError(this->cameras(values));
    } else {  // else of active flag
      return 0.0;
    }
  }

  TriangulationResult point() const { return result_; }

  TriangulationResult point(const Values& values) const {
    Cameras cameras = this->cameras(values);
    return triangulateSafe(cameras);
  }

  bool isValid() const { return result_.valid(); }

  bool isDegenerate() const { return result_.degenerate(); }

  bool isPointBehindCamera() const { return result_.behindCamera(); }

  bool isOutlier() const { return result_.outlier(); }

  bool isFarPoint() const { return result_.farPoint(); }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION  
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    ar& BOOST_SERIALIZATION_NVP(params_);
    ar& BOOST_SERIALIZATION_NVP(result_);
    ar& BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
  }
#endif
};

template <class CAMERA>
struct traits<SmartProjectionFactorBase<CAMERA>>
    : public Testable<SmartProjectionFactorBase<CAMERA>> {};
 
}  // namespace gtsam