127 lines
4.4 KiB
C#
127 lines
4.4 KiB
C#
/*
|
|
* 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;
|
|
}
|
|
}
|
|
|