Merge pull request #673 from ToniRV/feature/clean_smart_stereo_proj_p…

…ose_factor Cleaning SmartStereoProjectionPoseFactor
borglab · Jan 18, 2021 · c383b6c · c383b6c
2 parents edcd75f + 96dc9bf
commit c383b6c
Show file tree

Hide file tree

Showing 4 changed files with 155 additions and 91 deletions.
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
@@ -13,6 +13,7 @@
  * @file   SmartFactorBase.h
  * @brief  Base class to create smart factors on poses or cameras
  * @author Luca Carlone
+ * @author Antoni Rosinol
  * @author Zsolt Kira
  * @author Frank Dellaert
  * @author Chris Beall
@@ -131,9 +132,10 @@ class SmartFactorBase: public NonlinearFactor {
   /**
    * Add a bunch of measurements, together with the camera keys
    */
-  void add(ZVector& measurements, KeyVector& cameraKeys) {
+  void add(const ZVector& measurements, const KeyVector& cameraKeys) {
+    assert(measurements.size() == cameraKeys.size());
     for (size_t i = 0; i < measurements.size(); i++) {
-      this->add(measurements.at(i), cameraKeys.at(i));
+      this->add(measurements[i], cameraKeys[i]);
     }
   }
 

diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp → .../examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp → .../examples/ISAM2_SmartFactorStereo_IMU.cpp
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   SmartStereoProjectionPoseFactor.cpp
+ * @brief  Smart stereo factor on poses, assuming camera calibration is fixed
+ * @author Luca Carlone
+ * @author Antoni Rosinol
+ * @author Chris Beall
+ * @author Zsolt Kira
+ * @author Frank Dellaert
+ */
+
+#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
+
+namespace gtsam {
+
+SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
+    const SharedNoiseModel& sharedNoiseModel,
+    const SmartStereoProjectionParams& params,
+    const boost::optional<Pose3>& body_P_sensor)
+    : Base(sharedNoiseModel, params, body_P_sensor) {}
+
+void SmartStereoProjectionPoseFactor::add(
+    const StereoPoint2& measured, const Key& poseKey,
+    const boost::shared_ptr<Cal3_S2Stereo>& K) {
+  Base::add(measured, poseKey);
+  K_all_.push_back(K);
+}
+
+void SmartStereoProjectionPoseFactor::add(
+    const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
+    const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
+  assert(measurements.size() == poseKeys.size());
+  assert(poseKeys.size() == Ks.size());
+  Base::add(measurements, poseKeys);
+  K_all_.insert(K_all_.end(), Ks.begin(), Ks.end());
+}
+
+void SmartStereoProjectionPoseFactor::add(
+    const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
+    const boost::shared_ptr<Cal3_S2Stereo>& K) {
+  assert(poseKeys.size() == measurements.size());
+  for (size_t i = 0; i < measurements.size(); i++) {
+    Base::add(measurements[i], poseKeys[i]);
+    K_all_.push_back(K);
+  }
+}
+
+void SmartStereoProjectionPoseFactor::print(
+    const std::string& s, const KeyFormatter& keyFormatter) const {
+  std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
+  for (const boost::shared_ptr<Cal3_S2Stereo>& K : K_all_) {
+    K->print("calibration = ");
+  }
+  Base::print("", keyFormatter);
+}
+
+bool SmartStereoProjectionPoseFactor::equals(const NonlinearFactor& p,
+                                             double tol) const {
+  const SmartStereoProjectionPoseFactor* e =
+      dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
+
+  return e && Base::equals(p, tol);
+}
+
+double SmartStereoProjectionPoseFactor::error(const Values& values) const {
+  if (this->active(values)) {
+    return this->totalReprojectionError(cameras(values));
+  } else {  // else of active flag
+    return 0.0;
+  }
+}
+
+SmartStereoProjectionPoseFactor::Base::Cameras
+SmartStereoProjectionPoseFactor::cameras(const Values& values) const {
+  assert(keys_.size() == K_all_.size());
+  Base::Cameras cameras;
+  for (size_t i = 0; i < keys_.size(); i++) {
+    Pose3 pose = values.at<Pose3>(keys_[i]);
+    if (Base::body_P_sensor_) {
+      pose = pose.compose(*(Base::body_P_sensor_));
+    }
+    cameras.push_back(StereoCamera(pose, K_all_[i]));
+  }
+  return cameras;
+}
+
+}  // \ namespace gtsam
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -13,6 +13,7 @@
  * @file   SmartStereoProjectionPoseFactor.h
  * @brief  Smart stereo factor on poses, assuming camera calibration is fixed
  * @author Luca Carlone
+ * @author Antoni Rosinol
  * @author Chris Beall
  * @author Zsolt Kira
  * @author Frank Dellaert
@@ -28,8 +29,9 @@ 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,
+ * 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.
  *
  */
@@ -41,14 +43,12 @@ namespace gtsam {
  * This factor requires that values contains the involved poses (Pose3).
  * @addtogroup SLAM
  */
-class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
-
-protected:
-
-  std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
-
-public:
+class SmartStereoProjectionPoseFactor : 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
@@ -65,129 +65,94 @@ class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
    * @param Isotropic measurement noise
    * @param params internal parameters of the smart factors
    */
-  SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
+  SmartStereoProjectionPoseFactor(
+      const SharedNoiseModel& sharedNoiseModel,
       const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
-      const boost::optional<Pose3> body_P_sensor = boost::none) :
-      Base(sharedNoiseModel, params, body_P_sensor) {
-  }
+      const boost::optional<Pose3>& body_P_sensor = boost::none);
 
   /** Virtual destructor */
-  virtual ~SmartStereoProjectionPoseFactor() {}
+  virtual ~SmartStereoProjectionPoseFactor() = default;
 
   /**
    * add a new measurement and pose key
-   * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
-   * @param poseKey is key corresponding to the camera observing the same landmark
+   * @param measured is the 2m dimensional location of the projection of a
+   * single landmark in the m view (the measurement)
+   * @param poseKey is key corresponding to the camera observing the same
+   * landmark
    * @param K is the (fixed) camera calibration
    */
-  void add(const StereoPoint2 measured, const Key poseKey,
-      const boost::shared_ptr<Cal3_S2Stereo> K) {
-    Base::add(measured, poseKey);
-    K_all_.push_back(K);
-  }
+  void add(const StereoPoint2& measured, const Key& poseKey,
+           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 measurement)
-   * @param poseKeys vector of keys corresponding to the camera observing the same landmark
+   * @param measurements vector of the 2m dimensional location of the projection
+   * of a single landmark in the m view (the measurement)
+   * @param poseKeys vector of keys corresponding to the camera observing
+   * the same landmark
    * @param Ks vector of calibration objects
    */
-  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
-      std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
-    Base::add(measurements, poseKeys);
-    for (size_t i = 0; i < measurements.size(); i++) {
-      K_all_.push_back(Ks.at(i));
-    }
-  }
+  void add(const std::vector<StereoPoint2>& measurements,
+           const KeyVector& poseKeys,
+           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 poseKeys vector of keys corresponding to the camera observing the same landmark
+   * 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 poseKeys vector of keys corresponding to the camera observing the
+   * same landmark
    * @param K the (known) camera calibration (same for all measurements)
    */
-  void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
-      const boost::shared_ptr<Cal3_S2Stereo> K) {
-    for (size_t i = 0; i < measurements.size(); i++) {
-      Base::add(measurements.at(i), poseKeys.at(i));
-      K_all_.push_back(K);
-    }
-  }
+  void add(const std::vector<StereoPoint2>& measurements,
+           const KeyVector& poseKeys,
+           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 {
-    std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
-    for(const boost::shared_ptr<Cal3_S2Stereo>& K: K_all_)
-    K->print("calibration = ");
-    Base::print("", keyFormatter);
-  }
+   */ void print(
+      const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /// equals
-  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
-    const SmartStereoProjectionPoseFactor *e =
-        dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
-
-    return e && Base::equals(p, tol);
-  }
+  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const;
 
   /**
    * error calculates the error of the factor.
    */
-  double error(const Values& values) const override {
-    if (this->active(values)) {
-      return this->totalReprojectionError(cameras(values));
-    } else { // else of active flag
-      return 0.0;
-    }
-  }
+  double error(const Values& values) const override;
 
   /** return the calibration object */
-  inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
+  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
+   * @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 {
-    Base::Cameras cameras;
-    size_t i=0;
-    for(const Key& k: this->keys_) {
-      Pose3 pose = values.at<Pose3>(k);
-
-      if (Base::body_P_sensor_)
-    	  pose = pose.compose(*(Base::body_P_sensor_));
-
-      StereoCamera camera(pose, K_all_[i++]);
-      cameras.push_back(camera);
-    }
-    return cameras;
-  }
-
-private:
+  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_);
+  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
+};  // end of class declaration
 
 /// traits
-template<>
-struct traits<SmartStereoProjectionPoseFactor> : public Testable<
-    SmartStereoProjectionPoseFactor> {
-};
+template <>
+struct traits<SmartStereoProjectionPoseFactor>
+    : public Testable<SmartStereoProjectionPoseFactor> {};
 
-} // \ namespace gtsam
+}  // \ namespace gtsam