54 lines
1.4 KiB
C#
54 lines
1.4 KiB
C#
using CartographerSharp.Transform;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
|
|
/// <summary>
|
|
/// Helper class for converting between Cartographer Rigid3d and RobotNet10 Pose
|
|
/// </summary>
|
|
public static class PoseConverter
|
|
{
|
|
/// <summary>
|
|
/// Convert Rigid3d to Pose
|
|
/// </summary>
|
|
public static Pose ToPose(Rigid3d rigid3d)
|
|
{
|
|
return new Pose
|
|
{
|
|
Position = new Vector3
|
|
{
|
|
X = rigid3d.Translation.X,
|
|
Y = rigid3d.Translation.Y,
|
|
Z = rigid3d.Translation.Z
|
|
},
|
|
Orientation = new Quaternion
|
|
{
|
|
X = rigid3d.Rotation.X,
|
|
Y = rigid3d.Rotation.Y,
|
|
Z = rigid3d.Rotation.Z,
|
|
W = rigid3d.Rotation.W
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert Pose to Rigid3d
|
|
/// </summary>
|
|
public static Rigid3d ToRigid3d(Pose pose)
|
|
{
|
|
var translation = new Vector3(
|
|
(float)pose.Position.X,
|
|
(float)pose.Position.Y,
|
|
(float)pose.Position.Z);
|
|
|
|
var rotation = new Quaternion(
|
|
(float)pose.Orientation.X,
|
|
(float)pose.Orientation.Y,
|
|
(float)pose.Orientation.Z,
|
|
(float)pose.Orientation.W);
|
|
|
|
return new Rigid3d(translation, rotation);
|
|
}
|
|
}
|