Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/PoseConverter.cs

@@ -0,0 +1,53 @@
+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);
+    }
+}