Class RelativeOrientation3DStampedConstraint

Inheritance Relationships

Base Type

  • public fuse_core::Constraint

Class Documentation

class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint

A constraint that represents a measurement on the difference between 3D orientation variables.

This constraint holds the measured 3D orientation and the measurement uncertainty/covariance.

Public Functions

RelativeOrientation3DStampedConstraint() = default

Default constructor.

RelativeOrientation3DStampedConstraint(const std::string &source, const fuse_variables::Orientation3DStamped &orientation1, const fuse_variables::Orientation3DStamped &orientation2, const fuse_core::Vector4d &delta, const fuse_core::Matrix3d &covariance)

Create a constraint using a measurement/prior of a 3D orientation.

Parameters:

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

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

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

  • delta – [in] The measured orientation change as a quaternion (4x1 vector: w, x, y, z)

  • covariance – [in] The measurement covariance (3x3 matrix: qx, qy, qz)

RelativeOrientation3DStampedConstraint(const std::string &source, const fuse_variables::Orientation3DStamped &orientation1, const fuse_variables::Orientation3DStamped &orientation2, const Eigen::Quaterniond &delta, const fuse_core::Matrix3d &covariance)

Create a constraint using a measurement/prior of a 3D orientation.

Parameters:

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

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

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

  • delta – [in] The measured orientation change as an Eigen quaternion

  • covariance – [in] The measurement covariance (3x3 matrix: qx, qy, qz)

RelativeOrientation3DStampedConstraint(const std::string &source, const fuse_variables::Orientation3DStamped &orientation1, const fuse_variables::Orientation3DStamped &orientation2, const geometry_msgs::msg::Quaternion &delta, const std::array<double, 9> &covariance)

Create a constraint using a measurement/prior of a 3D orientation.

Parameters:

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

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

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

  • delta – [in] The measured orientation change as a ROS quaternion message

  • covariance – [in] The measurement covariance (3x3 matrix: qx, qy, qz)

virtual ~RelativeOrientation3DStampedConstraint() = default

Destructor.

inline const fuse_core::Vector4d &delta() const

Read-only access to the measured change between variables.

Order is (w, x, y, z)

inline const fuse_core::Matrix3d &sqrtInformation() const

Read-only access to the square root information matrix.

Order is (x, y, z)

fuse_core::Matrix3d covariance() const

Compute the measurement covariance matrix.

Order is (x, y, z)

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::Vector4d delta_

The measured/prior mean vector for this variable.

fuse_core::Matrix3d sqrt_information_

The square root information matrix.

Protected Static Functions

static fuse_core::Vector4d toEigen(const Eigen::Quaterniond &quaternion)

Utility method to convert an Eigen quaternion to an Eigen Vector4d.

Parameters:

quaternion – [in] The input Eigen quaternion

Returns:

The quaternion, converted to an Eigen Vector4d

static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion &quaternion)

Utility method to convert an ROS quaternion message to an Eigen Vector4d.

Parameters:

quaternion – [in] The input ROS quaternion message

Returns:

The quaternion, converted to an Eigen Vector4d

static fuse_core::Matrix3d toEigen(const std::array<double, 9> &covariance)

Utility method to convert a flat 1D array to a 3x3 Eigen matrix.

Parameters:

covariance – [in] The input covariance array

Returns:

The covariance, converted to an Eigen Matrix3d

Friends

friend class boost::serialization::access