Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/3D/ImuIntegration.cs

@@ -0,0 +1,126 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.Internal.D3D;
+
+/// <summary>
+/// Result of IMU integration.
+/// </summary>
+public struct IntegrateImuResult
+{
+    public Vector3 DeltaVelocity { get; set; }
+    public Vector3 DeltaTranslation { get; set; }
+    public Quaternion DeltaRotation { get; set; }
+
+    public IntegrateImuResult(Vector3 deltaVelocity, Vector3 deltaTranslation, Quaternion deltaRotation)
+    {
+        DeltaVelocity = deltaVelocity;
+        DeltaTranslation = deltaTranslation;
+        DeltaRotation = deltaRotation;
+    }
+}
+
+/// <summary>
+/// IMU integration utilities.
+/// </summary>
+public static class ImuIntegration
+{
+    /// <summary>
+    /// Integrates IMU data between start_time and end_time.
+    /// Returns delta_velocity, delta_translation, and delta_rotation.
+    /// </summary>
+    public static IntegrateImuResult IntegrateImu(
+        List<ImuData> imuData,
+        long startTime,
+        long endTime,
+        ref int imuIndex)
+    {
+        if (startTime > endTime)
+            throw new ArgumentException("startTime must be <= endTime");
+        
+        if (imuIndex < 0 || imuIndex >= imuData.Count)
+            throw new ArgumentOutOfRangeException(nameof(imuIndex));
+        
+        if (imuData[imuIndex].Time > startTime)
+            throw new ArgumentException("imuData[imuIndex].Time must be <= startTime");
+        
+        if (imuIndex + 1 < imuData.Count && imuData[imuIndex + 1].Time <= startTime)
+            throw new ArgumentException("imuData[imuIndex+1].Time must be > startTime");
+
+        var result = new IntegrateImuResult(
+            Vector3.Zero,
+            Vector3.Zero,
+            Quaternion.Identity);
+
+        long currentTime = startTime;
+
+        while (currentTime < endTime)
+        {
+            long nextImuTime = long.MaxValue;
+            if (imuIndex + 1 < imuData.Count)
+            {
+                nextImuTime = imuData[imuIndex + 1].Time;
+            }
+            
+            long nextTime = Math.Min(nextImuTime, endTime);
+            double deltaT = (nextTime - currentTime) / 10_000_000.0; // Convert ticks to seconds (10 million ticks per second)
+
+            var currentImu = imuData[imuIndex];
+            
+            // Compute delta angle from angular velocity
+            var deltaAngle = currentImu.AngularVelocity * deltaT;
+            
+            // Convert angle-axis to quaternion (simplified - assumes small angles)
+            // For small angles: q ≈ [1, 0.5*angle.x, 0.5*angle.y, 0.5*angle.z]
+            var angleLength = deltaAngle.Length();
+            Quaternion deltaRotation;
+            if (angleLength < 1e-6)
+            {
+                deltaRotation = Quaternion.Identity;
+            }
+            else
+            {
+                var axis = Vector3.Normalize(deltaAngle);
+                deltaRotation = Quaternion.CreateFromAxisAngle(axis, angleLength);
+            }
+            
+            // Update cumulative rotation
+            result.DeltaRotation = Quaternion.Multiply(result.DeltaRotation, deltaRotation);
+            
+            // Integrate linear acceleration
+            // Rotate acceleration to current orientation frame
+            var rotatedAcceleration = Vector3.Transform(currentImu.LinearAcceleration, result.DeltaRotation);
+            var deltaVelocity = rotatedAcceleration * deltaT;
+            result.DeltaVelocity += deltaVelocity;
+            
+            // Integrate velocity to get translation
+            result.DeltaTranslation += result.DeltaVelocity * deltaT;
+
+            currentTime = nextTime;
+            if (currentTime == nextImuTime)
+            {
+                imuIndex++;
+            }
+        }
+
+        return result;
+    }
+}
+