Merge pull request #976 from borglab/fix/spherical-camera

borglab · Dec 21, 2021 · 86039bf · 86039bf
2 parents 598c81f + d42044f
commit 86039bf
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 9 deletions.
diff --git 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
 

diff --git 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
@@ -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
@@ -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);