Function fuse_models::common::processDifferentialPoseWithTwistCovariance

Function Documentation

inline bool fuse_models::common::processDifferentialPoseWithTwistCovariance(const std::string &source, const fuse_core::UUID &device_id, const geometry_msgs::msg::PoseWithCovarianceStamped &pose1, const geometry_msgs::msg::PoseWithCovarianceStamped &pose2, const geometry_msgs::msg::TwistWithCovarianceStamped &twist, const fuse_core::Matrix3d &minimum_pose_relative_covariance, const fuse_core::Matrix3d &twist_covariance_offset, const fuse_core::Loss::SharedPtr &loss, const std::vector<size_t> &position_indices, const std::vector<size_t> &orientation_indices, const bool validate, fuse_core::Transaction &transaction)

Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a fuse Transaction.

This method computes the delta between two poses and creates the required fuse variables and constraints, and then adds them to the given transaction. Only 2D data is used. The pose delta is calculated as

pose_relative = pose_absolute1^-1 * pose_absolute2

Additionally, the twist covariance of the last message is used to compute the relative pose covariance using the time difference between the pose_absolute2 and pose_absolute1 time stamps. This assumes the pose measurements are dependent. A small minimum relative covariance is added to avoid getting a zero or ill-conditioned covariance. This could happen if the twist covariance is very small, e.g. when the twist is zero.

Parameters:

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

  • device_id – [in] - The UUID of the machine

  • pose1 – [in] - The first (and temporally earlier) PoseWithCovarianceStamped message

  • pose2 – [in] - The second (and temporally later) PoseWithCovarianceStamped message

  • twist – [in] - The second (and temporally later) TwistWithCovarianceStamped message

  • minimum_pose_relative_covariance – [in] - The minimum pose relative covariance that is always added to the resulting pose relative covariance

  • twist_covariance_offset – [in] - The twist covariance offset that was added to the twist covariance and must be substracted from it before computing the pose relative covariance from it

  • loss – [in] - The loss function for the 2D pose constraint generated

  • validate – [in] - Whether to validate the measurements or not. If the validation fails no constraint is added

  • transaction – [out] - The generated variables and constraints are added to this transaction

Returns:

true if any constraints were added, false otherwise