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

namespace gtsam {
template<class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<
    PinholePose<CALIBRATION> > {

private:
  typedef PinholePose<CALIBRATION> Camera;
  typedef SmartProjectionFactor<Camera> Base;
  typedef SmartProjectionPoseFactor<CALIBRATION> This;

protected:

  boost::shared_ptr<CALIBRATION> K_; 

public:

  typedef boost::shared_ptr<This> shared_ptr;

  SmartProjectionPoseFactor() {}

  SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
      const boost::shared_ptr<CALIBRATION> K,
      const boost::optional<Pose3> body_P_sensor = boost::none,
      const SmartProjectionParams& params = SmartProjectionParams()) :
      Base(sharedNoiseModel, body_P_sensor, params), K_(K) {
  }

  virtual ~SmartProjectionPoseFactor() {
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const {
    std::cout << s << "SmartProjectionPoseFactor, z = \n ";
    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 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 boost::shared_ptr<CALIBRATION> calibration() const {
    return K_;
  }

  typename Base::Cameras cameras(const Values& values) const {
    typename Base::Cameras cameras;
    for(const Key& k: this->keys_) {
      Pose3 pose = values.at<Pose3>(k);
      if (Base::body_P_sensor_)
        pose = pose.compose(*(Base::body_P_sensor_));

      Camera camera(pose, K_);
      cameras.push_back(camera);
    }
    return cameras;
  }

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_);
  }

};
// end of class declaration

template<class CALIBRATION>
struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
    SmartProjectionPoseFactor<CALIBRATION> > {
};

} // \ namespace gtsam