From 8d9ede82853eb20230d72f03e02dd0e17c1ea86e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 20:35:18 -0400 Subject: [PATCH] reverted all changes back to master --- gtsam/slam/SmartProjectionPoseFactor.h | 35 ++++++++++++-------------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 8731eaec4..c7b1d5424 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionFactorP.h + * @file SmartProjectionPoseFactor.h * @brief Smart factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall @@ -41,14 +41,14 @@ namespace gtsam { * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM */ -template -class SmartProjectionFactorP: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { private: - typedef SmartProjectionFactor Base; - typedef SmartProjectionFactorP This; - typedef CAMERA Camera; - typedef typename CAMERA::CalibrationType CALIBRATION; + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; protected: @@ -62,7 +62,7 @@ public: /** * Default constructor, only for serialization */ - SmartProjectionFactorP() {} + SmartProjectionPoseFactor() {} /** * Constructor @@ -70,7 +70,7 @@ public: * @param K (fixed) calibration, assumed to be the same for all cameras * @param params parameters for the smart projection factors */ - SmartProjectionFactorP( + SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const SmartProjectionParams& params = SmartProjectionParams()) @@ -84,17 +84,17 @@ public: * @param body_P_sensor pose of the camera in the body frame (optional) * @param params parameters for the smart projection factors */ - SmartProjectionFactorP( + SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const boost::optional body_P_sensor, const SmartProjectionParams& params = SmartProjectionParams()) - : SmartProjectionFactorP(sharedNoiseModel, K, params) { + : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { this->body_P_sensor_ = body_P_sensor; } /** Virtual destructor */ - ~SmartProjectionFactorP() override { + ~SmartProjectionPoseFactor() override { } /** @@ -104,7 +104,7 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionFactorP, z = \n "; + std::cout << s << "SmartProjectionPoseFactor, z = \n "; Base::print("", keyFormatter); } @@ -161,12 +161,9 @@ public: // end of class declaration /// traits -template -struct traits > : public Testable< - SmartProjectionFactorP > { +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { }; -// legacy smart factor class, only templated on calibration and assuming pinhole -template using SmartProjectionPoseFactor = SmartProjectionFactorP< PinholePose >; - } // \ namespace gtsam