Class RelativePose2DStampedConstraint

Inheritance Relationships

Base Type

  • public fuse_core::Constraint

Class Documentation

class RelativePose2DStampedConstraint : public fuse_core::Constraint

A constraint that represents a measurement on the difference between two poses.

This type of constraint arises in many situations. Many types of incremental odometry measurements (wheel encoders, inertial strap-down, visual odometry) measure the change in the pose, not the pose directly. This constraint holds the measured 2D pose change and the measurement uncertainty/covariance. It also permits measurement of a subset of the relative pose provided in the position and orientation varables.

Public Functions

RelativePose2DStampedConstraint() = default

Default constructor.

RelativePose2DStampedConstraint(const std::string &source, const fuse_variables::Position2DStamped &position1, const fuse_variables::Orientation2DStamped &orientation1, const fuse_variables::Position2DStamped &position2, const fuse_variables::Orientation2DStamped &orientation2, const fuse_core::VectorXd &partial_delta, const fuse_core::MatrixXd &partial_covariance, const std::vector<size_t> &linear_indices = {fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y}, const std::vector<size_t> &angular_indices = {fuse_variables::Orientation2DStamped::YAW})

Constructor.

Note that, when measuring subset of dimensions, empty axis vectors are permitted. This signifies that you don’t want to measure any of the quantities in that variable.

The mean is given as a vector. The first components (if any) will be dictated, both in content and in ordering, by the value of the linear_indices. The final component (if any) is dictated by the angular_indices. The covariance matrix follows the same ordering.

Parameters:

  • source – [in] The name of the sensor or motion model that generated this constraint

  • position1 – [in] The variable representing the position components of the first pose

  • orientation1 – [in] The variable representing the orientation components of the first pose

  • position2 – [in] The variable representing the position components of the second pose

  • orientation2 – [in] The variable representing the orientation components of the second pose

  • partial_delta – [in] The measured change in the pose (max 3x1 vector, components are dictated by linear_indices and angular_indices)

  • partial_covariance – [in] The measurement covariance (max 3x3 matrix, components are dictated by linear_indices and angular_indices)

  • linear_indices – [in]

    The set of indices corresponding to the measured position dimensions e.g., “{fuse_variables::Position2DStamped::X,

    fuse_variables::Position2DStamped::Y}”

  • angular_indices – [in] The set of indices corresponding to the measured orientation dimensions e.g., “{fuse_variables::Orientation2DStamped::Yaw}”

virtual ~RelativePose2DStampedConstraint() = default

Destructor.

inline const fuse_core::Vector3d &delta() const

Read-only access to the measured pose change.

Order is (dx, dy, dyaw). Note that the returned vector will be full sized (3x1) and in the stated order.

inline const fuse_core::MatrixXd &sqrtInformation() const

Read-only access to the square root information matrix.

If only a partial covariance matrix was provided in the constructor, this covariance matrix will not be square.

fuse_core::Matrix3d covariance() const

Compute the measurement covariance matrix.

Order is (dx, dy, dyaw). Note that the returned covariance matrix will be full sized (3x3) and in the stated order. If only a partial covariance matrix was provided in the constructor, this covariance matrix may be a different size and in a different order than the constructor input.

void print(std::ostream &stream = std::cout) const override

Print a human-readable description of the constraint to the provided stream.

Parameters:

stream – [out] The stream to write to. Defaults to stdout.

ceres::CostFunction *costFunction() const override

Access the cost function for this constraint.

The function caller will own the new cost function instance. It is the responsibility of the caller to delete the cost function object when it is no longer needed. If the pointer is provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the pointer and delete it during destruction.

Returns:

A base pointer to an instance of a derived CostFunction.

Protected Attributes

fuse_core::Vector3d delta_

The measured pose change (dx, dy, dyaw)

fuse_core::MatrixXd sqrt_information_

The square root information matrix (derived from the covariance matrix)

Friends

friend class boost::serialization::access