SmartProjectionRigFactor.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/slam/SmartProjectionFactor.h>

namespace gtsam {
template <class CAMERA>
class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
 private:
  typedef SmartProjectionFactor<CAMERA> Base;
  typedef SmartProjectionRigFactor<CAMERA> This;
  typedef typename CAMERA::CalibrationType CALIBRATION;
  typedef typename CAMERA::Measurement MEASUREMENT;
  typedef typename CAMERA::MeasurementVector MEASUREMENTS;

  static const int DimPose = 6;  
  static const int ZDim = 2;     

 protected:
  KeyVector nonUniqueKeys_;

  std::shared_ptr<typename Base::Cameras> cameraRig_;

  FastVector<size_t> cameraIds_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef CAMERA Camera;
  typedef CameraSet<CAMERA> Cameras;

  typedef std::shared_ptr<This> shared_ptr;

  SmartProjectionRigFactor() {}

  SmartProjectionRigFactor(
      const SharedNoiseModel& sharedNoiseModel,
      const std::shared_ptr<Cameras>& cameraRig,
      const SmartProjectionParams& params = SmartProjectionParams())
      : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
    // throw exception if configuration is not supported by this factor
    if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
      throw std::runtime_error(
          "SmartProjectionRigFactor: "
          "degeneracyMode must be set to ZERO_ON_DEGENERACY");
    if (Base::params_.linearizationMode != gtsam::HESSIAN)
      throw std::runtime_error(
          "SmartProjectionRigFactor: "
          "linearizationMode must be set to HESSIAN");
  }

  void add(const MEASUREMENT& measured, const Key& poseKey,
           const size_t& cameraId = 0) {
    // store measurement and key
    this->measured_.push_back(measured);
    this->nonUniqueKeys_.push_back(poseKey);

    //  also store keys in the keys_ vector: these keys are assumed to be
    //  unique, so we avoid duplicates here
    if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
        this->keys_.end())
      this->keys_.push_back(poseKey);  // add only unique keys

    // store id of the camera taking the measurement
    cameraIds_.push_back(cameraId);
  }

  void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
           const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
    if (poseKeys.size() != measurements.size() ||
        (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
      throw std::runtime_error(
          "SmartProjectionRigFactor: "
          "trying to add inconsistent inputs");
    }
    if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
      throw std::runtime_error(
          "SmartProjectionRigFactor: "
          "camera rig includes multiple camera "
          "but add did not input cameraIds");
    }
    for (size_t i = 0; i < measurements.size(); i++) {
      add(measurements[i], poseKeys[i],
          cameraIds.size() == 0 ? 0 : cameraIds[i]);
    }
  }

  const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }

  const std::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }

  const FastVector<size_t>& cameraIds() const { return cameraIds_; }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << "SmartProjectionRigFactor: \n ";
    for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
      std::cout << "-- Measurement nr " << i << std::endl;
      std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
      std::cout << "cameraId: " << cameraIds_[i] << std::endl;
      (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
    }
    Base::print("", keyFormatter);
  }

  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&p);
    return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
           cameraRig_->equals(*(e->cameraRig())) &&
           std::equal(cameraIds_.begin(), cameraIds_.end(),
                      e->cameraIds().begin());
  }

  typename Base::Cameras cameras(const Values& values) const override {
    typename Base::Cameras cameras;
    cameras.reserve(nonUniqueKeys_.size());  // preallocate
    for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
      const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
      const Pose3 world_P_sensor_i =
          values.at<Pose3>(nonUniqueKeys_[i])  // = world_P_body
          * camera_i.pose();                   // = body_P_cam_i
      cameras.emplace_back(world_P_sensor_i,
                           make_shared<typename CAMERA::CalibrationType>(
                               camera_i.calibration()));
    }
    return cameras;
  }

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

  void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
                                             Matrix& E, Vector& b,
                                             const Cameras& cameras) const {
    if (!this->result_) {
      throw("computeJacobiansWithTriangulatedPoint");
    } else {  // valid result: compute jacobians
      b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
      for (size_t i = 0; i < Fs.size(); i++) {
        const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
        const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
        Eigen::Matrix<double, DimPose, DimPose> H;
        world_P_body.compose(body_P_sensor, H);
        Fs.at(i) = Fs.at(i) * H;
      }
    }
  }

  std::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
      const Values& values, const double& lambda = 0.0,
      bool diagonalDamping = false) const {
    // we may have multiple observation sharing the same keys (e.g., 2 cameras
    // measuring from the same body pose), hence the number of unique keys may
    // be smaller than nrMeasurements
    size_t nrUniqueKeys =
        this->keys_
            .size();  // note: by construction, keys_ only contains unique keys

    Cameras cameras = this->cameras(values);

    // Create structures for Hessian Factors
    std::vector<size_t> js;
    std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
    std::vector<Vector> gs(nrUniqueKeys);

    if (this->measured_.size() != cameras.size())  // 1 observation per camera
      throw std::runtime_error(
          "SmartProjectionRigFactor: "
          "measured_.size() inconsistent with input");

    // triangulate 3D point at given linearization point
    this->triangulateSafe(cameras);

    if (!this->result_) {  // failed: return "empty/zero" Hessian
      if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
        for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
        for (Vector& v : gs) v = Vector::Zero(DimPose);
        return std::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
                                                                  Gs, gs, 0.0);
      } else {
        throw std::runtime_error(
            "SmartProjectionRigFactor: "
            "only supported degeneracy mode is ZERO_ON_DEGENERACY");
      }
    }

    // compute Jacobian given triangulated 3D Point
    typename Base::FBlocks Fs;
    Matrix E;
    Vector b;
    this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);

    // Whiten using noise model
    this->noiseModel_->WhitenSystem(E, b);
    for (size_t i = 0; i < Fs.size(); i++) {
      Fs[i] = this->noiseModel_->Whiten(Fs[i]);
    }

    const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);

    // Build augmented Hessian (with last row/column being the information
    // vector) Note: we need to get the augumented hessian wrt the unique keys
    // in key_
    SymmetricBlockMatrix augmentedHessianUniqueKeys =
        Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
            Fs, E, P, b, nonUniqueKeys_, this->keys_);

    return std::make_shared<RegularHessianFactor<DimPose> >(
        this->keys_, augmentedHessianUniqueKeys);
  }

  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
    switch (this->params_.linearizationMode) {
      case HESSIAN:
        return this->createHessianFactor(values, lambda);
      default:
        throw std::runtime_error(
            "SmartProjectionRigFactor: unknown linearization mode");
    }
  }

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

 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(nonUniqueKeys_);
    // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
    // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
  }
#endif
};
// end of class declaration

template <class CAMERA>
struct traits<SmartProjectionRigFactor<CAMERA> >
    : public Testable<SmartProjectionRigFactor<CAMERA> > {};

}  // namespace gtsam