138 lines
4.3 KiB
C#
138 lines
4.3 KiB
C#
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();
|
|
}
|
|
}
|