# Extract Transformation Matrix C#

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
{
{
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)
{
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)}");
}
}
}```