12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
Varun Agrawal 2021-12-21 06:35:41 -05:00 committed by GitHub
parent 598c81f38f d42044f22e
commit 86039bf282
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/geometry/SphericalCamera.h
View File

@ -43,9 +43,22 @@ class GTSAM_EXPORT EmptyCal {
EmptyCal() {} EmptyCal() {}
virtual ~EmptyCal() = default; virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>; 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 { void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl; 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: public:
enum { dimension = 6 }; enum { dimension = 6 };
typedef Unit3 Measurement; using Measurement = Unit3;
typedef std::vector<Unit3> MeasurementVector; using MeasurementVector = std::vector<Unit3>;
typedef EmptyCal CalibrationType; using CalibrationType = EmptyCal;
private: private:
Pose3 pose_; ///< 3D pose of camera Pose3 pose_; ///< 3D pose of camera
@ -83,8 +96,8 @@ class GTSAM_EXPORT SphericalCamera {
/// Constructor with empty intrinsics (needed for smart factors) /// Constructor with empty intrinsics (needed for smart factors)
explicit SphericalCamera(const Pose3& pose, explicit SphericalCamera(const Pose3& pose,
const boost::shared_ptr<EmptyCal>& cal) const EmptyCal::shared_ptr& cal)
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {} : pose_(pose), emptyCal_(cal) {}
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
@ -95,7 +108,7 @@ class GTSAM_EXPORT SphericalCamera {
virtual ~SphericalCamera() = default; virtual ~SphericalCamera() = default;
/// return shared pointer to calibration /// return shared pointer to calibration
const boost::shared_ptr<EmptyCal>& sharedCalibration() const { const EmptyCal::shared_ptr& sharedCalibration() const {
return emptyCal_; return emptyCal_;
} }
@ -213,6 +226,9 @@ class GTSAM_EXPORT SphericalCamera {
void serialize(Archive& ar, const unsigned int /*version*/) { void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(pose_); ar& BOOST_SERIALIZATION_NVP(pose_);
} }
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
// end of class SphericalCamera // end of class SphericalCamera

2
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -138,7 +138,7 @@ namespace sphericalCamera {
typedef SphericalCamera Camera; typedef SphericalCamera Camera;
typedef CameraSet<Camera> Cameras; typedef CameraSet<Camera> Cameras;
typedef SmartProjectionRigFactor<Camera> SmartFactorP; typedef SmartProjectionRigFactor<Camera> SmartFactorP;
static EmptyCal::shared_ptr emptyK; static EmptyCal::shared_ptr emptyK(new EmptyCal());
Camera level_camera(level_pose); Camera level_camera(level_pose);
Camera level_camera_right(pose_right); Camera level_camera_right(pose_right);
Camera cam1(level_pose); Camera cam1(level_pose);

2
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -1524,7 +1524,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) { TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
typedef SphericalCamera Camera; typedef SphericalCamera Camera;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor; typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static EmptyCal::shared_ptr emptyK; EmptyCal::shared_ptr emptyK(new EmptyCal());
Pose3 poseA = Pose3( Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), 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 Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame

2
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -1394,7 +1394,7 @@ typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1); Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2); Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3); 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 cam1(interp_pose1, emptyK);
Camera cam2(interp_pose2, emptyK); Camera cam2(interp_pose2, emptyK);
Camera cam3(interp_pose3, emptyK); Camera cam3(interp_pose3, emptyK);