BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/Internal/Optimization/CostHelpers.cs

/*
 * Copyright 2018 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.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.Internal.Optimization;

/// <summary>
/// Helper functions for cost function computation.
/// </summary>
internal static class CostHelpers
{
    /// <summary>
    /// Computes spherical linear interpolation of unit quaternions.
    /// </summary>
    public static Quaternion SlerpQuaternions(Quaternion start, Quaternion end, double factor)
    {
        // Normalize quaternions
        start = Quaternion.Normalize(start);
        end = Quaternion.Normalize(end);

        // Compute dot product
        var cosTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
        // Clamp to [-1, 1] to handle floating-point errors that could cause Math.Acos to return NaN
        var absCosTheta = Math.Min(1.0, Math.Abs(cosTheta));

        // If quaternions are nearly collinear, use linear interpolation
        const double kEpsilon = 1e-6;
        double prevScale = 1.0 - factor;
        double nextScale = factor;

        if (absCosTheta < 1.0 - kEpsilon)
        {
            var theta = Math.Acos(absCosTheta);
            var sinTheta = Math.Sin(theta);
            if (sinTheta > kEpsilon)
            {
                prevScale = Math.Sin((1.0 - factor) * theta) / sinTheta;
                nextScale = Math.Sin(factor * theta) / sinTheta;
            }
        }

        if (cosTheta < 0.0)
        {
            nextScale = -nextScale;
        }

        // Quaternion constructor is (x, y, z, w), matching C++ output format [w, x, y, z]
        // but converting to C# Quaternion format (x, y, z, w)
        var result = new Quaternion(
            prevScale * start.X + nextScale * end.X,
            prevScale * start.Y + nextScale * end.Y,
            prevScale * start.Z + nextScale * end.Z,
            prevScale * start.W + nextScale * end.W
        );
        // Normalize to ensure unit quaternion (Eigen SLERP automatically normalizes)
        return Quaternion.Normalize(result);
    }

    /// <summary>
    /// Interpolates 3D nodes.
    /// </summary>
    public static (Quaternion rotation, Vector3 translation) InterpolateNodes3D(
        double[] prevNodeRotation, // [w, x, y, z]
        double[] prevNodeTranslation, // [x, y, z]
        double[] nextNodeRotation, // [w, x, y, z]
        double[] nextNodeTranslation, // [x, y, z]
        double interpolationParameter)
    {
        // Match C++: prev_node_rotation is [w, x, y, z]
        // System.Numerics.Quaternion constructor is (x, y, z, w)
        var prevQuaternion = new Quaternion(
            prevNodeRotation[1], // x
            prevNodeRotation[2], // y
            prevNodeRotation[3], // z
            prevNodeRotation[0]  // w
        );
        var nextQuaternion = new Quaternion(
            nextNodeRotation[1], // x
            nextNodeRotation[2], // y
            nextNodeRotation[3], // z
            nextNodeRotation[0]  // w
        );

        // Interpolate rotation using SLERP
        var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);

        // Interpolate translation linearly
        var interpolatedTranslation = new Vector3(
            (prevNodeTranslation[0] + interpolationParameter * (nextNodeTranslation[0] - prevNodeTranslation[0])),
            (prevNodeTranslation[1] + interpolationParameter * (nextNodeTranslation[1] - prevNodeTranslation[1])),
            (prevNodeTranslation[2] + interpolationParameter * (nextNodeTranslation[2] - prevNodeTranslation[2]))
        );

        return (interpolatedRotation, interpolatedTranslation);
    }

    /// <summary>
    /// Interpolates 2D nodes embedded in 3D space.
    /// </summary>
    public static (Quaternion rotation, Vector3 translation) InterpolateNodes2D(
        double[] prevNodePose, // [x, y, theta]
        Quaternion prevNodeGravityAlignment,
        double[] nextNodePose, // [x, y, theta]
        Quaternion nextNodeGravityAlignment,
        double interpolationParameter)
    {
        // Embed 2D pose into 3D with gravity alignment
        // Equivalent to: Embed3D(prev_node_pose) * Rigid3d::Rotation(prev_node_gravity_alignment)
        var prevRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, prevNodePose[2]);
        var prevQuaternion = Quaternion.Normalize(prevRotation2D * prevNodeGravityAlignment);

        var nextRotation2D = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, nextNodePose[2]);
        var nextQuaternion = Quaternion.Normalize(nextRotation2D * nextNodeGravityAlignment);

        // Interpolate rotation using SLERP
        var interpolatedRotation = SlerpQuaternions(prevQuaternion, nextQuaternion, interpolationParameter);

        // Interpolate translation linearly (2D, z=0)
        var interpolatedTranslation = new Vector3(
            (prevNodePose[0] + interpolationParameter * (nextNodePose[0] - prevNodePose[0])),
            (prevNodePose[1] + interpolationParameter * (nextNodePose[1] - prevNodePose[1])),
            0.0
        );

        return (interpolatedRotation, interpolatedTranslation);
    }

    /// <summary>
    /// Computes unscaled error for 3D poses.
    /// Error = observed_relative_pose - computed_relative_pose
    /// </summary>
    public static double[] ComputeUnscaledError3D(
        Rigid3d observedRelativePose,
        Quaternion startRotation,
        Vector3 startTranslation,
        Quaternion endRotation,
        Vector3 endTranslation)
    {
        // Compute relative transform: start^-1 * end
        var startInverse = Quaternion.Inverse(startRotation);
        var deltaTranslation = endTranslation - startTranslation;
        var rotatedDelta = Vector3.Transform(deltaTranslation, startInverse);

        // Compute h_rotation_inverse = (end^-1) * start (matching C++ implementation)
        // This is equivalent to: endRotation.Inverse() * startRotation
        var endInverse = Quaternion.Inverse(endRotation);
        var hRotationInverse = endInverse * startRotation;

        // Error rotation: h_rotation_inverse * observed_relative_rotation
        var errorRotation = hRotationInverse * observedRelativePose.Rotation;

        // Convert rotation error to angle-axis
        var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(errorRotation);

        return
        [
            observedRelativePose.Translation.X - rotatedDelta.X,
            observedRelativePose.Translation.Y - rotatedDelta.Y,
            observedRelativePose.Translation.Z - rotatedDelta.Z,
            angleAxis.X,
            angleAxis.Y,
            angleAxis.Z
        ];
    }

    /// <summary>
    /// Scales error with translation and rotation weights.
    /// </summary>
    public static double[] ScaleError3D(
        double[] unscaledError,
        double translationWeight,
        double rotationWeight)
    {
        return
        [
            translationWeight * unscaledError[0],
            translationWeight * unscaledError[1],
            translationWeight * unscaledError[2],
            rotationWeight * unscaledError[3],
            rotationWeight * unscaledError[4],
            rotationWeight * unscaledError[5]
        ];
    }
}