diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index d6b549acb..8fc8880e1 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -11,7 +11,7 @@ /** * @file SmartProjectionFactor.h - * @brief Base class to create smart factors on poses or cameras + * @brief Smart factor on cameras (pose + calibration) * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert @@ -108,16 +108,22 @@ public: void setEnableEPI(bool enableEPI) { triangulation.enableEPI = enableEPI; } - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) { + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; } - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) { + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; } }; /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. + * This factor operates with monocular cameras, where a camera is expected to + * behave like PinholeCamera or PinholePose. This factor is intended + * to be used directly with PinholeCamera, which optimizes the camera pose + * and calibration. This also requires that values contains the involved + * cameras (instead of poses and calibrations separately). + * If the calibration is fixed use SmartProjectionPoseFactor instead! */ template class SmartProjectionFactor: public SmartFactorBase { diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 93a4449f5..834c9c9b6 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -11,7 +11,7 @@ /** * @file SmartProjectionPoseFactor.h - * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @brief Smart factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira @@ -34,7 +34,11 @@ namespace gtsam { */ /** - * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * This factor assumes that camera calibration is fixed, and that + * the calibration is the same for all cameras involved in this factor. + * The factor only constrains poses (variable dimension is 6). + * This factor requires that values contains the involved poses (Pose3). + * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM */ template @@ -58,7 +62,7 @@ public: /** * Constructor * @param K (fixed) calibration, assumed to be the same for all cameras - * @param body_P_sensor pose of the camera in the body frame + * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ SmartProjectionPoseFactor(const boost::shared_ptr K,