Added documentation for Simulated3D.h
Alex Cunningham
parent
44b74c3d55
commit
cf72788bea
|
|
@ -31,62 +31,102 @@
|
|||
namespace gtsam {
|
||||
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 LieValues<PointKey> PointValues;
|
||||
typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
|
||||
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
|
||||
|
||||
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);
|
||||
|
||||
/**
|
||||
* 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) :
|
||||
PointPrior3D(const Point3& z,
|
||||
const SharedNoiseModel& model, const PoseKey& j) :
|
||||
NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
|
||||
}
|
||||
}
|
||||
|
||||
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
|
||||
boost::none) {
|
||||
return (prior(x, H) - z_).vector();
|
||||
}
|
||||
};
|
||||
/**
|
||||
* Evaluates the error at a given value of x,
|
||||
* 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> (
|
||||
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) {
|
||||
return (mea(x1, x2, H1, H2) - z_).vector();
|
||||
}
|
||||
};
|
||||
/**
|
||||
* Error function with optional derivatives
|
||||
* @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
|
||||
|
|
|
|||
Loading…
Reference in New Issue