Extract Transformation Matrix C#

mail@pastecode.io avatar
unknown
csharp
a year ago
2.5 kB
2
Indexable
Never
using System;
using System.IO;
using MathNet.Numerics.LinearAlgebra;

public class CameraPoseExtractor
{
    public static (Vector<double>, Vector<double>) ExtractTranslationAndRotation(Matrix<double> cameraPoseMatrix)
    {
        // Extract the translation vector (XYZ) from the transformation matrix
        var translation = cameraPoseMatrix.SubMatrix(0, 3, 3, 1);

        // Extract the rotation submatrix (3x3) from the transformation matrix
        var rotationSubmatrix = cameraPoseMatrix.SubMatrix(0, 3, 0, 3);

        // Calculate the XYZ Euler angles from the rotation submatrix
        double pitch = Math.Asin(-rotationSubmatrix[2, 0]);                                    // Rotation around X-axis
        double roll = Math.Atan2(rotationSubmatrix[2, 1], rotationSubmatrix[2, 2]);           // Rotation around Y-axis
        double yaw = Math.Atan2(rotationSubmatrix[1, 0], rotationSubmatrix[0, 0]);             // Rotation around Z-axis

        // Convert Euler angles from radians to degrees
        double pitchDeg = MathHelper.ToDegrees(pitch);
        double rollDeg = MathHelper.ToDegrees(roll);
        double yawDeg = MathHelper.ToDegrees(yaw);

        return (translation, Vector<double>.Build.DenseOfArray(new double[] { rollDeg, pitchDeg, yawDeg }));
    }
}

public static class MathHelper
{
    public static double ToDegrees(double radians)
    {
        return radians * (180.0 / Math.PI);
    }
}

class Program
{
    static void Main()
    {
        string folderPath = "poses";
        string[] files = Directory.GetFiles(folderPath);
        Array.Sort(files);

        foreach (string file in files)
        {
            string[] lines = File.ReadAllLines(file);
            double[,] cameraPoseMatrixArray = new double[lines.Length, lines[0].Split().Length];
            for (int i = 0; i < lines.Length; i++)
            {
                string[] elements = lines[i].Split();
                for (int j = 0; j < elements.Length; j++)
                {
                    cameraPoseMatrixArray[i, j] = Convert.ToDouble(elements[j]);
                }
            }

            var cameraPoseMatrix = Matrix<double>.Build.DenseOfArray(cameraPoseMatrixArray);
            var (translation, rotationAngles) = CameraPoseExtractor.ExtractTranslationAndRotation(cameraPoseMatrix);

            Console.WriteLine($"Translation (XYZ): {string.Join(", ", translation)}");
            Console.WriteLine($"Rotation (XYZ Euler Angles): {string.Join(", ", rotationAngles)}");
        }
    }
}