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