SmartStereoProjectionPoseFactor.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_unstable/slam/SmartStereoProjectionFactor.h>

namespace gtsam {
class SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor {
 protected:
  std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef SmartStereoProjectionFactor Base;

  typedef SmartStereoProjectionPoseFactor This;

  typedef boost::shared_ptr<This> shared_ptr;

  SmartStereoProjectionPoseFactor(
      const SharedNoiseModel& sharedNoiseModel,
      const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
      const boost::optional<Pose3>& body_P_sensor = boost::none);

  ~SmartStereoProjectionPoseFactor() override = default;

  void add(const StereoPoint2& measured, const Key& poseKey,
           const boost::shared_ptr<Cal3_S2Stereo>& K);

  void add(const std::vector<StereoPoint2>& measurements,
           const KeyVector& poseKeys,
           const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);

  void add(const std::vector<StereoPoint2>& measurements,
           const KeyVector& poseKeys,
           const boost::shared_ptr<Cal3_S2Stereo>& K);

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
                                            DefaultKeyFormatter) const override;

  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;

  double error(const Values& values) const override;

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

  Base::Cameras cameras(const Values& values) const override;

 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

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

}  // namespace gtsam