Initial commit

srcs/RobotNet10/RobotApp/Communication/CeresSharp.Test/ManifoldTests.cs

@@ -0,0 +1,137 @@
+using CeresSharp;
+
+namespace CeresSharp.Test;
+
+[TestFixture]
+public class ManifoldTests
+{
+    [Test]
+    public void QuaternionManifold_ShouldCreate()
+    {
+        using var manifold = new QuaternionManifold();
+        Assert.That(manifold, Is.Not.Null);
+    }
+
+    [Test]
+    public void QuaternionManifold_WithProblem_ShouldWork()
+    {
+        using var problem = new Problem();
+        var quaternion = new double[] { 0.0, 0.0, 0.0, 1.0 }; // qx, qy, qz, qw
+        problem.AddParameterBlock(quaternion, quaternion.Length);
+        
+        using var manifold = new QuaternionManifold();
+        problem.SetManifold(quaternion, manifold);
+        
+        // Should not throw
+        Assert.Pass();
+    }
+
+    [Test]
+    public void SphereManifold_ShouldCreate()
+    {
+        using var manifold = new SphereManifold(dimension: 3);
+        Assert.That(manifold, Is.Not.Null);
+    }
+
+    [Test]
+    public void SphereManifold_WithProblem_ShouldWork()
+    {
+        using var problem = new Problem();
+        var point = new double[] { 1.0, 0.0, 0.0 }; // Unit vector
+        problem.AddParameterBlock(point, point.Length);
+        
+        using var manifold = new SphereManifold(dimension: 3);
+        problem.SetManifold(point, manifold);
+        
+        // Should not throw
+        Assert.Pass();
+    }
+
+    [Test]
+    public void LineManifold_ShouldCreate()
+    {
+        using var manifold = new LineManifold(dimension: 3);
+        Assert.That(manifold, Is.Not.Null);
+    }
+
+    [Test]
+    public void EuclideanManifold_ShouldCreate()
+    {
+        using var manifold = new EuclideanManifold(dimension: 3);
+        Assert.That(manifold, Is.Not.Null);
+    }
+
+    [Test]
+    public void SubsetManifold_ShouldCreate()
+    {
+        var constantSubset = new int[] { 0, 2 }; // Fix indices 0 and 2
+        using var manifold = new SubsetManifold(constantSubset, ambientSize: 4);
+        Assert.That(manifold, Is.Not.Null);
+    }
+
+    [Test]
+    public void SubsetManifold_WithProblem_ShouldWork()
+    {
+        using var problem = new Problem();
+        var parameters = new double[] { 1.0, 2.0, 3.0, 4.0 };
+        problem.AddParameterBlock(parameters, parameters.Length);
+        
+        var constantSubset = new int[] { 0, 2 };
+        using var manifold = new SubsetManifold(constantSubset, ambientSize: 4);
+        problem.SetManifold(parameters, manifold);
+        
+        // Should not throw
+        Assert.Pass();
+    }
+
+    [Test]
+    public void ProductManifold_ShouldCreate()
+    {
+        using var quaternionManifold = new QuaternionManifold();
+        using var euclideanManifold = new EuclideanManifold(dimension: 3);
+        
+        var manifolds = new Manifold[] { quaternionManifold, euclideanManifold };
+        using var productManifold = new ProductManifold(manifolds);
+        
+        Assert.That(productManifold, Is.Not.Null);
+    }
+
+    [Test]
+    public void ProductManifold_WithProblem_ShouldWork()
+    {
+        using var problem = new Problem();
+        // 4 for quaternion + 3 for translation = 7
+        var pose = new double[] { 0.0, 0.0, 0.0, 1.0, 1.0, 2.0, 3.0 };
+        problem.AddParameterBlock(pose, pose.Length);
+        
+        using var quaternionManifold = new QuaternionManifold();
+        using var euclideanManifold = new EuclideanManifold(dimension: 3);
+        var manifolds = new Manifold[] { quaternionManifold, euclideanManifold };
+        
+        using var productManifold = new ProductManifold(manifolds);
+        problem.SetManifold(pose, productManifold);
+        
+        // Should not throw
+        Assert.Pass();
+    }
+
+    [Test]
+    public void MultipleManifolds_ShouldWork()
+    {
+        using var problem = new Problem();
+        
+        var quaternion1 = new double[] { 0.0, 0.0, 0.0, 1.0 };
+        var quaternion2 = new double[] { 0.0, 0.0, 0.0, 1.0 };
+        problem.AddParameterBlock(quaternion1, quaternion1.Length);
+        problem.AddParameterBlock(quaternion2, quaternion2.Length);
+        
+        using var manifold1 = new QuaternionManifold();
+        using var manifold2 = new QuaternionManifold();
+        
+        problem.SetManifold(quaternion1, manifold1);
+        problem.SetManifold(quaternion2, manifold2);
+        
+        // Should not throw
+        Assert.Pass();
+    }
+}