diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index b5b325bfe..92c9a7660 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -74,6 +74,10 @@ protected: const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) /// @} + /// @name Pose of the camera in the body frame + const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + /// @} + public: /// shorthand for a smart pointer to a factor @@ -94,14 +98,16 @@ public: double rankTolerance = 1, DegeneracyMode degeneracyMode = IGNORE_DEGENERACY, bool enableEPI = false, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : + double dynamicOutlierRejectionThreshold = -1, + boost::optional body_P_sensor = boost::none) : linearizationMode_(linearizationMode), // degeneracyMode_(degeneracyMode), // parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), // result_(TriangulationResult::Degenerate()), // retriangulationThreshold_(1e-5), // - throwCheirality_(false), verboseCheirality_(false) { + throwCheirality_(false), verboseCheirality_(false), + body_P_sensor_(body_P_sensor){ } /** Virtual destructor */ @@ -119,6 +125,8 @@ public: std::cout << "linearizationMode:\n" << linearizationMode_ << std::endl; std::cout << "triangulationParameters:\n" << parameters_ << std::endl; std::cout << "result:\n" << result_ << std::endl; + if(body_P_sensor_) + body_P_sensor_->print("body_P_sensor_:\n"); Base::print("", keyFormatter); } @@ -468,6 +476,13 @@ public: return throwCheirality_; } + Pose3 body_P_sensor() const{ + if(body_P_sensor_) + return *body_P_sensor_; + else + return Pose3(); // if unspecified, the transformation is the identity + } + private: /// Serialization function diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 275056735..9a47bd34f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -49,7 +49,6 @@ private: protected: boost::shared_ptr K_; ///< calibration object (one for all cameras) - boost::optional body_P_sensor_; ///< Pose of the camera in the body frame public: @@ -71,7 +70,7 @@ public: LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, landmarkDistanceThreshold, - dynamicOutlierRejectionThreshold), K_(K), body_P_sensor_(body_P_sensor) {} + dynamicOutlierRejectionThreshold, body_P_sensor), K_(K) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -84,8 +83,6 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartProjectionPoseFactor, z = \n "; - if(body_P_sensor_) - body_P_sensor_->print("body_P_sensor_:\n"); Base::print("", keyFormatter); } @@ -129,13 +126,6 @@ public: return K_; } - Pose3 body_P_sensor() const{ - if(body_P_sensor_) - return *body_P_sensor_; - else - return Pose3(); // if unspecified, the transformation is the identity - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding @@ -146,8 +136,8 @@ public: typename Base::Cameras cameras; BOOST_FOREACH(const Key& k, this->keys_) { Pose3 pose = values.at(k); - if(body_P_sensor_) - pose = pose.compose(*(body_P_sensor_)); + if(Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); Camera camera(pose, K_); cameras.push_back(camera);