SmartStereoProjectionFactor.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/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam_unstable/dllexport.h>

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/optional.hpp>
#endif

#include <optional>
#include <vector>

namespace gtsam {

/*
 *  Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
 */
typedef SmartProjectionParams SmartStereoProjectionParams;

class SmartStereoProjectionFactor
    : public SmartFactorBase<StereoCamera> {
 private:

  typedef SmartFactorBase<StereoCamera> Base;

protected:

  const SmartStereoProjectionParams params_;

  mutable TriangulationResult result_; 
  mutable std::vector<Pose3> cameraPosesTriangulation_; 

public:

  typedef std::shared_ptr<SmartStereoProjectionFactor> shared_ptr;

  typedef CameraSet<StereoCamera> Cameras;

  typedef PinholeCamera<Cal3_S2> MonoCamera;
  typedef CameraSet<MonoCamera> MonoCameras;
  typedef MonoCamera::MeasurementVector MonoMeasurements;

  SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
      const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
      const std::optional<Pose3> body_P_sensor = {}) :
      Base(sharedNoiseModel, body_P_sensor), //
      params_(params), //
      result_(TriangulationResult::Degenerate()) {
  }

  ~SmartStereoProjectionFactor() override {
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const override {
    std::cout << s << "SmartStereoProjectionFactor\n";
    std::cout << "linearizationMode:\n" << 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 SmartStereoProjectionFactor *e =
        dynamic_cast<const SmartStereoProjectionFactor*>(&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;

    // 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],
            params_.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
  }

//  /// triangulateSafe
//  size_t triangulateSafe(const Values& values) const {
//    return triangulateSafe(this->cameras(values));
//  }

  TriangulationResult triangulateSafe(const Cameras& cameras) const {

    size_t m = cameras.size();
    bool retriangulate = decideIfTriangulate(cameras);

    // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
    MonoCameras monoCameras;
    MonoMeasurements monoMeasured;
    for(size_t i = 0; i < m; i++) {
      const Pose3 leftPose = cameras[i].pose();
      const Cal3_S2 monoCal = cameras[i].calibration().calibration();
      const MonoCamera leftCamera_i(leftPose,monoCal);
      const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0));
      const Pose3 rightPose = leftPose.compose( left_Pose_right );
      const MonoCamera rightCamera_i(rightPose,monoCal);
      const StereoPoint2 zi = measured_[i];
      monoCameras.push_back(leftCamera_i);
      monoMeasured.push_back(Point2(zi.uL(),zi.v()));
      if(!std::isnan(zi.uR())){ // if right point is valid
        monoCameras.push_back(rightCamera_i);
        monoMeasured.push_back(Point2(zi.uR(),zi.v()));
      }
    }
    if (retriangulate)
      result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
          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("SmartStereoProjectionHessianFactor: 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().
    Base::FBlocks Fs;
    Matrix F, 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 factor
//  std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > 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<StereoCamera> >();
//  }
//
//  /// create factor
//  std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > 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, Base::ZDim> >(this->keys_);
//  }
//
//  /// Create a factor, takes values
//  std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > 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
      return std::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
  }

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

//  /// linearize to an Implicit Schur factor
//  virtual std::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
//      const Values& values, double lambda = 0.0) const {
//    return createRegularImplicitSchurFactor(this->cameras(values), lambda);
//  }
//
//  /// linearize to a JacobianfactorQ
//  virtual std::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > 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("SmartStereoFactorlinearize: 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(
      FBlocks& Fs,
      Matrix& E, Vector& b,
      const Cameras& cameras) const {

    if (!result_) {
      throw ("computeJacobiansWithTriangulatedPoint");
//      // 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(
      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(
      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() * Base::ZDim);
  }

  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) {
      throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
//      // Otherwise, manage the exceptions with rotation-only factors
//      const StereoPoint2& z0 = this->measured_.at(0);
//      Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
//
//      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(Base::cameras(values));
    } else { // else of active flag
      return 0.0;
    }
  }

  void correctForMissingMeasurements(
      const Cameras& cameras, Vector& ue,
      typename Cameras::FBlocks* Fs = nullptr,
      Matrix* E = nullptr) const override {
    // when using stereo cameras, some of the measurements might be missing:
    for (size_t i = 0; i < cameras.size(); i++) {
      const StereoPoint2& z = measured_.at(i);
      if (std::isnan(z.uR()))  // if the right 2D measurement is invalid
      {
        if (Fs) {  // delete influence of right point on jacobian Fs
          MatrixZD& Fi = Fs->at(i);
          for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
        }
        if (E)  // delete influence of right point on jacobian E
          E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());

        // set the corresponding entry of vector ue to zero
        ue(ZDim * i + 1) = 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:

#ifdef 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_.throwCheirality);
    ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
  }
#endif
};

template<>
struct traits<SmartStereoProjectionFactor > : public Testable<
    SmartStereoProjectionFactor> {
};

} // \ namespace gtsam