50 lines
1.4 KiB
C#
50 lines
1.4 KiB
C#
namespace RobotNet10.Shared.Geometry;
|
|
|
|
/// <summary>
|
|
/// PoseWithCovariance message (geometry_msgs/PoseWithCovariance)
|
|
/// A Pose with associated covariance matrix
|
|
/// </summary>
|
|
public struct PoseWithCovariance
|
|
{
|
|
/// <summary>
|
|
/// Pose
|
|
/// </summary>
|
|
public Pose Pose { get; set; }
|
|
|
|
/// <summary>
|
|
/// Covariance matrix (row-major order)
|
|
/// The orientation parameters use a fixed-axis representation.
|
|
/// In order, the parameters are:
|
|
/// (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
|
|
/// </summary>
|
|
public double[] Covariance { get; set; }
|
|
|
|
/// <summary>
|
|
/// Size of covariance matrix (6x6 = 36 elements)
|
|
/// </summary>
|
|
public const int CovarianceSize = 36;
|
|
|
|
/// <summary>
|
|
/// Default constructor
|
|
/// </summary>
|
|
public PoseWithCovariance()
|
|
{
|
|
Pose = new Pose();
|
|
Covariance = new double[CovarianceSize];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Constructor with parameters
|
|
/// </summary>
|
|
public PoseWithCovariance(Pose pose, double[] covariance)
|
|
{
|
|
Pose = pose;
|
|
if (covariance == null || covariance.Length != CovarianceSize)
|
|
{
|
|
throw new ArgumentException($"Covariance array must have {CovarianceSize} elements", nameof(covariance));
|
|
}
|
|
Covariance = covariance;
|
|
}
|
|
}
|
|
|