Function fuse_models::common::processDifferentialPoseWithCovariance

Function Documentation

inline bool fuse_models::common::processDifferentialPoseWithCovariance(const std::string &source, const fuse_core::UUID &device_id, const geometry_msgs::msg::PoseWithCovarianceStamped &pose1, const geometry_msgs::msg::PoseWithCovarianceStamped &pose2, const bool independent, const fuse_core::Matrix3d &minimum_pose_relative_covariance, 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 covariance of each pose message is rotated into the robot’s base frame at the time of pose_absolute1. They are then added in the constraint if the pose measurements are independent. Otherwise, if the pose measurements are dependent, the covariance of pose_absolute1 is substracted from the covariance of pose_absolute2. A small minimum relative covariance is added to avoid getting a zero or ill-conditioned covariance. This could happen if both covariance matrices are the same or very similar, e.g. when pose_absolute1 == pose_absolute2, it’s possible that the covariance is the same for both poses.

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

  • independent – [in] - Whether the pose measurements are indepent or not

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

  • 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