Class AbsolutePose3DStampedConstraint

Inheritance Relationships

Base Type

  • public fuse_core::Constraint

Class Documentation

class AbsolutePose3DStampedConstraint : public fuse_core::Constraint

A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose.

A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, this class applies an absolute constraint on both variables at once. This type of constraint arises in many situations. In mapping it is common to define the very first pose as the origin. Some sensors, such as GPS, provide direct measurements of the robot’s pose in the global frame. And localization systems often match laserscans or pointclouds to a prior map (scan-to-map measurements). This constraint holds the measured 3D pose and the measurement uncertainty/covariance. Orientations are represented as quaternions.

Public Functions

AbsolutePose3DStampedConstraint() = default

Default constructor.

AbsolutePose3DStampedConstraint(const std::string &source, const fuse_variables::Position3DStamped &position, const fuse_variables::Orientation3DStamped &orientation, const fuse_core::Vector7d &mean, const fuse_core::Matrix6d &covariance)

Create a constraint using a measurement/prior of the 3D pose.

Parameters:

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

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

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

  • mean – [in] The measured/prior pose as a vector (7x1 vector: x, y, z, qw, qx, qy, qz)

  • covariance – [in] The measurement/prior covariance (6x6 matrix: x, y, z, qx, qy, qz)

virtual ~AbsolutePose3DStampedConstraint() = default

Destructor.

inline const fuse_core::Vector7d &mean() const

Read-only access to the measured/prior vector of mean values.

Order is (x, y, z, qw, qx, qy, qz)

inline const fuse_core::Matrix6d &sqrtInformation() const

Read-only access to the square root information matrix.

Order is (x, y, z, qx, qy, qz)

inline fuse_core::Matrix6d covariance() const

Compute the measurement covariance matrix.

Order is (x, y, z, qx, qy, qz)

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

Construct an instance of this constraint’s cost function.

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::Vector7d mean_

The measured/prior mean vector for this variable.

fuse_core::Matrix6d sqrt_information_

The square root information matrix.

Friends

friend class boost::serialization::access