From f84e1750eaf138e6fdeeb3fc85becf34b1dc76c0 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sat, 13 Mar 2021 13:50:47 -0500
Subject: [PATCH] done factor!

---
 .../slam/SmartStereoProjectionFactorPP.cpp    | 106 ++++++++++++++++++
 .../slam/SmartStereoProjectionFactorPP.h      |   6 +
 2 files changed, 112 insertions(+)
 create mode 100644 gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp

diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
new file mode 100644
index 000000000..f0fadb1c0
--- /dev/null
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
@@ -0,0 +1,106 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ */
+
+#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
+
+namespace gtsam {
+
+SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
+    const SharedNoiseModel& sharedNoiseModel,
+    const SmartStereoProjectionParams& params)
+    : Base(sharedNoiseModel, params) {} // note: no extrinsic specified!
+
+void SmartStereoProjectionFactorPP::add(
+    const StereoPoint2& measured,
+    const Key& w_P_body_key, const Key& body_P_cam_key,
+    const boost::shared_ptr<Cal3_S2Stereo>& K) {
+  // we index by cameras..
+  Base::add(measured, w_P_body_key);
+  // but we also store the extrinsic calibration keys in the same order
+  body_P_cam_keys_.push_back(body_P_cam_key);
+  K_all_.push_back(K);
+}
+
+void SmartStereoProjectionFactorPP::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) {
+  assert(measurements.size() == poseKeys.size());
+  assert(w_P_body_keys.size() == body_P_cam_keys.size());
+  assert(w_P_body_keys.size() == Ks.size());
+  // we index by cameras..
+  Base::add(measurements, w_P_body_keys);
+  // but we also store the extrinsic calibration keys in the same order
+  body_P_cam_keys_.insert(body_P_cam_keys_.end(), body_P_cam_keys.begin(), body_P_cam_keys.end());
+  K_all_.insert(K_all_.end(), Ks.begin(), Ks.end());
+}
+
+void SmartStereoProjectionFactorPP::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) {
+  assert(poseKeys.size() == measurements.size());
+  assert(w_P_body_keys.size() == body_P_cam_keys.size());
+  for (size_t i = 0; i < measurements.size(); i++) {
+    Base::add(measurements[i], w_P_body_keys[i]);
+    body_P_cam_keys_.push_back(body_P_cam_keys[i]);
+    K_all_.push_back(K);
+  }
+}
+void SmartStereoProjectionFactorPP::print(
+    const std::string& s, const KeyFormatter& keyFormatter) const {
+  std::cout << s << "SmartStereoProjectionFactorPP, z = \n ";
+  for (size_t i = 0; i < K_all_.size(); i++) {
+    K_all_[i]->print("calibration = ");
+    std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]);
+  }
+  Base::print("", keyFormatter);
+}
+
+bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p,
+                                             double tol) const {
+  const SmartStereoProjectionFactorPP* e =
+      dynamic_cast<const SmartStereoProjectionFactorPP*>(&p);
+
+  return e && Base::equals(p, tol) &&
+      body_P_cam_keys_ == p.getExtrinsicPoseKeys();
+}
+
+double SmartStereoProjectionFactorPP::error(const Values& values) const {
+  if (this->active(values)) {
+    return this->totalReprojectionError(cameras(values));
+  } else {  // else of active flag
+    return 0.0;
+  }
+}
+
+SmartStereoProjectionFactorPP::Base::Cameras
+SmartStereoProjectionFactorPP::cameras(const Values& values) const {
+  assert(keys_.size() == K_all_.size());
+  assert(keys_.size() == body_P_cam_keys_.size());
+  Base::Cameras cameras;
+  for (size_t i = 0; i < keys_.size(); i++) {
+    Pose3 w_P_body = values.at<Pose3>(keys_[i]);
+    Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_[i]);
+    w_P_cam = w_P_body.compose(body_P_cam);
+    cameras.push_back(StereoCamera(w_P_cam, K_all_[i]));
+  }
+  return cameras;
+}
+
+}  // \ namespace gtsam
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index 3486e3d5f..db992f328 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -43,6 +43,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
   /// shared pointer to calibration object (one for each camera)
   std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
 
+  /// The keys corresponding to the extrinsic pose calibration for each view
+  KeyVector body_P_cam_keys_;
+
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -115,6 +118,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
   /// equals
   bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
 
+  /// equals
+  KeyVector& getExtrinsicPoseKeys() const {return body_P_cam_keys_;};
+
   /**
    * error calculates the error of the factor.
    */