Initial commit
This commit is contained in:
@@ -0,0 +1,310 @@
|
||||
using CeresSharp;
|
||||
using CeresSharp.Enums;
|
||||
|
||||
namespace CeresSharp.Test;
|
||||
|
||||
[TestFixture]
|
||||
public class AutoDiffManifoldTests
|
||||
{
|
||||
[Test]
|
||||
public void AutoDiffManifold_ShouldCreate()
|
||||
{
|
||||
// Euclidean manifold: Plus = x + delta, Minus = y - x
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
Assert.That(manifold, Is.Not.Null);
|
||||
Assert.That(manifold.AmbientSize, Is.EqualTo(3));
|
||||
Assert.That(manifold.TangentSize, Is.EqualTo(3));
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_Plus_ShouldWork()
|
||||
{
|
||||
// Test Plus operation: x + delta → x_plus_delta
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
// Test via Problem integration
|
||||
using var problem = new Problem();
|
||||
var parameters = new double[] { 1.0, 2.0, 3.0 };
|
||||
problem.AddParameterBlock(parameters, parameters.Length);
|
||||
problem.SetManifold(parameters, manifold);
|
||||
|
||||
// Verify manifold was set
|
||||
Assert.That(problem.HasManifold(parameters), Is.True);
|
||||
Assert.That(problem.GetParameterBlockTangentSize(parameters), Is.EqualTo(3));
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_Minus_ShouldWork()
|
||||
{
|
||||
// Test Minus operation: y - x → y_minus_x
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
// Test via Problem integration
|
||||
using var problem = new Problem();
|
||||
var parameters = new double[] { 1.0, 2.0, 3.0 };
|
||||
problem.AddParameterBlock(parameters, parameters.Length);
|
||||
problem.SetManifold(parameters, manifold);
|
||||
|
||||
// Verify manifold dimensions
|
||||
Assert.That(manifold.AmbientSize, Is.EqualTo(3));
|
||||
Assert.That(manifold.TangentSize, Is.EqualTo(3));
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_WithProblem_ShouldWork()
|
||||
{
|
||||
// Based on C test: Euclidean manifold integration
|
||||
using var problem = new Problem();
|
||||
var parameters = new double[] { 1.0, 2.0, 3.0 };
|
||||
problem.AddParameterBlock(parameters, parameters.Length);
|
||||
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
problem.SetManifold(parameters, manifold);
|
||||
|
||||
// Verify manifold was set
|
||||
Assert.That(problem.HasManifold(parameters), Is.True);
|
||||
Assert.That(problem.GetParameterBlockTangentSize(parameters), Is.EqualTo(3));
|
||||
Assert.That(problem.GetParameterBlockSize(parameters), Is.EqualTo(3));
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_InvalidSizes_ShouldThrow()
|
||||
{
|
||||
// Test invalid ambient size
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
{
|
||||
new AutoDiffManifold(
|
||||
ambientSize: 0,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) => true,
|
||||
minus: (y, x, yMinusX) => true);
|
||||
});
|
||||
|
||||
// Test invalid tangent size
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
{
|
||||
new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 0,
|
||||
plus: (x, delta, xPlusDelta) => true,
|
||||
minus: (y, x, yMinusX) => true);
|
||||
});
|
||||
|
||||
// Test tangent size > ambient size
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
{
|
||||
new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 4,
|
||||
plus: (x, delta, xPlusDelta) => true,
|
||||
minus: (y, x, yMinusX) => true);
|
||||
});
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_NullCallbacks_ShouldThrow()
|
||||
{
|
||||
// Test null Plus callback
|
||||
Assert.Throws<ArgumentNullException>(() =>
|
||||
{
|
||||
new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: null!,
|
||||
minus: (y, x, yMinusX) => true);
|
||||
});
|
||||
|
||||
// Test null Minus callback
|
||||
Assert.Throws<ArgumentNullException>(() =>
|
||||
{
|
||||
new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) => true,
|
||||
minus: null!);
|
||||
});
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_Dispose_ShouldNotCrash()
|
||||
{
|
||||
var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
// Dispose should not crash
|
||||
manifold.Dispose();
|
||||
|
||||
// Verify disposed
|
||||
Assert.Pass("Dispose completed without crash");
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_UsingStatement_ShouldWork()
|
||||
{
|
||||
// Test with using statement
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
// Should not throw
|
||||
Assert.That(manifold, Is.Not.Null);
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_DifferentSizes_ShouldWork()
|
||||
{
|
||||
// Test with different ambient and tangent sizes (e.g., quaternion-like: 4D ambient, 3D tangent)
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 4,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
// Simple example: just copy x and add delta to first 3 elements
|
||||
for (int i = 0; i < 4; i++)
|
||||
xPlusDelta[i] = x[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] += delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
// Simple example: difference of first 3 elements
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
Assert.That(manifold.AmbientSize, Is.EqualTo(4));
|
||||
Assert.That(manifold.TangentSize, Is.EqualTo(3));
|
||||
}
|
||||
|
||||
[Test]
|
||||
public void AutoDiffManifold_WithCostFunction_ShouldWork()
|
||||
{
|
||||
// Test AutoDiffManifold with a cost function (integration test)
|
||||
using var problem = new Problem();
|
||||
var parameters = new double[] { 1.0, 2.0, 3.0 };
|
||||
problem.AddParameterBlock(parameters, parameters.Length);
|
||||
|
||||
using var manifold = new AutoDiffManifold(
|
||||
ambientSize: 3,
|
||||
tangentSize: 3,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
xPlusDelta[i] = x[i] + delta[i];
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
yMinusX[i] = y[i] - x[i];
|
||||
return true;
|
||||
});
|
||||
|
||||
problem.SetManifold(parameters, manifold);
|
||||
|
||||
// Add a cost function
|
||||
var costFunction = new AutoDiffCostFunction(
|
||||
(parameters, residuals) =>
|
||||
{
|
||||
// Simple cost: minimize ||parameters||^2
|
||||
residuals[0] = parameters[0][0] * parameters[0][0] +
|
||||
parameters[0][1] * parameters[0][1] +
|
||||
parameters[0][2] * parameters[0][2];
|
||||
return true;
|
||||
},
|
||||
numResiduals: 1,
|
||||
parameterBlockSizes: new[] { 3 });
|
||||
|
||||
problem.AddResidualBlock(costFunction, lossFunction: null,
|
||||
parameterBlocks: new[] { parameters });
|
||||
|
||||
// Verify problem setup
|
||||
Assert.That(problem.NumResidualBlocks, Is.EqualTo(1));
|
||||
Assert.That(problem.HasManifold(parameters), Is.True);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user