diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index b4ca6bd8b..14440b5a2 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -31,38 +31,71 @@ namespace gtsam { // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM { - // Keys and Values + /// Typedef for a PoseKey with Pose2 data and 'x' symbol typedef TypedSymbol PoseKey; + + /// Typedef for a PointKey with Point2 data and 'l' symbol typedef TypedSymbol PointKey; + + /// Typedef for LieValues structure with PoseKey type typedef LieValues PoseValues; + + /// Typedef for LieValues structure with PointKey type typedef LieValues PointValues; + + /// Typedef for the TupleValues2 to use PoseKeys and PointKeys typedef TupleValues2 Values; - // Factors + /** + * List of typedefs for factors + */ + + /// A hard constraint for PoseKeys to enforce particular values typedef NonlinearEquality Constraint; + /// A prior factor to bias the value of a PoseKey typedef PriorFactor Prior; + /// A factor between two PoseKeys set with a Pose2 typedef BetweenFactor Odometry; + /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) typedef BearingFactor Bearing; + /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) typedef RangeFactor Range; + /// A factor between a PoseKey and a PointKey to express difference in rotation and location typedef BearingRangeFactor BearingRange; - // Graph + /// Creates a NonlinearFactorGraph with the Values type struct Graph: public NonlinearFactorGraph { + + /// Default constructor for a NonlinearFactorGraph Graph(){} + + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph Graph(const NonlinearFactorGraph& graph); + + /// Biases the value of PoseKey i with Pose2 p given a noise model void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); + + /// Creates a hard constraint to enforce Pose2 p for PoseKey i's value void addPoseConstraint(const PoseKey& i, const Pose2& p); + + /// Creates a factor with a Pose2 between PoseKeys i and j (i.e. an odometry measurement) void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, const SharedNoiseModel& model); + + /// Creates a factor with a Rot2 between a PoseKey i and PointKey j for difference in rotation void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, const SharedNoiseModel& model); + + /// Creates a factor with a Rot2 between a PoseKey i and PointKey j for difference in location void addRange(const PoseKey& i, const PointKey& j, double z, const SharedNoiseModel& model); + + /// Creates a factor with a Rot2 between a PoseKey i and PointKey j for difference in rotation and location void addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, double z2, const SharedNoiseModel& model); }; - // Optimizer + /// Optimizer typedef NonlinearOptimizer Optimizer; } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index a00b91ff2..4f092e9f1 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -34,8 +34,10 @@ namespace gtsam { // Use pose2SLAM namespace for specific SLAM instance namespace pose2SLAM { - // Keys and Values + /// Keys with Pose2 and symbol 'x' typedef TypedSymbol Key; + + /// LieValues with type 'Key' typedef LieValues Values; /** @@ -47,33 +49,49 @@ namespace gtsam { */ Values circle(size_t n, double R); - // Factors + /// A prior factor on Key with Pose2 data type. typedef PriorFactor Prior; + + /// A factor to put constraints between two factors. typedef BetweenFactor Constraint; + + /// A hard constraint would enforce that the given key would have the input value in the results. typedef NonlinearEquality HardConstraint; - // Graph + /// Graph struct Graph: public NonlinearFactorGraph { + + /// Adds a factor between keys of the same type typedef BetweenFactor Constraint; + + /// A simple typedef from Pose2 to Pose typedef Pose2 Pose; + + /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph void addPrior(const Key& i, const Pose2& p, const SharedNoiseModel& model); + + /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedNoiseModel& model); + + /// Creates a hard constraint for key i with the given Pose2 p. void addHardConstraint(const Key& i, const Pose2& p); }; - // Optimizer + /// The sequential optimizer typedef NonlinearOptimizer OptimizerSequential; - typedef NonlinearOptimizer Optimizer; + + /// The multifrontal optimizer + typedef NonlinearOptimizer Optimizer; } // pose2SLAM /** * Backwards compatibility */ - typedef pose2SLAM::Values Pose2Values; - typedef pose2SLAM::Prior Pose2Prior; - typedef pose2SLAM::Constraint Pose2Factor; - typedef pose2SLAM::Graph Pose2Graph; + typedef pose2SLAM::Values Pose2Values; ///< Typedef for Values class for backwards compatibility + typedef pose2SLAM::Prior Pose2Prior; ///< Typedef for Prior class for backwards compatibility + typedef pose2SLAM::Constraint Pose2Factor; ///< Typedef for Constraint class for backwards compatibility + typedef pose2SLAM::Graph Pose2Graph; ///< Typedef for Graph class for backwards compatibility } // namespace gtsam diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 0125452fb..c8b615bb3 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -28,37 +28,45 @@ namespace gtsam { - // Use pose3SLAM namespace for specific SLAM instance + /// Use pose3SLAM namespace for specific SLAM instance namespace pose3SLAM { - // Keys and Values + /// Creates a Key with data Pose3 and symbol 'x' typedef TypedSymbol Key; + /// Creates a LieValues structure with type 'Key' typedef LieValues Values; /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * @param n number of poses * @param R radius of circle - * @param c character to use for keys * @return circle of n 3D poses */ Values circle(size_t n, double R); - // Factors + /// A prior factor on Key with Pose3 data type. typedef PriorFactor Prior; + /// A factor to put constraints between two factors. typedef BetweenFactor Constraint; + /// A hard constraint would enforce that the given key would have the input value in the results. typedef NonlinearEquality HardConstraint; - // Graph + /// Graph struct Graph: public NonlinearFactorGraph { + + /// Adds a factor between keys of the same type void addPrior(const Key& i, const Pose3& p, const SharedNoiseModel& model); + + /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph void addConstraint(const Key& i, const Key& j, const Pose3& z, const SharedNoiseModel& model); + + /// Creates a hard constraint for key i with the given Pose3 p. void addHardConstraint(const Key& i, const Pose3& p); }; - // Optimizer + /// Optimizer typedef NonlinearOptimizer Optimizer; } // pose3SLAM @@ -66,10 +74,10 @@ namespace gtsam { /** * Backwards compatibility */ - typedef pose3SLAM::Values Pose3Values; - typedef pose3SLAM::Prior Pose3Prior; - typedef pose3SLAM::Constraint Pose3Factor; - typedef pose3SLAM::Graph Pose3Graph; + typedef pose3SLAM::Values Pose3Values; ///< Typedef for Values class for backwards compatibility + typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility + typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility + typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility } // namespace gtsam