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; + + /// 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 + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "EmptyCal", boost::serialization::base_object(*this)); + } }; /** @@ -58,9 +71,9 @@ class GTSAM_EXPORT SphericalCamera { public: enum { dimension = 6 }; - typedef Unit3 Measurement; - typedef std::vector MeasurementVector; - typedef EmptyCal CalibrationType; + using Measurement = Unit3; + using MeasurementVector = std::vector; + 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& cal) - : pose_(pose), emptyCal_(boost::make_shared()) {} + 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& 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 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 Cameras; typedef SmartProjectionRigFactor 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 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 SmartFactorRS_spherical; Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); Pose3 interp_pose3 = interpolate(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);