diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 2b5eff78d..cf96166ae 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -28,6 +28,7 @@ #include #include #include +#include namespace gtsam { @@ -47,6 +48,7 @@ namespace gtsam { typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point typedef PriorFactor PosePrior; ///< put a soft prior on a Pose typedef PriorFactor PointPrior; ///< put a soft prior on a point + typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point /// monocular and stereo camera typedefs for general use typedef GenericProjectionFactor ProjectionFactor; @@ -115,7 +117,7 @@ namespace gtsam { * @param p to which pose to constrain it to * @param model uncertainty model of this prior */ - void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { boost::shared_ptr factor(new PosePrior(j, p, model)); push_back(factor); } @@ -126,11 +128,15 @@ namespace gtsam { * @param p index of point * @param model uncertainty model of this prior */ - void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { boost::shared_ptr factor(new PointPrior(j, p, model)); push_back(factor); } + void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + push_back(boost::shared_ptr(new RangeFactor(i, j, range, model))); + } + }; // Graph /// typedef for Optimizer. The current default will use the multi-frontal solver