created .h

gtsam_unstable/slam/SmartStereoProjectionFactorPP.h

@@ -0,0 +1,153 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   SmartStereoProjectionFactorPP.h
+ * @brief  Smart stereo factor on poses and extrinsic calibration
+ * @author Luca Carlone
+ * @author Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
+
+namespace gtsam {
+/**
+ *
+ * @addtogroup SLAM
+ *
+ * If you are using the factor, please cite:
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
+ * Eliminating conditionally independent sets in factor graphs:
+ * a unifying perspective based on smart factors,
+ * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ */
+
+/**
+ * This factor optimizes the extrinsic camera calibration (pose of camera wrt body),
+ * and each camera has its own extrinsic calibration.
+ * This factor requires that values contain the involved poses and extrinsics (both Pose3).
+ * @addtogroup SLAM
+ */
+class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
+ protected:
+  /// shared pointer to calibration object (one for each camera)
+  std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// shorthand for base class type
+  typedef SmartStereoProjectionFactor Base;
+
+  /// shorthand for this class
+  typedef SmartStereoProjectionFactorPP This;
+
+  /// shorthand for a smart pointer to a factor
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /**
+   * Constructor
+   * @param Isotropic measurement noise
+   * @param params internal parameters of the smart factors
+   */
+  SmartStereoProjectionFactorPP(
+      const SharedNoiseModel& sharedNoiseModel,
+      const SmartStereoProjectionParams& params = SmartStereoProjectionParams());
+
+  /** Virtual destructor */
+  ~SmartStereoProjectionFactorPP() override = default;
+
+  /**
+   * add a new measurement, with a pose key, and an extrinsic pose key
+   * @param measured is the 3-dimensional location of the projection of a
+   * single landmark in the a single view (the measurement)
+   * @param w_P_body_key is key corresponding to the camera observing the same landmark
+   * @param body_P_cam_key is key corresponding to the camera observing the same landmark
+   * @param K is the (fixed) camera calibration
+   */
+  void add(const StereoPoint2& measured,
+           const Key& w_P_body_key, const Key& body_P_cam_key,
+           const boost::shared_ptr<Cal3_S2Stereo>& K);
+
+  /**
+   *  Variant of the previous one in which we include a set of measurements
+   * @param measurements vector of the 2m dimensional location of the projection
+   * of a single landmark in the m view (the measurements)
+   * @param w_P_body_keys are the ordered keys corresponding to the camera observing the same landmark
+   * @param body_P_cam_keys are the ordered keys corresponding to the camera observing the same landmark
+   * @param Ks vector of calibration objects
+   */
+  void add(const std::vector<StereoPoint2>& measurements,
+           const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
+           const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);
+
+  /**
+   * Variant of the previous one in which we include a set of measurements with
+   * the same noise and calibration
+   * @param measurements vector of the 2m dimensional location of the projection
+   * of a single landmark in the m view (the measurement)
+   * @param w_P_body_keys are the ordered keys corresponding to the camera observing the same landmark
+   * @param body_P_cam_keys are the ordered keys corresponding to the camera observing the same landmark
+   * @param K the (known) camera calibration (same for all measurements)
+   */
+  void add(const std::vector<StereoPoint2>& measurements,
+           const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
+           const boost::shared_ptr<Cal3_S2Stereo>& K);
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   * @param keyFormatter optional formatter useful for printing Symbols
+   */
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+                                            DefaultKeyFormatter) const override;
+
+  /// equals
+  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
+
+  /**
+   * error calculates the error of the factor.
+   */
+  double error(const Values& values) const override;
+
+  /** return the calibration object */
+  inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const {
+    return K_all_;
+  }
+
+  /**
+   * Collect all cameras involved in this factor
+   * @param values Values structure which must contain camera poses
+   * corresponding
+   * to keys involved in this factor
+   * @return vector of Values
+   */
+  Base::Cameras cameras(const Values& values) const override;
+
+ private:
+  /// Serialization function
+  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
+
+/// traits
+template <>
+struct traits<SmartStereoProjectionFactorPP>
+    : public Testable<SmartStereoProjectionFactorPP> {};
+
+}  // namespace gtsam