From 8ddfd8135b78400be7bbb67c092559892c6e4539 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 20 Dec 2021 21:19:26 -0500
Subject: [PATCH 1/2] use passed in calibration for initialization and add
 EmptyCal serialization

---
 gtsam/geometry/SphericalCamera.h | 28 ++++++++++++++++++++++------
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h
index 59658f3ce..4880423d3 100644
--- a/gtsam/geometry/SphericalCamera.h
+++ b/gtsam/geometry/SphericalCamera.h
@@ -43,9 +43,22 @@ class GTSAM_EXPORT EmptyCal {
   EmptyCal() {}
   virtual ~EmptyCal() = default;
   using shared_ptr = boost::shared_ptr<EmptyCal>;
+
+  /// return DOF, dimensionality of tangent space
+  inline static size_t Dim() { return dimension; }
+
   void print(const std::string& s) const {
     std::cout << "empty calibration: " << s << std::endl;
   }
+
+ private:
+  /// Serialization function
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
+  }
 };
 
 /**
@@ -58,9 +71,9 @@ class GTSAM_EXPORT SphericalCamera {
  public:
   enum { dimension = 6 };
 
-  typedef Unit3 Measurement;
-  typedef std::vector<Unit3> MeasurementVector;
-  typedef EmptyCal CalibrationType;
+  using Measurement = Unit3;
+  using MeasurementVector = std::vector<Unit3>;
+  using CalibrationType = EmptyCal;
 
  private:
   Pose3 pose_;  ///< 3D pose of camera
@@ -83,8 +96,8 @@ class GTSAM_EXPORT SphericalCamera {
 
   /// Constructor with empty intrinsics (needed for smart factors)
   explicit SphericalCamera(const Pose3& pose,
-                           const boost::shared_ptr<EmptyCal>& cal)
-      : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
+                           const EmptyCal::shared_ptr& cal)
+      : pose_(pose), emptyCal_(cal) {}
 
   /// @}
   /// @name Advanced Constructors
@@ -95,7 +108,7 @@ class GTSAM_EXPORT SphericalCamera {
   virtual ~SphericalCamera() = default;
 
   /// return shared pointer to calibration
-  const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
+  const EmptyCal::shared_ptr& sharedCalibration() const {
     return emptyCal_;
   }
 
@@ -213,6 +226,9 @@ class GTSAM_EXPORT SphericalCamera {
   void serialize(Archive& ar, const unsigned int /*version*/) {
     ar& BOOST_SERIALIZATION_NVP(pose_);
   }
+
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class SphericalCamera
 

From d42044f22e414cf5428985caf8e9513c9ffd92f9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 20 Dec 2021 21:19:54 -0500
Subject: [PATCH 2/2] initialize all EmptyCal shared pointers

---
 gtsam/slam/tests/smartFactorScenarios.h                         | 2 +-
 gtsam/slam/tests/testSmartProjectionRigFactor.cpp               | 2 +-
 .../slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp  | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h
index 310dbe79e..66be08c67 100644
--- a/gtsam/slam/tests/smartFactorScenarios.h
+++ b/gtsam/slam/tests/smartFactorScenarios.h
@@ -138,7 +138,7 @@ namespace sphericalCamera {
 typedef SphericalCamera Camera;
 typedef CameraSet<Camera> Cameras;
 typedef SmartProjectionRigFactor<Camera> SmartFactorP;
-static EmptyCal::shared_ptr emptyK;
+static EmptyCal::shared_ptr emptyK(new EmptyCal());
 Camera level_camera(level_pose);
 Camera level_camera_right(pose_right);
 Camera cam1(level_pose);
diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
index 7f65a3c33..b4876b27e 100644
--- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp
@@ -1524,7 +1524,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
 TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
   typedef SphericalCamera Camera;
   typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
-  static EmptyCal::shared_ptr emptyK;
+  EmptyCal::shared_ptr emptyK(new EmptyCal());
   Pose3 poseA = Pose3(
       Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
       Point3(0.0, 0.0, 0.0));  // with z pointing along x axis of global frame
diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
index d7e72d129..b5962d777 100644
--- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
+++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
@@ -1394,7 +1394,7 @@ typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
 Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
 Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
 Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
-static EmptyCal::shared_ptr emptyK;
+static EmptyCal::shared_ptr emptyK(new EmptyCal());
 Camera cam1(interp_pose1, emptyK);
 Camera cam2(interp_pose2, emptyK);
 Camera cam3(interp_pose3, emptyK);