SmartProjectionPoseFactor.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 "SmartProjectionFactor.h"

namespace gtsam {
template<class POSE, class LANDMARK, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
protected:

  LinearizationMode linearizeTo_;  

  std::vector<boost::shared_ptr<CALIBRATION> > K_all_; 

public:

  typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;

  typedef SmartProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This;

  typedef boost::shared_ptr<This> shared_ptr;

  SmartProjectionPoseFactor(const double rankTol = 1,
      const double linThreshold = -1, const bool manageDegeneracy = false,
      const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
      LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
      double dynamicOutlierRejectionThreshold = -1) :
        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
        landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}

  virtual ~SmartProjectionPoseFactor() {}

  void add(const Point2 measured_i, const Key poseKey_i,
      const SharedNoiseModel noise_i,
      const boost::shared_ptr<CALIBRATION> K_i) {
    Base::add(measured_i, poseKey_i, noise_i);
    K_all_.push_back(K_i);
  }

  void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
      std::vector<SharedNoiseModel> noises,
      std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
    Base::add(measurements, poseKeys, noises);
    for (size_t i = 0; i < measurements.size(); i++) {
      K_all_.push_back(Ks.at(i));
    }
  }

  void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
      const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
    for (size_t i = 0; i < measurements.size(); i++) {
      Base::add(measurements.at(i), poseKeys.at(i), noise);
      K_all_.push_back(K);
    }
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const {
    std::cout << s << "SmartProjectionPoseFactor, z = \n ";
    BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
    K->print("calibration = ");
    Base::print("", keyFormatter);
  }

  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
    const This *e = dynamic_cast<const This*>(&p);

    return e && Base::equals(p, tol);
  }

  virtual size_t dim() const {
    return 6 * this->keys_.size();
  }

  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<Pose3>(k);
      typename Base::Camera camera(pose, *K_all_[i++]);
      cameras.push_back(camera);
    }
    return cameras;
  }

  virtual boost::shared_ptr<GaussianFactor> linearize(
      const Values& values) const {
    // depending on flag set on construction we may linearize to different linear factors
    switch(linearizeTo_){
    case JACOBIAN_SVD :
      return this->createJacobianSVDFactor(cameras(values), 0.0);
      break;
    case JACOBIAN_Q :
      return this->createJacobianQFactor(cameras(values), 0.0);
      break;
    default:
      return this->createHessianFactor(cameras(values));
      break;
    }
  }

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

  inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
    return K_all_;
  }

private:

  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(K_all_);
  }

}; // end of class declaration

} // \ namespace gtsam