Class Unicycle2DStateKinematicConstraint

Inheritance Relationships

Base Type

  • public fuse_core::Constraint

Class Documentation

class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint

A class that represents a kinematic constraint between 2D states at two different times.

The fuse_models 2D state is a combination of 2D position, 2D orientation, 2D linear velocity, 2D angular velocity, and 2D linear acceleration.

Public Functions

Unicycle2DStateKinematicConstraint() = default

Default constructor.

Unicycle2DStateKinematicConstraint(const std::string &source, const fuse_variables::Position2DStamped &position1, const fuse_variables::Orientation2DStamped &yaw1, const fuse_variables::VelocityLinear2DStamped &linear_velocity1, const fuse_variables::VelocityAngular2DStamped &yaw_velocity1, const fuse_variables::AccelerationLinear2DStamped &linear_acceleration1, const fuse_variables::Position2DStamped &position2, const fuse_variables::Orientation2DStamped &yaw2, const fuse_variables::VelocityLinear2DStamped &linear_velocity2, const fuse_variables::VelocityAngular2DStamped &yaw_velocity2, const fuse_variables::AccelerationLinear2DStamped &linear_acceleration2, const fuse_core::Matrix8d &covariance)

Create a constraint using a time delta and a kinematic model cost functor.

The constraint is created between two states. The state is broken up into multiple fuse variable types.

Parameters:

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

  • position1 – [in] Position component variable of the fist state

  • yaw1 – [in] Yaw component variable of the first state

  • linear_velocity1 – [in] Linear velocity component variable of the first state

  • yaw_velocity1 – [in] Yaw velocity component variable of the first state

  • linear_acceleration1 – [in] Linear acceleration component variable of the first state

  • position2 – [in] Position component variable of the second state

  • yaw2 – [in] Yaw component variable of the second state

  • linear_velocity2 – [in] Linear velocity component variable of the second state

  • yaw_velocity2 – [in] Yaw velocity component variable of the second state

  • linear_acceleration2 – [in] Linear acceleration component variable of the second state

  • covariance – [in] - The covariance matrix used to weight the constraint. Order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc)

virtual ~Unicycle2DStateKinematicConstraint() = default

Destructor.

inline double dt() const

Read-only access to the time delta between the first and second state (really, between the position1 and position2 variables in the constructor)

inline const fuse_core::Matrix8d &sqrtInformation() const

Read-only access to the square root information matrix.

Order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc)

inline fuse_core::Matrix8d covariance() const

Compute the measurement covariance matrix.

Order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc)

virtual 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.

virtual 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

double dt_

The time delta for the constraint.

fuse_core::Matrix8d sqrt_information_

The square root information matrix.

Friends

friend class boost::serialization::access