12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added documentation for Simulated3D.h

release/4.3a0
Alex Cunningham 2011-09-07 01:52:24 +00:00
parent 44b74c3d55
commit cf72788bea
1 changed files with 86 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
gtsam/slam/Simulated3D.h
View File

@ -31,62 +31,102 @@
namespace gtsam { namespace gtsam {
namespace simulated3D { namespace simulated3D {
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey; /**
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey; * This is a linear SLAM domain where both poses and landmarks are
* 3D points, without rotation. The structure and use is based on
* the simulated2D domain.
*/
typedef LieValues<PoseKey> PoseValues; typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
typedef LieValues<PointKey> PointValues; typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
typedef TupleValues2<PoseValues, PointValues> Values;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef TupleValues2<PoseValues, PointValues> Values;
/**
* Prior on a single pose
*/
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
/**
* odometry between two poses
*/
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
/**
* measurement between landmark and pose
*/
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
/**
* A prior factor on a single linear robot pose
*/
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
Point3 z_; ///< The prior pose value for the variable attached to this factor
/** /**
* Prior on a single pose * Constructor for a prior factor
* @param z is the measured/prior position for the pose
* @param model is the measurement model for the factor (Dimension: 3)
* @param j is the key for the pose
*/ */
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none); PointPrior3D(const Point3& z,
const SharedNoiseModel& model, const PoseKey& j) :
/**
* odometry between two poses
*/
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
/**
* measurement between landmark and pose
*/
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none);
struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
Point3 z_;
PointPrior3D(const Point3& z,
const SharedNoiseModel& model, const PoseKey& j) :
NonlinearFactor1<Values, PoseKey> (model, j), z_(z) { NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
} }
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H = /**
boost::none) { * Evaluates the error at a given value of x,
return (prior(x, H) - z_).vector(); * with optional derivatives.
} * @param x is the current value of the variable
}; * @param H is an optional Jacobian matrix (Dimension: 3x3)
* @return Vector error between prior value and x (Dimension: 3)
*/
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) {
return (prior(x, H) - z_).vector();
}
};
struct Simulated3DMeasurement: public NonlinearFactor2<Values, /**
PoseKey, PointKey> { * Models a linear 3D measurement between 3D points
*/
struct Simulated3DMeasurement: public NonlinearFactor2<Values,
PoseKey, PointKey> {
Point3 z_; Point3 z_; ///< Linear displacement between a pose and landmark
Simulated3DMeasurement(const Point3& z, /**
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : * Creates a measurement factor with a given measurement
* @param z is the measurement, a linear displacement between poses and landmarks
* @param model is a measurement model for the factor (Dimension: 3)
* @param j1 is the pose key of the robot
* @param j2 is the point key for the landmark
*/
Simulated3DMeasurement(const Point3& z,
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
NonlinearFactor2<Values, PoseKey, PointKey> ( NonlinearFactor2<Values, PoseKey, PointKey> (
model, j1, j2), z_(z) { model, j1, j2), z_(z) {
} }
Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< /**
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) { * Error function with optional derivatives
return (mea(x1, x2, H1, H2) - z_).vector(); * @param x1 a robot pose value
} * @param x2 a landmark point value
}; * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3)
* @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
* @return vector error between measurement and prediction (Dimension: 3)
*/
Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
return (mea(x1, x2, H1, H2) - z_).vector();
}
};
}} // namespace simulated3D }} // namespace simulated3D