diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 770cd711d..12cc20b3e 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -60,15 +60,14 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix from the given essential matrix E * and calibration matrices Ka and Kb. * - * @tparam CAL Calibration type, expected to have a matrix() method * @param E Essential matrix * @param Ka Calibration matrix for the left camera * @param Kb Calibration matrix for the right camera */ - template - FundamentalMatrix(const CAL& Ka, const EssentialMatrix& E, const CAL& Kb) - : FundamentalMatrix(Ka.K().transpose().inverse() * E.matrix() * - Kb.K().inverse()) {} + FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E, + const Matrix3& Kb) + : FundamentalMatrix(Ka.transpose().inverse() * E.matrix() * + Kb.inverse()) {} /** * @brief Construct from calibration matrices Ka, Kb, and pose aPb @@ -76,13 +75,11 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix from the given calibration * matrices Ka and Kb, and the pose aPb. * - * @tparam CAL Calibration type, expected to have a matrix() method * @param Ka Calibration matrix for the left camera * @param aPb Pose from the left to the right camera * @param Kb Calibration matrix for the right camera */ - template - FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) + FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb) : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {} /// Return the fundamental matrix representation diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 13cdcf70d..671965064 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -913,10 +913,10 @@ class FundamentalMatrix { FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types - FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::EssentialMatrix& E, - const gtsam::Cal3_S2& Kb); - FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, - const gtsam::Cal3_S2& Kb); + FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::EssentialMatrix& E, + const gtsam::Matrix3& Kb); + FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::Pose3& aPb, + const gtsam::Matrix3& Kb); // Methods gtsam::Matrix3 matrix() const; @@ -1066,6 +1066,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3f; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; #include