Initial commit
This commit is contained in:
@@ -0,0 +1,271 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// AutoDiff manifold for custom manifolds defined via Plus and Minus operations.
|
||||
/// This is the replacement for AutoDiffLocalParameterization in Ceres 2.2.0.
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// <para>
|
||||
/// AutoDiffManifold allows you to define custom manifolds by providing Plus and Minus operations
|
||||
/// as C# delegates. The Jacobians are computed automatically using numeric differentiation.
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// Use cases:
|
||||
/// - ConstantYawQuaternion: Quaternion with constant yaw angle (Cartographer IMU extrapolation)
|
||||
/// - Custom geometric constraints
|
||||
/// - Domain-specific manifolds
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// Example (Euclidean manifold):
|
||||
/// <code>
|
||||
/// 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;
|
||||
/// });
|
||||
/// </code>
|
||||
/// </para>
|
||||
/// </remarks>
|
||||
public sealed class AutoDiffManifold : Manifold
|
||||
{
|
||||
private readonly GCHandle _wrapperHandle;
|
||||
private readonly int _ambientSize;
|
||||
private readonly int _tangentSize;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Delegate for Plus operation: x + delta → x_plus_delta
|
||||
/// </summary>
|
||||
/// <param name="x">Point on manifold (ambient_size elements)</param>
|
||||
/// <param name="delta">Tangent vector (tangent_size elements)</param>
|
||||
/// <param name="xPlusDelta">Output point on manifold (ambient_size elements)</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
public delegate bool PlusOperation(double[] x, double[] delta, double[] xPlusDelta);
|
||||
|
||||
/// <summary>
|
||||
/// Delegate for Minus operation: y - x → y_minus_x
|
||||
/// </summary>
|
||||
/// <param name="y">Point on manifold (ambient_size elements)</param>
|
||||
/// <param name="x">Point on manifold (ambient_size elements)</param>
|
||||
/// <param name="yMinusX">Output tangent vector (tangent_size elements)</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
public delegate bool MinusOperation(double[] y, double[] x, double[] yMinusX);
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new AutoDiff manifold.
|
||||
/// </summary>
|
||||
/// <param name="ambientSize">Dimension of ambient space (must be positive)</param>
|
||||
/// <param name="tangentSize">Dimension of tangent space (must be positive, ≤ ambientSize)</param>
|
||||
/// <param name="plus">Plus operation: x + delta → x_plus_delta</param>
|
||||
/// <param name="minus">Minus operation: y - x → y_minus_x</param>
|
||||
/// <exception cref="ArgumentNullException">Thrown when plus or minus is null</exception>
|
||||
/// <exception cref="ArgumentException">Thrown when sizes are invalid</exception>
|
||||
/// <exception cref="CeresException">Thrown when native creation fails</exception>
|
||||
public AutoDiffManifold(
|
||||
int ambientSize,
|
||||
int tangentSize,
|
||||
PlusOperation plus,
|
||||
MinusOperation minus)
|
||||
: base(CreateHandle(ambientSize, tangentSize, plus, minus, out var wrapperHandle))
|
||||
{
|
||||
_ambientSize = ambientSize;
|
||||
_tangentSize = tangentSize;
|
||||
_wrapperHandle = wrapperHandle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the ambient size.
|
||||
/// </summary>
|
||||
public new int AmbientSize => _ambientSize;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the tangent size.
|
||||
/// </summary>
|
||||
public new int TangentSize => _tangentSize;
|
||||
|
||||
public new void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
// Call base Dispose first (frees native handle)
|
||||
base.Dispose();
|
||||
|
||||
// Now safe to free wrapper handle after native handle is freed
|
||||
try
|
||||
{
|
||||
if (_wrapperHandle.IsAllocated)
|
||||
{
|
||||
var wrapper = (CallbackWrapper)_wrapperHandle.Target!;
|
||||
|
||||
// Free native callback handles
|
||||
if (wrapper.NativeCallbackHandles != null)
|
||||
{
|
||||
foreach (var handle in wrapper.NativeCallbackHandles)
|
||||
{
|
||||
if (handle.IsAllocated)
|
||||
handle.Free();
|
||||
}
|
||||
}
|
||||
|
||||
_wrapperHandle.Free();
|
||||
}
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handles may already be freed
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
|
||||
private static ManifoldHandle CreateHandle(
|
||||
int ambientSize,
|
||||
int tangentSize,
|
||||
PlusOperation plus,
|
||||
MinusOperation minus,
|
||||
out GCHandle wrapperHandle)
|
||||
{
|
||||
if (ambientSize <= 0)
|
||||
throw new ArgumentException("Ambient size must be positive", nameof(ambientSize));
|
||||
if (tangentSize <= 0)
|
||||
throw new ArgumentException("Tangent size must be positive", nameof(tangentSize));
|
||||
if (tangentSize > ambientSize)
|
||||
throw new ArgumentException("Tangent size must be less than or equal to ambient size", nameof(tangentSize));
|
||||
if (plus == null)
|
||||
throw new ArgumentNullException(nameof(plus));
|
||||
if (minus == null)
|
||||
throw new ArgumentNullException(nameof(minus));
|
||||
|
||||
// Create wrapper struct to store callbacks and sizes
|
||||
var wrapper = new CallbackWrapper
|
||||
{
|
||||
Plus = plus,
|
||||
Minus = minus,
|
||||
AmbientSize = ambientSize,
|
||||
TangentSize = tangentSize
|
||||
};
|
||||
|
||||
// Pin the wrapper
|
||||
wrapperHandle = GCHandle.Alloc(wrapper);
|
||||
|
||||
// Create native Plus callback
|
||||
var plusCallback = new CeresNative.CeresAutoDiffManifoldPlus((userData, xPtr, deltaPtr, xPlusDeltaPtr) =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var handle = GCHandle.FromIntPtr(userData);
|
||||
var wrapperObj = (CallbackWrapper)handle.Target!;
|
||||
|
||||
// Marshal arrays
|
||||
var x = new double[wrapperObj.AmbientSize];
|
||||
var delta = new double[wrapperObj.TangentSize];
|
||||
var xPlusDelta = new double[wrapperObj.AmbientSize];
|
||||
|
||||
Marshal.Copy(xPtr, x, 0, wrapperObj.AmbientSize);
|
||||
Marshal.Copy(deltaPtr, delta, 0, wrapperObj.TangentSize);
|
||||
|
||||
// Call C# delegate
|
||||
var success = wrapperObj.Plus(x, delta, xPlusDelta);
|
||||
|
||||
// Marshal result back
|
||||
if (success)
|
||||
{
|
||||
Marshal.Copy(xPlusDelta, 0, xPlusDeltaPtr, wrapperObj.AmbientSize);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Return failure on any exception
|
||||
return 0;
|
||||
}
|
||||
});
|
||||
|
||||
// Create native Minus callback
|
||||
var minusCallback = new CeresNative.CeresAutoDiffManifoldMinus((userData, yPtr, xPtr, yMinusXPtr) =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var handle = GCHandle.FromIntPtr(userData);
|
||||
var wrapperObj = (CallbackWrapper)handle.Target!;
|
||||
|
||||
// Marshal arrays
|
||||
var y = new double[wrapperObj.AmbientSize];
|
||||
var x = new double[wrapperObj.AmbientSize];
|
||||
var yMinusX = new double[wrapperObj.TangentSize];
|
||||
|
||||
Marshal.Copy(yPtr, y, 0, wrapperObj.AmbientSize);
|
||||
Marshal.Copy(xPtr, x, 0, wrapperObj.AmbientSize);
|
||||
|
||||
// Call C# delegate
|
||||
var success = wrapperObj.Minus(y, x, yMinusX);
|
||||
|
||||
// Marshal result back
|
||||
if (success)
|
||||
{
|
||||
Marshal.Copy(yMinusX, 0, yMinusXPtr, wrapperObj.TangentSize);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Return failure on any exception
|
||||
return 0;
|
||||
}
|
||||
});
|
||||
|
||||
// Pin the native callbacks
|
||||
var plusHandle = GCHandle.Alloc(plusCallback);
|
||||
var minusHandle = GCHandle.Alloc(minusCallback);
|
||||
wrapper.NativeCallbackHandles = new[] { plusHandle, minusHandle };
|
||||
|
||||
// Call native function
|
||||
var handle = CeresNative.ceres_wrapper_create_autodiff_manifold(
|
||||
ambientSize,
|
||||
tangentSize,
|
||||
plusCallback,
|
||||
minusCallback,
|
||||
GCHandle.ToIntPtr(wrapperHandle)); // Pass wrapper handle as user_data
|
||||
|
||||
if (handle == IntPtr.Zero)
|
||||
{
|
||||
// Cleanup on failure
|
||||
plusHandle.Free();
|
||||
minusHandle.Free();
|
||||
wrapperHandle.Free();
|
||||
throw new Exceptions.CeresException(Exceptions.CeresErrorCode.OutOfMemory, "Failed to create AutoDiff manifold");
|
||||
}
|
||||
|
||||
return ManifoldHandle.Create(handle);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Wrapper struct to store callbacks and sizes.
|
||||
/// </summary>
|
||||
private class CallbackWrapper
|
||||
{
|
||||
public PlusOperation Plus = null!;
|
||||
public MinusOperation Minus = null!;
|
||||
public int AmbientSize;
|
||||
public int TangentSize;
|
||||
public GCHandle[]? NativeCallbackHandles;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
using System;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// BiCubic interpolator for 2D grid data.
|
||||
/// </summary>
|
||||
public sealed class BiCubicInterpolator : IDisposable
|
||||
{
|
||||
private readonly InterpolatorHandle _handle;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new BiCubic interpolator from 2D grid data.
|
||||
/// </summary>
|
||||
/// <param name="data">The data array in row-major order (size = rows * cols).</param>
|
||||
/// <param name="rows">The number of rows.</param>
|
||||
/// <param name="cols">The number of columns.</param>
|
||||
public BiCubicInterpolator(double[] data, int rows, int cols)
|
||||
{
|
||||
if (data == null)
|
||||
throw new ArgumentNullException(nameof(data));
|
||||
if (rows <= 0)
|
||||
throw new ArgumentException("Rows must be positive", nameof(rows));
|
||||
if (cols <= 0)
|
||||
throw new ArgumentException("Cols must be positive", nameof(cols));
|
||||
if (data.Length < rows * cols)
|
||||
throw new ArgumentException("Data array length must be at least rows * cols", nameof(data));
|
||||
|
||||
unsafe
|
||||
{
|
||||
fixed (double* ptr = data)
|
||||
{
|
||||
var handle = CeresNative.ceres_wrapper_create_bicubic_interpolator(
|
||||
(IntPtr)ptr,
|
||||
rows,
|
||||
cols);
|
||||
_handle = InterpolatorHandle.CreateBicubic(handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the interpolator at point (x, y).
|
||||
/// </summary>
|
||||
/// <param name="x">X coordinate in grid space (0 <= x < cols).</param>
|
||||
/// <param name="y">Y coordinate in grid space (0 <= y < rows).</param>
|
||||
/// <param name="value">Output value at (x, y).</param>
|
||||
/// <param name="gradientX">Output gradient in X direction (can be null).</param>
|
||||
/// <param name="gradientY">Output gradient in Y direction (can be null).</param>
|
||||
public void Evaluate(double x, double y, out double value, out double? gradientX, out double? gradientY)
|
||||
{
|
||||
unsafe
|
||||
{
|
||||
double val = 0, gradX = 0, gradY = 0;
|
||||
|
||||
CeresNative.ceres_wrapper_bicubic_interpolator_evaluate(
|
||||
_handle.DangerousGetHandle(),
|
||||
x,
|
||||
y,
|
||||
(IntPtr)(&val),
|
||||
(IntPtr)(&gradX),
|
||||
(IntPtr)(&gradY));
|
||||
|
||||
value = val;
|
||||
gradientX = gradX;
|
||||
gradientY = gradY;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the interpolator at point (x, y) without gradients.
|
||||
/// </summary>
|
||||
public double Evaluate(double x, double y)
|
||||
{
|
||||
Evaluate(x, y, out var value, out _, out _);
|
||||
return value;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_handle?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Callback delegates and helpers for Ceres Solver.
|
||||
/// </summary>
|
||||
public static class Callbacks
|
||||
{
|
||||
/// <summary>
|
||||
/// Delegate for iteration callback.
|
||||
/// Called at the end of each iteration.
|
||||
/// </summary>
|
||||
/// <param name="summary">The current solver summary.</param>
|
||||
/// <returns>True to continue, false to stop optimization.</returns>
|
||||
public delegate bool IterationCallback(SolverSummary summary);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods for setting callbacks.
|
||||
/// </summary>
|
||||
public static class SolverOptionsExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Sets an iteration callback for the solver options.
|
||||
/// </summary>
|
||||
/// <param name="options">The solver options.</param>
|
||||
/// <param name="callback">The iteration callback.</param>
|
||||
public static void SetIterationCallback(this SolverOptions options, Callbacks.IterationCallback callback)
|
||||
{
|
||||
if (options == null)
|
||||
throw new ArgumentNullException(nameof(options));
|
||||
if (callback == null)
|
||||
throw new ArgumentNullException(nameof(callback));
|
||||
|
||||
// Pin the callback delegate
|
||||
var callbackHandle = GCHandle.Alloc(callback);
|
||||
|
||||
// Create native callback wrapper
|
||||
var nativeCallback = new CeresNative.CeresIterationCallback((userData, summaryPtr) =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var handle = GCHandle.FromIntPtr(userData);
|
||||
var csCallback = (Callbacks.IterationCallback)handle.Target!;
|
||||
|
||||
// Create a temporary SolverSummary wrapper
|
||||
// Note: We need to be careful here - the summary is owned by the solver
|
||||
// We'll create a temporary handle that doesn't own the summary
|
||||
var summaryHandle = new SolverSummaryHandle(summaryPtr, false);
|
||||
var summary = new SolverSummary(summaryHandle);
|
||||
|
||||
// Call C# callback
|
||||
var shouldContinue = csCallback(summary);
|
||||
|
||||
// Don't dispose the summary - it's owned by the solver
|
||||
return shouldContinue ? 1 : 0;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return 0; // Stop on error
|
||||
}
|
||||
});
|
||||
|
||||
// Pin the native callback
|
||||
var nativeCallbackHandle = GCHandle.Alloc(nativeCallback);
|
||||
|
||||
CeresNative.ceres_wrapper_solver_options_set_iteration_callback(
|
||||
options.Handle.DangerousGetHandle(),
|
||||
nativeCallback,
|
||||
GCHandle.ToIntPtr(callbackHandle));
|
||||
|
||||
// Track handles for cleanup when SolverOptions is disposed
|
||||
options.SetIterationCallbackHandles(callbackHandle, nativeCallbackHandle);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Exceptions;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Provides information about the Ceres Solver library, including version and build configuration.
|
||||
/// </summary>
|
||||
public static class CeresLibraryInfo
|
||||
{
|
||||
private const int BufferSize = 4096;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the Ceres version string (e.g., "2.2.0").
|
||||
/// </summary>
|
||||
public static string GetVersionString()
|
||||
{
|
||||
var versionPtr = CeresNative.ceres_wrapper_get_version_string();
|
||||
if (versionPtr == IntPtr.Zero)
|
||||
{
|
||||
return "Unknown";
|
||||
}
|
||||
return Marshal.PtrToStringAnsi(versionPtr) ?? "Unknown";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the Ceres major version number.
|
||||
/// </summary>
|
||||
public static int GetVersionMajor()
|
||||
{
|
||||
return CeresNative.ceres_wrapper_get_version_major();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the Ceres minor version number.
|
||||
/// </summary>
|
||||
public static int GetVersionMinor()
|
||||
{
|
||||
return CeresNative.ceres_wrapper_get_version_minor();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the Ceres revision version number.
|
||||
/// </summary>
|
||||
public static int GetVersionRevision()
|
||||
{
|
||||
return CeresNative.ceres_wrapper_get_version_revision();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the detailed Ceres version string with build configuration.
|
||||
/// </summary>
|
||||
/// <returns>The detailed version string, or null if an error occurred.</returns>
|
||||
public static string? GetDetailedVersionString()
|
||||
{
|
||||
var buffer = new StringBuilder(BufferSize);
|
||||
var errorCode = CeresNative.ceres_wrapper_get_detailed_version_string(buffer, BufferSize);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return buffer.ToString();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets comprehensive library information including version, build configuration, and features.
|
||||
/// </summary>
|
||||
/// <returns>The library information string, or null if an error occurred.</returns>
|
||||
public static string? GetLibraryInfo()
|
||||
{
|
||||
var buffer = new StringBuilder(BufferSize);
|
||||
var errorCode = CeresNative.ceres_wrapper_get_library_info(buffer, BufferSize);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return buffer.ToString();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Prints the Ceres library information to the console.
|
||||
/// </summary>
|
||||
public static void PrintLibraryInfo()
|
||||
{
|
||||
|
||||
// Basic version info
|
||||
|
||||
// Detailed version
|
||||
var detailedVersion = GetDetailedVersionString();
|
||||
if (detailedVersion != null)
|
||||
{
|
||||
}
|
||||
|
||||
// Full library info
|
||||
var libraryInfo = GetLibraryInfo();
|
||||
if (libraryInfo != null)
|
||||
{
|
||||
}
|
||||
else
|
||||
{
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets a formatted string containing all Ceres library information.
|
||||
/// </summary>
|
||||
/// <returns>A formatted string with all library information.</returns>
|
||||
public static string GetFormattedInfo()
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
sb.AppendLine("========================================");
|
||||
sb.AppendLine("Ceres Solver Library Information");
|
||||
sb.AppendLine("========================================");
|
||||
sb.AppendLine();
|
||||
|
||||
// Basic version info
|
||||
sb.AppendLine($"Version: {GetVersionString()}");
|
||||
sb.AppendLine($" Major: {GetVersionMajor()}");
|
||||
sb.AppendLine($" Minor: {GetVersionMinor()}");
|
||||
sb.AppendLine($" Revision: {GetVersionRevision()}");
|
||||
sb.AppendLine();
|
||||
|
||||
// Detailed version
|
||||
var detailedVersion = GetDetailedVersionString();
|
||||
if (detailedVersion != null)
|
||||
{
|
||||
sb.AppendLine($"Detailed Version: {detailedVersion}");
|
||||
sb.AppendLine();
|
||||
}
|
||||
|
||||
// Full library info
|
||||
var libraryInfo = GetLibraryInfo();
|
||||
if (libraryInfo != null)
|
||||
{
|
||||
sb.AppendLine(libraryInfo);
|
||||
}
|
||||
else
|
||||
{
|
||||
sb.AppendLine("(Unable to retrieve detailed library information)");
|
||||
}
|
||||
|
||||
sb.AppendLine("========================================");
|
||||
|
||||
return sb.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,219 @@
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Constants for Ceres Solver default values and mathematical constants.
|
||||
/// </summary>
|
||||
public static class Constants
|
||||
{
|
||||
/// <summary>
|
||||
/// Mathematical constant Pi.
|
||||
/// </summary>
|
||||
public const double Pi = 3.141592653589793238462643383279502884;
|
||||
|
||||
// ============================================================================
|
||||
// SolverOptions Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum number of iterations.
|
||||
/// </summary>
|
||||
public const int DefaultMaxNumIterations = 50;
|
||||
|
||||
/// <summary>
|
||||
/// Default number of threads.
|
||||
/// </summary>
|
||||
public const int DefaultNumThreads = 1;
|
||||
|
||||
/// <summary>
|
||||
/// Default function tolerance.
|
||||
/// </summary>
|
||||
public const double DefaultFunctionTolerance = 1e-6;
|
||||
|
||||
/// <summary>
|
||||
/// Default gradient tolerance.
|
||||
/// </summary>
|
||||
public const double DefaultGradientTolerance = 1e-10;
|
||||
|
||||
/// <summary>
|
||||
/// Default parameter tolerance.
|
||||
/// </summary>
|
||||
public const double DefaultParameterTolerance = 1e-8;
|
||||
|
||||
/// <summary>
|
||||
/// Default initial trust region radius.
|
||||
/// </summary>
|
||||
public const double DefaultInitialTrustRegionRadius = 1e4;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum trust region radius.
|
||||
/// </summary>
|
||||
public const double DefaultMaxTrustRegionRadius = 1e16;
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum trust region radius.
|
||||
/// </summary>
|
||||
public const double DefaultMinTrustRegionRadius = 1e-32;
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum relative decrease.
|
||||
/// </summary>
|
||||
public const double DefaultMinRelativeDecrease = 1e-3;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum solver time in seconds (effectively unlimited).
|
||||
/// </summary>
|
||||
public const double DefaultMaxSolverTimeInSeconds = 1e9;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum L-BFGS rank.
|
||||
/// </summary>
|
||||
public const int DefaultMaxLbfgsRank = 20;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum consecutive non-monotonic steps.
|
||||
/// </summary>
|
||||
public const int DefaultMaxConsecutiveNonmonotonicSteps = 5;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum number of consecutive invalid steps.
|
||||
/// </summary>
|
||||
public const int DefaultMaxNumConsecutiveInvalidSteps = 5;
|
||||
|
||||
// ============================================================================
|
||||
// Line Search Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum line search step size.
|
||||
/// </summary>
|
||||
public const double DefaultMinLineSearchStepSize = 1e-9;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum line search step contraction.
|
||||
/// </summary>
|
||||
public const double DefaultMaxLineSearchStepContraction = 1e-3;
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum line search step contraction.
|
||||
/// </summary>
|
||||
public const double DefaultMinLineSearchStepContraction = 0.6;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum number of line search step size iterations.
|
||||
/// </summary>
|
||||
public const int DefaultMaxNumLineSearchStepSizeIterations = 20;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum number of line search direction restarts.
|
||||
/// </summary>
|
||||
public const int DefaultMaxNumLineSearchDirectionRestarts = 5;
|
||||
|
||||
/// <summary>
|
||||
/// Default line search sufficient function decrease.
|
||||
/// </summary>
|
||||
public const double DefaultLineSearchSufficientFunctionDecrease = 1e-4;
|
||||
|
||||
/// <summary>
|
||||
/// Default line search sufficient curvature decrease.
|
||||
/// </summary>
|
||||
public const double DefaultLineSearchSufficientCurvatureDecrease = 0.9;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum line search step expansion.
|
||||
/// </summary>
|
||||
public const double DefaultMaxLineSearchStepExpansion = 10.0;
|
||||
|
||||
// ============================================================================
|
||||
// Trust Region Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum LM diagonal.
|
||||
/// </summary>
|
||||
public const double DefaultMinLmDiagonal = 1e-6;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum LM diagonal.
|
||||
/// </summary>
|
||||
public const double DefaultMaxLmDiagonal = 1e32;
|
||||
|
||||
// ============================================================================
|
||||
// Linear Solver Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum linear solver iterations.
|
||||
/// </summary>
|
||||
public const int DefaultMaxLinearSolverIterations = 500;
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum linear solver iterations.
|
||||
/// </summary>
|
||||
public const int DefaultMinLinearSolverIterations = 0;
|
||||
|
||||
/// <summary>
|
||||
/// Default linear solver tolerance.
|
||||
/// </summary>
|
||||
public const double DefaultLinearSolverTolerance = 1e-20;
|
||||
|
||||
// ============================================================================
|
||||
// Inner Iterations Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default inner iteration tolerance.
|
||||
/// </summary>
|
||||
public const double DefaultInnerIterationTolerance = 1e-3;
|
||||
|
||||
// ============================================================================
|
||||
// Covariance Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default minimum reciprocal condition number for covariance.
|
||||
/// </summary>
|
||||
public const double DefaultMinReciprocalConditionNumber = 1e-14;
|
||||
|
||||
// ============================================================================
|
||||
// Gradient Checker Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default gradient check relative precision.
|
||||
/// </summary>
|
||||
public const double DefaultGradientCheckRelativePrecision = 1e-4;
|
||||
|
||||
/// <summary>
|
||||
/// Default numeric derivative relative step size.
|
||||
/// </summary>
|
||||
public const double DefaultNumericDerivativeRelativeStepSize = 1e-6;
|
||||
|
||||
// ============================================================================
|
||||
// Numeric Diff Default Values
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Default relative step size for numeric differentiation.
|
||||
/// </summary>
|
||||
public const double DefaultNumericDiffRelativeStepSize = 1e-6;
|
||||
|
||||
/// <summary>
|
||||
/// Default Ridders relative initial step size.
|
||||
/// </summary>
|
||||
public const double DefaultRiddersRelativeInitialStepSize = 1e-2;
|
||||
|
||||
/// <summary>
|
||||
/// Default maximum number of Ridders extrapolations.
|
||||
/// </summary>
|
||||
public const int DefaultMaxNumRiddersExtrapolations = 10;
|
||||
|
||||
/// <summary>
|
||||
/// Default Ridders epsilon.
|
||||
/// </summary>
|
||||
public const double DefaultRiddersEpsilon = 1e-12;
|
||||
|
||||
/// <summary>
|
||||
/// Default Ridders step shrink factor.
|
||||
/// </summary>
|
||||
public const double DefaultRiddersStepShrinkFactor = 2.0;
|
||||
}
|
||||
@@ -0,0 +1,607 @@
|
||||
using System;
|
||||
using System.Linq;
|
||||
using System.Runtime.InteropServices;
|
||||
using CeresSharp.Enums;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Base class for cost functions in Ceres Solver.
|
||||
/// </summary>
|
||||
public abstract class CostFunction : IDisposable
|
||||
{
|
||||
private readonly CostFunctionHandle _handle;
|
||||
private GCHandle _callbackHandle;
|
||||
private GCHandle? _nativeCallbackHandle;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Initializes a new instance of the CostFunction class.
|
||||
/// </summary>
|
||||
internal CostFunction(CostFunctionHandle handle)
|
||||
{
|
||||
_handle = handle ?? throw new ArgumentNullException(nameof(handle));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the native handle.
|
||||
/// </summary>
|
||||
internal IntPtr Handle => _handle.DangerousGetHandle();
|
||||
|
||||
/// <summary>
|
||||
/// Sets the callback handle for pinning the delegate.
|
||||
/// </summary>
|
||||
protected void SetCallbackHandle(GCHandle handle)
|
||||
{
|
||||
if (_callbackHandle.IsAllocated)
|
||||
_callbackHandle.Free();
|
||||
_callbackHandle = handle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets the native callback handle for cleanup.
|
||||
/// </summary>
|
||||
protected void SetNativeCallbackHandle(GCHandle handle)
|
||||
{
|
||||
if (_nativeCallbackHandle.HasValue && _nativeCallbackHandle.Value.IsAllocated)
|
||||
_nativeCallbackHandle.Value.Free();
|
||||
_nativeCallbackHandle = handle;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
// Suppress finalizer to prevent crash in finalizer thread
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
// IMPORTANT: Free native handle FIRST before unpinning GCHandles
|
||||
// This ensures native code has finished using the callbacks
|
||||
|
||||
// Dispose SafeHandle and suppress finalization to prevent double-free
|
||||
if (_handle != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
_handle.Dispose();
|
||||
// Suppress finalization to prevent crash in finalizer thread
|
||||
GC.SuppressFinalize(_handle);
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors during disposal
|
||||
}
|
||||
}
|
||||
|
||||
// Now safe to unpin callbacks after native handle is freed
|
||||
// CRITICAL: Only free GCHandles in Dispose(), never in finalizer
|
||||
// Native code may still be using callbacks during finalization
|
||||
try
|
||||
{
|
||||
if (_callbackHandle.IsAllocated)
|
||||
_callbackHandle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (_nativeCallbackHandle.HasValue && _nativeCallbackHandle.Value.IsAllocated)
|
||||
_nativeCallbackHandle.Value.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Finalizer path - DO NOT free GCHandles here!
|
||||
// Native code may still be using them. Only free native handle.
|
||||
// SafeHandle finalizer will handle native handle cleanup.
|
||||
// GCHandles will be freed when objects are GC'd (after native code is done).
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether this cost function has been added to a Problem and is now owned by it.
|
||||
/// If true, the cost function will be freed when the Problem is disposed.
|
||||
/// If false, the caller is responsible for disposing.
|
||||
/// </summary>
|
||||
public bool IsOwnedByProblem { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Marks this cost function as owned by a Problem, preventing standalone cleanup.
|
||||
/// </summary>
|
||||
internal void MarkOwnedByProblem()
|
||||
{
|
||||
IsOwnedByProblem = true;
|
||||
_handle.MarkOwnedByProblem();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Protected virtual method to free additional GCHandles in derived classes.
|
||||
/// This is called by FreeGCHandlesAfterProblemDisposed() to allow derived classes
|
||||
/// to free their own GCHandles (e.g., _wrapperHandle in AutoDiffCostFunction).
|
||||
/// </summary>
|
||||
protected virtual void FreeAdditionalGCHandles()
|
||||
{
|
||||
// Base class has no additional handles
|
||||
// Derived classes override this to free their _wrapperHandle
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Internal method to free GCHandles after native Problem is disposed.
|
||||
/// This is called by Problem.Dispose() after native handle is freed to prevent memory leaks.
|
||||
///
|
||||
/// CRITICAL: After Problem is disposed, native CostFunction objects are already deleted by Ceres.
|
||||
/// However, the C# wrapper objects still hold GCHandles (for callbacks and wrappers) that MUST be freed
|
||||
/// to prevent memory leaks. Since CostFunctionHandle.ReleaseNativeHandle() is a no-op (to prevent double-free),
|
||||
/// it's safe to call Dispose() logic which will free GCHandles but not try to delete native handle.
|
||||
/// </summary>
|
||||
internal void FreeGCHandlesAfterProblemDisposed()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
// Free base class GCHandles (_callbackHandle, _nativeCallbackHandle)
|
||||
// Native handle is already freed by Ceres, so CostFunctionHandle.Dispose() will be a no-op
|
||||
try
|
||||
{
|
||||
if (_callbackHandle.IsAllocated)
|
||||
_callbackHandle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (_nativeCallbackHandle.HasValue && _nativeCallbackHandle.Value.IsAllocated)
|
||||
_nativeCallbackHandle.Value.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
|
||||
// Free additional GCHandles in derived classes (e.g., _wrapperHandle)
|
||||
try
|
||||
{
|
||||
FreeAdditionalGCHandles();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors during cleanup
|
||||
}
|
||||
|
||||
// Also dispose the handle (it's a no-op but ensures proper state)
|
||||
// CostFunctionHandle.ReleaseNativeHandle() is intentionally a no-op to prevent double-free
|
||||
try
|
||||
{
|
||||
_handle?.Dispose();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore - handle may already be disposed
|
||||
}
|
||||
|
||||
// Mark as disposed to prevent double-free
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Note: No finalizer for CostFunction
|
||||
// Problem owns cost functions, so they should be disposed when Problem is disposed
|
||||
// If not disposed, SafeHandle finalizer will cleanup native handle
|
||||
// But we don't want to cleanup GCHandles in finalizer as native code may still use them
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AutoDiff cost function with fixed parameter block sizes.
|
||||
/// </summary>
|
||||
public sealed class AutoDiffCostFunction : CostFunction
|
||||
{
|
||||
private readonly GCHandle _wrapperHandle;
|
||||
private readonly int[] _parameterBlockSizes;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new AutoDiff cost function.
|
||||
/// </summary>
|
||||
/// <param name="callback">The callback function to evaluate the cost.</param>
|
||||
/// <param name="numResiduals">The number of residuals.</param>
|
||||
/// <param name="parameterBlockSizes">Array of sizes for each parameter block.</param>
|
||||
public AutoDiffCostFunction(
|
||||
AutoDiffCostFunctionCallback callback,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes) : base(CreateHandle(callback, numResiduals, parameterBlockSizes, out var wrapperHandle, out var nativeCallbackHandle))
|
||||
{
|
||||
_wrapperHandle = wrapperHandle;
|
||||
_parameterBlockSizes = parameterBlockSizes;
|
||||
SetNativeCallbackHandle(nativeCallbackHandle);
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
try { if (_wrapperHandle.IsAllocated) _wrapperHandle.Free(); } catch { }
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Override to free _wrapperHandle when Problem is disposed.
|
||||
/// </summary>
|
||||
protected override void FreeAdditionalGCHandles()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_wrapperHandle.IsAllocated)
|
||||
_wrapperHandle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
}
|
||||
|
||||
private static CostFunctionHandle CreateHandle(
|
||||
AutoDiffCostFunctionCallback callback,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes,
|
||||
out GCHandle wrapperHandle,
|
||||
out GCHandle nativeCallbackHandle)
|
||||
{
|
||||
CostFunctionHelpers.ValidateCostFunctionParameters(callback, numResiduals, parameterBlockSizes);
|
||||
|
||||
// Create wrapper struct to store callback and parameter info
|
||||
var wrapper = new CostFunctionHelpers.AutoDiffCallbackWrapper
|
||||
{
|
||||
Callback = callback,
|
||||
NumResiduals = numResiduals,
|
||||
ParameterBlockSizes = parameterBlockSizes
|
||||
};
|
||||
|
||||
// Pin the wrapper
|
||||
wrapperHandle = GCHandle.Alloc(wrapper);
|
||||
|
||||
// Create native callback using helper
|
||||
var nativeCallback = CostFunctionHelpers.CreateAutoDiffCallback(wrapper);
|
||||
|
||||
// Pin the native callback
|
||||
nativeCallbackHandle = GCHandle.Alloc(nativeCallback);
|
||||
|
||||
try
|
||||
{
|
||||
var handle = CostFunctionHelpers.CreateAutoDiffHandle(
|
||||
nativeCallback,
|
||||
GCHandle.ToIntPtr(wrapperHandle),
|
||||
numResiduals,
|
||||
parameterBlockSizes,
|
||||
isDynamic: false);
|
||||
|
||||
return handle;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Cleanup on failure
|
||||
wrapperHandle.Free();
|
||||
nativeCallbackHandle.Free();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dynamic AutoDiff cost function with runtime-determined parameter block sizes.
|
||||
/// </summary>
|
||||
public sealed class DynamicAutoDiffCostFunction : CostFunction
|
||||
{
|
||||
private readonly GCHandle _wrapperHandle;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new Dynamic AutoDiff cost function.
|
||||
/// </summary>
|
||||
/// <param name="callback">The callback function to evaluate the cost.</param>
|
||||
/// <param name="numResiduals">The number of residuals (determined at runtime).</param>
|
||||
/// <param name="parameterBlockSizes">Array of sizes for each parameter block.</param>
|
||||
public DynamicAutoDiffCostFunction(
|
||||
AutoDiffCostFunctionCallback callback,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes) : base(CreateHandle(callback, numResiduals, parameterBlockSizes, out var wrapperHandle, out var nativeCallbackHandle))
|
||||
{
|
||||
_wrapperHandle = wrapperHandle;
|
||||
SetNativeCallbackHandle(nativeCallbackHandle);
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
try { if (_wrapperHandle.IsAllocated) _wrapperHandle.Free(); } catch { }
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Override to free _wrapperHandle when Problem is disposed.
|
||||
/// </summary>
|
||||
protected override void FreeAdditionalGCHandles()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_wrapperHandle.IsAllocated)
|
||||
_wrapperHandle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
}
|
||||
|
||||
private static CostFunctionHandle CreateHandle(
|
||||
AutoDiffCostFunctionCallback callback,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes,
|
||||
out GCHandle wrapperHandle,
|
||||
out GCHandle nativeCallbackHandle)
|
||||
{
|
||||
CostFunctionHelpers.ValidateCostFunctionParameters(callback, numResiduals, parameterBlockSizes);
|
||||
|
||||
// Create wrapper struct to store callback and parameter info
|
||||
var wrapper = new CostFunctionHelpers.AutoDiffCallbackWrapper
|
||||
{
|
||||
Callback = callback,
|
||||
NumResiduals = numResiduals,
|
||||
ParameterBlockSizes = parameterBlockSizes
|
||||
};
|
||||
|
||||
// Pin the wrapper
|
||||
wrapperHandle = GCHandle.Alloc(wrapper);
|
||||
|
||||
// Create native callback using helper
|
||||
var nativeCallback = CostFunctionHelpers.CreateAutoDiffCallback(wrapper);
|
||||
|
||||
// Pin the native callback
|
||||
nativeCallbackHandle = GCHandle.Alloc(nativeCallback);
|
||||
|
||||
try
|
||||
{
|
||||
var handle = CostFunctionHelpers.CreateAutoDiffHandle(
|
||||
nativeCallback,
|
||||
GCHandle.ToIntPtr(wrapperHandle),
|
||||
numResiduals,
|
||||
parameterBlockSizes,
|
||||
isDynamic: true);
|
||||
|
||||
return handle;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Cleanup on failure
|
||||
wrapperHandle.Free();
|
||||
nativeCallbackHandle.Free();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NumericDiff cost function with fixed parameter block sizes.
|
||||
/// </summary>
|
||||
public sealed class NumericDiffCostFunction : CostFunction
|
||||
{
|
||||
private readonly GCHandle _wrapperHandle;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new NumericDiff cost function.
|
||||
/// </summary>
|
||||
/// <param name="callback">The callback function to evaluate the cost.</param>
|
||||
/// <param name="method">The numeric differentiation method.</param>
|
||||
/// <param name="numResiduals">The number of residuals.</param>
|
||||
/// <param name="parameterBlockSizes">Array of sizes for each parameter block.</param>
|
||||
public NumericDiffCostFunction(
|
||||
NumericDiffCostFunctionCallback callback,
|
||||
NumericDiffMethod method,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes) : base(CreateHandle(callback, method, numResiduals, parameterBlockSizes, out var wrapperHandle, out var nativeCallbackHandle))
|
||||
{
|
||||
_wrapperHandle = wrapperHandle;
|
||||
SetNativeCallbackHandle(nativeCallbackHandle);
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
try { if (_wrapperHandle.IsAllocated) _wrapperHandle.Free(); } catch { }
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Override to free _wrapperHandle when Problem is disposed.
|
||||
/// </summary>
|
||||
protected override void FreeAdditionalGCHandles()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_wrapperHandle.IsAllocated)
|
||||
_wrapperHandle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
}
|
||||
|
||||
private static CostFunctionHandle CreateHandle(
|
||||
NumericDiffCostFunctionCallback callback,
|
||||
NumericDiffMethod method,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes,
|
||||
out GCHandle wrapperHandle,
|
||||
out GCHandle nativeCallbackHandle)
|
||||
{
|
||||
CostFunctionHelpers.ValidateCostFunctionParameters(callback, numResiduals, parameterBlockSizes);
|
||||
|
||||
var wrapper = new CostFunctionHelpers.NumericCallbackWrapper
|
||||
{
|
||||
Callback = callback,
|
||||
NumResiduals = numResiduals,
|
||||
ParameterBlockSizes = parameterBlockSizes
|
||||
};
|
||||
|
||||
wrapperHandle = GCHandle.Alloc(wrapper);
|
||||
|
||||
// Create native callback using helper
|
||||
var nativeCallback = CostFunctionHelpers.CreateNumericDiffCallback(wrapper);
|
||||
|
||||
nativeCallbackHandle = GCHandle.Alloc(nativeCallback);
|
||||
|
||||
try
|
||||
{
|
||||
var handle = CostFunctionHelpers.CreateNumericDiffHandle(
|
||||
nativeCallback,
|
||||
GCHandle.ToIntPtr(wrapperHandle),
|
||||
numResiduals,
|
||||
parameterBlockSizes,
|
||||
method,
|
||||
isDynamic: false);
|
||||
|
||||
return handle;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Cleanup on failure
|
||||
wrapperHandle.Free();
|
||||
nativeCallbackHandle.Free();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dynamic NumericDiff cost function with runtime-determined parameter block sizes.
|
||||
/// </summary>
|
||||
public sealed class DynamicNumericDiffCostFunction : CostFunction
|
||||
{
|
||||
private readonly GCHandle _wrapperHandle;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new Dynamic NumericDiff cost function.
|
||||
/// </summary>
|
||||
/// <param name="callback">The callback function to evaluate the cost.</param>
|
||||
/// <param name="method">The numeric differentiation method.</param>
|
||||
/// <param name="numResiduals">The number of residuals (determined at runtime).</param>
|
||||
/// <param name="parameterBlockSizes">Array of sizes for each parameter block.</param>
|
||||
public DynamicNumericDiffCostFunction(
|
||||
NumericDiffCostFunctionCallback callback,
|
||||
NumericDiffMethod method,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes) : base(CreateHandle(callback, method, numResiduals, parameterBlockSizes, out var wrapperHandle, out var nativeCallbackHandle))
|
||||
{
|
||||
_wrapperHandle = wrapperHandle;
|
||||
SetNativeCallbackHandle(nativeCallbackHandle);
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
try { if (_wrapperHandle.IsAllocated) _wrapperHandle.Free(); } catch { }
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Override to free _wrapperHandle when Problem is disposed.
|
||||
/// </summary>
|
||||
protected override void FreeAdditionalGCHandles()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_wrapperHandle.IsAllocated)
|
||||
_wrapperHandle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - handle may already be freed
|
||||
}
|
||||
}
|
||||
|
||||
private static CostFunctionHandle CreateHandle(
|
||||
NumericDiffCostFunctionCallback callback,
|
||||
NumericDiffMethod method,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes,
|
||||
out GCHandle wrapperHandle,
|
||||
out GCHandle nativeCallbackHandle)
|
||||
{
|
||||
CostFunctionHelpers.ValidateCostFunctionParameters(callback, numResiduals, parameterBlockSizes);
|
||||
|
||||
var wrapper = new CostFunctionHelpers.NumericCallbackWrapper
|
||||
{
|
||||
Callback = callback,
|
||||
NumResiduals = numResiduals,
|
||||
ParameterBlockSizes = parameterBlockSizes
|
||||
};
|
||||
|
||||
wrapperHandle = GCHandle.Alloc(wrapper);
|
||||
|
||||
// Create native callback using helper
|
||||
var nativeCallback = CostFunctionHelpers.CreateNumericDiffCallback(wrapper);
|
||||
|
||||
nativeCallbackHandle = GCHandle.Alloc(nativeCallback);
|
||||
|
||||
try
|
||||
{
|
||||
var handle = CostFunctionHelpers.CreateNumericDiffHandle(
|
||||
nativeCallback,
|
||||
GCHandle.ToIntPtr(wrapperHandle),
|
||||
numResiduals,
|
||||
parameterBlockSizes,
|
||||
method,
|
||||
isDynamic: true);
|
||||
|
||||
return handle;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Cleanup on failure
|
||||
wrapperHandle.Free();
|
||||
nativeCallbackHandle.Free();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Delegate for AutoDiff cost function evaluation.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Array of parameter blocks (each block is contiguous).</param>
|
||||
/// <param name="residuals">Output array for residuals.</param>
|
||||
/// <returns>True on success, false on failure.</returns>
|
||||
public delegate bool AutoDiffCostFunctionCallback(double[][] parameters, double[] residuals);
|
||||
|
||||
/// <summary>
|
||||
/// Delegate for NumericDiff cost function evaluation.
|
||||
/// </summary>
|
||||
/// <param name="parameters">Array of parameter blocks (each block is contiguous).</param>
|
||||
/// <param name="residuals">Output array for residuals.</param>
|
||||
/// <returns>True on success, false on failure.</returns>
|
||||
public delegate bool NumericDiffCostFunctionCallback(double[][] parameters, double[] residuals);
|
||||
@@ -0,0 +1,246 @@
|
||||
using System;
|
||||
using System.Linq;
|
||||
using System.Runtime.InteropServices;
|
||||
using CeresSharp.Enums;
|
||||
using CeresSharp.Exceptions;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for creating cost functions to reduce code duplication.
|
||||
/// </summary>
|
||||
internal static class CostFunctionHelpers
|
||||
{
|
||||
/// <summary>
|
||||
/// Validates cost function parameters.
|
||||
/// </summary>
|
||||
internal static void ValidateCostFunctionParameters(
|
||||
object? callback,
|
||||
int numResiduals,
|
||||
int[]? parameterBlockSizes,
|
||||
string callbackParamName = "callback",
|
||||
string numResidualsParamName = "numResiduals",
|
||||
string parameterBlockSizesParamName = "parameterBlockSizes")
|
||||
{
|
||||
if (callback == null)
|
||||
throw new ArgumentNullException(callbackParamName);
|
||||
if (numResiduals <= 0)
|
||||
throw new ArgumentException("Number of residuals must be positive", numResidualsParamName);
|
||||
if (parameterBlockSizes == null || parameterBlockSizes.Length == 0)
|
||||
throw new ArgumentException("Parameter block sizes cannot be null or empty", parameterBlockSizesParamName);
|
||||
if (parameterBlockSizes.Any(size => size <= 0))
|
||||
throw new ArgumentException("All parameter block sizes must be positive", parameterBlockSizesParamName);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a native AutoDiff cost function callback wrapper.
|
||||
/// </summary>
|
||||
internal static CeresNative.CeresAutoDiffCostFunctionCallback CreateAutoDiffCallback(
|
||||
AutoDiffCallbackWrapper wrapper)
|
||||
{
|
||||
return (userData, parametersPtr, residualsPtr) =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var handle = GCHandle.FromIntPtr(userData);
|
||||
var wrapperObj = (AutoDiffCallbackWrapper)handle.Target!;
|
||||
|
||||
// Copy parameter blocks from native memory
|
||||
var parameterBlocks = Native.UnsafeHelpers.CopyParameterBlocks(parametersPtr, wrapperObj.ParameterBlockSizes);
|
||||
|
||||
// Allocate residuals array
|
||||
var residuals = new double[wrapperObj.NumResiduals];
|
||||
if (residuals.Length <= 0)
|
||||
return 0;
|
||||
|
||||
// Call C# callback
|
||||
var success = wrapperObj.Callback(parameterBlocks, residuals);
|
||||
|
||||
// CRITICAL: Check for NaN/Inf in residuals before copying to native memory
|
||||
// This prevents Ceres from getting invalid residuals that cause InitialCost=-1
|
||||
if (success && residuals.Length == wrapperObj.NumResiduals)
|
||||
{
|
||||
bool hasInvalid = false;
|
||||
int nanCount = 0;
|
||||
int infCount = 0;
|
||||
double sumSquared = 0.0;
|
||||
|
||||
for (int i = 0; i < residuals.Length; ++i)
|
||||
{
|
||||
if (double.IsNaN(residuals[i]))
|
||||
{
|
||||
hasInvalid = true;
|
||||
nanCount++;
|
||||
if (nanCount <= 5)
|
||||
{
|
||||
}
|
||||
}
|
||||
else if (double.IsInfinity(residuals[i]))
|
||||
{
|
||||
hasInvalid = true;
|
||||
infCount++;
|
||||
if (infCount <= 5)
|
||||
{
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sumSquared += residuals[i] * residuals[i];
|
||||
}
|
||||
}
|
||||
|
||||
if (hasInvalid)
|
||||
{
|
||||
return 0; // Return failure
|
||||
}
|
||||
|
||||
// Copy residuals back
|
||||
Marshal.Copy(residuals, 0, residualsPtr, wrapperObj.NumResiduals);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Return failure on any exception
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a native NumericDiff cost function callback wrapper.
|
||||
/// </summary>
|
||||
internal static CeresNative.CeresNumericDiffCostFunctionCallback CreateNumericDiffCallback(
|
||||
NumericCallbackWrapper wrapper)
|
||||
{
|
||||
return (parametersPtr, residualsPtr, userData) =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var handle = GCHandle.FromIntPtr(userData);
|
||||
var wrapperObj = (NumericCallbackWrapper)handle.Target!;
|
||||
|
||||
// Copy parameter blocks from native memory
|
||||
var parameterBlocks = Native.UnsafeHelpers.CopyParameterBlocks(parametersPtr, wrapperObj.ParameterBlockSizes);
|
||||
|
||||
// Allocate residuals array
|
||||
var residuals = new double[wrapperObj.NumResiduals];
|
||||
if (residuals.Length <= 0)
|
||||
return 0;
|
||||
|
||||
// Call C# callback
|
||||
var success = wrapperObj.Callback(parameterBlocks, residuals);
|
||||
|
||||
// Copy residuals back
|
||||
if (success && residuals.Length == wrapperObj.NumResiduals)
|
||||
{
|
||||
Marshal.Copy(residuals, 0, residualsPtr, wrapperObj.NumResiduals);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates an AutoDiff cost function handle (fixed or dynamic).
|
||||
/// </summary>
|
||||
internal static CostFunctionHandle CreateAutoDiffHandle(
|
||||
CeresNative.CeresAutoDiffCostFunctionCallback nativeCallback,
|
||||
IntPtr userData,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes,
|
||||
bool isDynamic)
|
||||
{
|
||||
return Native.UnsafeHelpers.WithPinnedArray(parameterBlockSizes, (ptr) =>
|
||||
{
|
||||
var handle = isDynamic
|
||||
? CeresNative.ceres_wrapper_create_dynamic_autodiff_cost_function(
|
||||
nativeCallback,
|
||||
userData,
|
||||
numResiduals,
|
||||
parameterBlockSizes.Length,
|
||||
ptr)
|
||||
: CeresNative.ceres_wrapper_create_autodiff_cost_function(
|
||||
nativeCallback,
|
||||
userData,
|
||||
numResiduals,
|
||||
parameterBlockSizes.Length,
|
||||
ptr);
|
||||
|
||||
if (handle == IntPtr.Zero)
|
||||
{
|
||||
throw new CeresException(CeresErrorCode.OutOfMemory,
|
||||
$"Failed to create {(isDynamic ? "Dynamic " : "")}AutoDiff cost function");
|
||||
}
|
||||
|
||||
return CostFunctionHandle.Create(handle);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a NumericDiff cost function handle (fixed or dynamic).
|
||||
/// </summary>
|
||||
internal static CostFunctionHandle CreateNumericDiffHandle(
|
||||
CeresNative.CeresNumericDiffCostFunctionCallback nativeCallback,
|
||||
IntPtr userData,
|
||||
int numResiduals,
|
||||
int[] parameterBlockSizes,
|
||||
NumericDiffMethod method,
|
||||
bool isDynamic)
|
||||
{
|
||||
return Native.UnsafeHelpers.WithPinnedArray(parameterBlockSizes, (ptr) =>
|
||||
{
|
||||
var options = new CeresNative.CeresNumericDiffOptions
|
||||
{
|
||||
numResiduals = numResiduals,
|
||||
numParameterBlocks = parameterBlockSizes.Length,
|
||||
parameterBlockSizes = ptr,
|
||||
callback = nativeCallback,
|
||||
userData = userData
|
||||
};
|
||||
|
||||
var handle = isDynamic
|
||||
? CeresNative.ceres_wrapper_create_dynamic_numeric_diff_cost_function(ref options, (int)method)
|
||||
: CeresNative.ceres_wrapper_create_numeric_diff_cost_function(ref options, (int)method);
|
||||
|
||||
if (handle == IntPtr.Zero)
|
||||
{
|
||||
throw new CeresException(CeresErrorCode.OutOfMemory,
|
||||
$"Failed to create {(isDynamic ? "Dynamic " : "")}NumericDiff cost function");
|
||||
}
|
||||
|
||||
return CostFunctionHandle.Create(handle);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Wrapper struct for AutoDiff cost function callbacks.
|
||||
/// </summary>
|
||||
internal class AutoDiffCallbackWrapper
|
||||
{
|
||||
public AutoDiffCostFunctionCallback Callback = null!;
|
||||
public int NumResiduals;
|
||||
public int[] ParameterBlockSizes = null!;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Wrapper struct for NumericDiff cost function callbacks.
|
||||
/// </summary>
|
||||
internal class NumericCallbackWrapper
|
||||
{
|
||||
public NumericDiffCostFunctionCallback Callback = null!;
|
||||
public int NumResiduals;
|
||||
public int[] ParameterBlockSizes = null!;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
using System;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Cubic interpolator for 1D data.
|
||||
/// </summary>
|
||||
public sealed class CubicInterpolator : IDisposable
|
||||
{
|
||||
private readonly InterpolatorHandle _handle;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new cubic interpolator from 1D data array.
|
||||
/// </summary>
|
||||
/// <param name="data">The data array.</param>
|
||||
public CubicInterpolator(double[] data)
|
||||
{
|
||||
if (data == null)
|
||||
throw new ArgumentNullException(nameof(data));
|
||||
if (data.Length == 0)
|
||||
throw new ArgumentException("Data array cannot be empty", nameof(data));
|
||||
|
||||
unsafe
|
||||
{
|
||||
fixed (double* ptr = data)
|
||||
{
|
||||
var handle = CeresNative.ceres_wrapper_create_cubic_interpolator(
|
||||
(IntPtr)ptr,
|
||||
data.Length);
|
||||
_handle = InterpolatorHandle.CreateCubic(handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the interpolator at point x.
|
||||
/// </summary>
|
||||
/// <param name="x">Coordinate in data space (0 <= x < numValues).</param>
|
||||
/// <param name="value">Output value at x.</param>
|
||||
/// <param name="gradient">Output gradient at x (can be null).</param>
|
||||
public void Evaluate(double x, out double value, out double? gradient)
|
||||
{
|
||||
unsafe
|
||||
{
|
||||
double val = 0, grad = 0;
|
||||
CeresNative.ceres_wrapper_cubic_interpolator_evaluate(
|
||||
_handle.DangerousGetHandle(),
|
||||
x,
|
||||
(IntPtr)(&val),
|
||||
(IntPtr)(&grad));
|
||||
value = val;
|
||||
gradient = grad;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Evaluates the interpolator at point x without gradient.
|
||||
/// </summary>
|
||||
public double Evaluate(double x)
|
||||
{
|
||||
Evaluate(x, out var value, out _);
|
||||
return value;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_handle?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,180 @@
|
||||
using System;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Base class for loss functions in Ceres Solver.
|
||||
/// </summary>
|
||||
public abstract class LossFunction : IDisposable
|
||||
{
|
||||
private readonly LossFunctionHandle _handle;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Initializes a new instance of the LossFunction class.
|
||||
/// </summary>
|
||||
internal LossFunction(LossFunctionHandle handle)
|
||||
{
|
||||
_handle = handle ?? throw new ArgumentNullException(nameof(handle));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the native handle.
|
||||
/// </summary>
|
||||
internal IntPtr Handle => _handle.DangerousGetHandle();
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether this loss function is owned by a Problem.
|
||||
/// </summary>
|
||||
internal bool IsOwnedByProblem => _handle is { IsInvalid: false } && _ownedByProblem;
|
||||
private volatile bool _ownedByProblem;
|
||||
|
||||
/// <summary>
|
||||
/// Marks this loss function as owned by a Problem.
|
||||
/// After this, the native handle will NOT be freed on Dispose - Ceres handles cleanup.
|
||||
/// </summary>
|
||||
internal void MarkOwnedByProblem()
|
||||
{
|
||||
_ownedByProblem = true;
|
||||
_handle.MarkOwnedByProblem();
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_handle?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Trivial loss function (no loss applied).
|
||||
/// </summary>
|
||||
public sealed class TrivialLoss : LossFunction
|
||||
{
|
||||
public TrivialLoss() : base(LossFunctionHandle.Create(CeresNative.ceres_wrapper_create_trivial_loss()))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Huber loss function.
|
||||
/// </summary>
|
||||
public sealed class HuberLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Huber loss function.
|
||||
/// </summary>
|
||||
/// <param name="a">The scaling parameter.</param>
|
||||
public HuberLoss(double a) : base(LossFunctionHandle.Create(CeresNative.ceres_wrapper_create_huber_loss(a)))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Cauchy loss function.
|
||||
/// </summary>
|
||||
public sealed class CauchyLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Cauchy loss function.
|
||||
/// </summary>
|
||||
/// <param name="a">The scaling parameter.</param>
|
||||
public CauchyLoss(double a) : base(LossFunctionHandle.Create(CeresNative.ceres_wrapper_create_cauchy_loss(a)))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// SoftLOne loss function.
|
||||
/// </summary>
|
||||
public sealed class SoftLOneLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new SoftLOne loss function.
|
||||
/// </summary>
|
||||
/// <param name="a">The scaling parameter.</param>
|
||||
public SoftLOneLoss(double a) : base(LossFunctionHandle.Create(CeresNative.ceres_wrapper_create_softl1_loss(a)))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Arctan loss function.
|
||||
/// </summary>
|
||||
public sealed class ArctanLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Arctan loss function.
|
||||
/// </summary>
|
||||
/// <param name="a">The scaling parameter.</param>
|
||||
public ArctanLoss(double a) : base(LossFunctionHandle.Create(CeresNative.ceres_wrapper_create_arctan_loss(a)))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tolerant loss function.
|
||||
/// </summary>
|
||||
public sealed class TolerantLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Tolerant loss function.
|
||||
/// </summary>
|
||||
/// <param name="a">The first scaling parameter.</param>
|
||||
/// <param name="b">The second scaling parameter.</param>
|
||||
public TolerantLoss(double a, double b) : base(LossFunctionHandle.Create(CeresNative.ceres_wrapper_create_tolerant_loss(a, b)))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Composed loss function: f(g(s)).
|
||||
/// The composed loss takes ownership of both f and g.
|
||||
/// WARNING: After creating a ComposedLoss, do NOT dispose the original f and g
|
||||
/// LossFunction objects - ownership is transferred to the composed loss.
|
||||
/// </summary>
|
||||
public sealed class ComposedLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Composed loss function f(g(s)).
|
||||
/// </summary>
|
||||
/// <param name="f">The outer loss function.</param>
|
||||
/// <param name="g">The inner loss function.</param>
|
||||
public ComposedLoss(LossFunction f, LossFunction g)
|
||||
: base(LossFunctionHandle.Create(
|
||||
CeresNative.ceres_wrapper_create_composed_loss(
|
||||
f?.Handle ?? throw new ArgumentNullException(nameof(f)),
|
||||
1, // TAKE_OWNERSHIP
|
||||
g?.Handle ?? throw new ArgumentNullException(nameof(g)),
|
||||
1))) // TAKE_OWNERSHIP
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Scaled loss function: a * rho(s).
|
||||
/// The scaled loss takes ownership of the underlying loss function.
|
||||
/// WARNING: After creating a ScaledLoss, do NOT dispose the original rho
|
||||
/// LossFunction object - ownership is transferred to the scaled loss.
|
||||
/// </summary>
|
||||
public sealed class ScaledLoss : LossFunction
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Scaled loss function a * rho(s).
|
||||
/// </summary>
|
||||
/// <param name="rho">The underlying loss function (null for identity).</param>
|
||||
/// <param name="a">The scaling factor.</param>
|
||||
public ScaledLoss(LossFunction? rho, double a)
|
||||
: base(LossFunctionHandle.Create(
|
||||
CeresNative.ceres_wrapper_create_scaled_loss(
|
||||
rho?.Handle ?? IntPtr.Zero,
|
||||
a,
|
||||
1))) // TAKE_OWNERSHIP
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,313 @@
|
||||
using System;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Base class for manifolds in Ceres Solver.
|
||||
/// </summary>
|
||||
public abstract class Manifold : IDisposable
|
||||
{
|
||||
private readonly ManifoldHandle _handle;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Initializes a new instance of the Manifold class.
|
||||
/// </summary>
|
||||
internal Manifold(ManifoldHandle handle)
|
||||
{
|
||||
_handle = handle ?? throw new ArgumentNullException(nameof(handle));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the native handle.
|
||||
/// </summary>
|
||||
internal IntPtr Handle => _handle.DangerousGetHandle();
|
||||
|
||||
/// <summary>
|
||||
/// Gets the ambient size (the size of the parameter space).
|
||||
/// </summary>
|
||||
public int AmbientSize => CeresNative.ceres_wrapper_manifold_ambient_size(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the tangent size (the size of the tangent space).
|
||||
/// </summary>
|
||||
public int TangentSize => CeresNative.ceres_wrapper_manifold_tangent_size(_handle.DangerousGetHandle());
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(disposing: true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
_handle?.Dispose();
|
||||
}
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Quaternion manifold for 3D rotations.
|
||||
/// </summary>
|
||||
public sealed class QuaternionManifold : Manifold
|
||||
{
|
||||
public QuaternionManifold() : base(ManifoldHandle.Create(CeresNative.ceres_wrapper_create_quaternion_manifold()))
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sphere manifold.
|
||||
/// </summary>
|
||||
public sealed class SphereManifold : Manifold
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new sphere manifold.
|
||||
/// </summary>
|
||||
/// <param name="dimension">The dimension of the sphere.</param>
|
||||
public SphereManifold(int dimension) : base(ManifoldHandle.Create(CeresNative.ceres_wrapper_create_sphere_manifold(dimension)))
|
||||
{
|
||||
if (dimension <= 0)
|
||||
throw new ArgumentException("Dimension must be positive", nameof(dimension));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Line manifold.
|
||||
/// </summary>
|
||||
public sealed class LineManifold : Manifold
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new line manifold.
|
||||
/// </summary>
|
||||
/// <param name="dimension">The dimension of the line.</param>
|
||||
public LineManifold(int dimension) : base(ManifoldHandle.Create(CeresNative.ceres_wrapper_create_line_manifold(dimension)))
|
||||
{
|
||||
if (dimension <= 0)
|
||||
throw new ArgumentException("Dimension must be positive", nameof(dimension));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Euclidean manifold (standard Euclidean space, no constraints).
|
||||
/// </summary>
|
||||
public sealed class EuclideanManifold : Manifold
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new Euclidean manifold.
|
||||
/// </summary>
|
||||
/// <param name="dimension">The dimension of the Euclidean space.</param>
|
||||
public EuclideanManifold(int dimension) : base(ManifoldHandle.Create(CeresNative.ceres_wrapper_create_euclidean_manifold(dimension)))
|
||||
{
|
||||
if (dimension <= 0)
|
||||
throw new ArgumentException("Dimension must be positive", nameof(dimension));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Subset manifold (fix subset of parameters).
|
||||
/// </summary>
|
||||
public sealed class SubsetManifold : Manifold
|
||||
{
|
||||
/// <summary>
|
||||
/// Creates a new subset manifold.
|
||||
/// </summary>
|
||||
/// <param name="constantSubset">Array of indices to keep constant.</param>
|
||||
/// <param name="ambientSize">Total size of parameter block.</param>
|
||||
public SubsetManifold(int[] constantSubset, int ambientSize) : base(CreateHandle(constantSubset, ambientSize))
|
||||
{
|
||||
}
|
||||
|
||||
private static ManifoldHandle CreateHandle(int[] constantSubset, int ambientSize)
|
||||
{
|
||||
if (constantSubset == null)
|
||||
throw new ArgumentNullException(nameof(constantSubset));
|
||||
if (ambientSize <= 0)
|
||||
throw new ArgumentException("Ambient size must be positive", nameof(ambientSize));
|
||||
|
||||
unsafe
|
||||
{
|
||||
fixed (int* ptr = constantSubset)
|
||||
{
|
||||
var handle = CeresNative.ceres_wrapper_create_subset_manifold(
|
||||
(IntPtr)ptr,
|
||||
constantSubset.Length,
|
||||
ambientSize);
|
||||
return ManifoldHandle.Create(handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Product manifold (combines multiple manifolds).
|
||||
/// </summary>
|
||||
public sealed class ProductManifold : Manifold
|
||||
{
|
||||
// CRITICAL: Keep references to child manifolds to prevent GC collection
|
||||
// Native code may still use handles from these manifolds, so they must remain alive
|
||||
private readonly Manifold[] _manifolds;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new product manifold from multiple manifolds.
|
||||
/// </summary>
|
||||
/// <param name="manifolds">Array of manifolds to combine.</param>
|
||||
public ProductManifold(Manifold[] manifolds) : base(CreateHandle(manifolds))
|
||||
{
|
||||
// CRITICAL: Keep reference to prevent GC collection of child manifolds
|
||||
// Native code may still use handles from these manifolds during optimization
|
||||
_manifolds = manifolds ?? throw new ArgumentNullException(nameof(manifolds));
|
||||
}
|
||||
|
||||
private static ManifoldHandle CreateHandle(Manifold[] manifolds)
|
||||
{
|
||||
if (manifolds == null || manifolds.Length == 0)
|
||||
throw new ArgumentException("Manifolds cannot be null or empty", nameof(manifolds));
|
||||
|
||||
unsafe
|
||||
{
|
||||
var handles = new IntPtr[manifolds.Length];
|
||||
for (int i = 0; i < manifolds.Length; i++)
|
||||
{
|
||||
if (manifolds[i] == null)
|
||||
throw new ArgumentException($"Manifold {i} is null", nameof(manifolds));
|
||||
handles[i] = manifolds[i].Handle;
|
||||
}
|
||||
|
||||
fixed (IntPtr* ptr = handles)
|
||||
{
|
||||
var handle = CeresNative.ceres_wrapper_create_product_manifold(
|
||||
(IntPtr)ptr,
|
||||
manifolds.Length);
|
||||
return ManifoldHandle.Create(handle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
// Dispose child manifolds to cleanup their resources (including GCHandles in AutoDiffManifold)
|
||||
// This is critical because AutoDiffManifold (used by AngleManifold) contains GCHandles
|
||||
// that must be freed when the manifold is no longer needed
|
||||
if (_manifolds != null)
|
||||
{
|
||||
foreach (var manifold in _manifolds)
|
||||
{
|
||||
manifold?.Dispose();
|
||||
}
|
||||
}
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Angle manifold for 2D rotation angles.
|
||||
/// Constrains angles to [-π, π] range and handles wrap-around correctly.
|
||||
/// </summary>
|
||||
public sealed class AngleManifold : Manifold
|
||||
{
|
||||
private readonly AutoDiffManifold _autoDiffManifold;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new angle manifold for 2D rotation.
|
||||
/// </summary>
|
||||
public AngleManifold() : base(CreateHandle(out var autoDiffManifold))
|
||||
{
|
||||
_autoDiffManifold = autoDiffManifold;
|
||||
}
|
||||
|
||||
private static ManifoldHandle CreateHandle(out AutoDiffManifold autoDiffManifold)
|
||||
{
|
||||
autoDiffManifold = new AutoDiffManifold(
|
||||
ambientSize: 1,
|
||||
tangentSize: 1,
|
||||
plus: (x, delta, xPlusDelta) =>
|
||||
{
|
||||
// Normalize angle: x[0] + delta[0] -> [-π, π]
|
||||
double angle = x[0] + delta[0];
|
||||
xPlusDelta[0] = NormalizeAngle(angle);
|
||||
return true;
|
||||
},
|
||||
minus: (y, x, yMinusX) =>
|
||||
{
|
||||
// Normalize both angles and compute difference
|
||||
double normalizedY = NormalizeAngle(y[0]);
|
||||
double normalizedX = NormalizeAngle(x[0]);
|
||||
yMinusX[0] = NormalizeAngle(normalizedY - normalizedX);
|
||||
return true;
|
||||
});
|
||||
|
||||
// Get handle from AutoDiffManifold using reflection
|
||||
var handleField = typeof(Manifold).GetField("_handle", System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance);
|
||||
return (ManifoldHandle)handleField!.GetValue(autoDiffManifold)!;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Normalizes angle to [-π, π] range.
|
||||
/// </summary>
|
||||
private static double NormalizeAngle(double angle)
|
||||
{
|
||||
while (angle > Math.PI)
|
||||
angle -= 2.0 * Math.PI;
|
||||
while (angle < -Math.PI)
|
||||
angle += 2.0 * Math.PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
_autoDiffManifold?.Dispose();
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Pose2D manifold for 2D poses [x, y, theta].
|
||||
/// Combines EuclideanManifold for translation [x, y] and AngleManifold for rotation [theta].
|
||||
/// </summary>
|
||||
public sealed class Pose2DManifold : Manifold
|
||||
{
|
||||
private readonly ProductManifold _productManifold;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new Pose2D manifold.
|
||||
/// </summary>
|
||||
public Pose2DManifold() : base(CreateHandle(out var productManifold))
|
||||
{
|
||||
_productManifold = productManifold;
|
||||
}
|
||||
|
||||
private static ManifoldHandle CreateHandle(out ProductManifold productManifold)
|
||||
{
|
||||
var translationManifold = new EuclideanManifold(2); // [x, y]
|
||||
var rotationManifold = new AngleManifold(); // [theta]
|
||||
productManifold = new ProductManifold([translationManifold, rotationManifold]);
|
||||
|
||||
// Get handle from ProductManifold using reflection
|
||||
var handleField = typeof(Manifold).GetField("_handle", System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Instance);
|
||||
return (ManifoldHandle)handleField!.GetValue(productManifold)!;
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
_productManifold?.Dispose();
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,852 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using CeresSharp.Advanced;
|
||||
using CeresSharp.Exceptions;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
using static CeresSharp.Native.NativeHelpers;
|
||||
using static CeresSharp.Native.UnsafeHelpers;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Represents a Ceres optimization problem.
|
||||
/// </summary>
|
||||
public sealed class Problem : IDisposable
|
||||
{
|
||||
private readonly ProblemHandle _handle;
|
||||
// CRITICAL: Use volatile for thread-safe disposal check
|
||||
// Dispose() should be idempotent, but volatile ensures proper memory visibility
|
||||
// across threads when checking _disposed flag
|
||||
private volatile bool _disposed;
|
||||
// Keep references to parameter blocks to prevent GC and keep them pinned
|
||||
// Dictionary maps pinned pointer address -> GCHandle for efficient lookup on RemoveParameterBlock
|
||||
private readonly Dictionary<IntPtr, GCHandle> _pinnedParameterBlocks = new Dictionary<IntPtr, GCHandle>();
|
||||
// CRITICAL: Keep references to cost functions and loss functions to prevent GC collection
|
||||
// Native code may still call callbacks even after solve completes, so we must keep
|
||||
// these objects alive for the lifetime of the Problem
|
||||
private readonly List<CostFunction> _costFunctions = new List<CostFunction>();
|
||||
private readonly List<LossFunction> _lossFunctions = new List<LossFunction>();
|
||||
// CRITICAL: Keep references to manifolds to prevent GC collection
|
||||
// Native code may still call manifold callbacks (Plus/Minus) even after solve completes,
|
||||
// especially for AutoDiffManifold with GCHandle callbacks, so we must keep these objects alive
|
||||
private readonly List<Manifold> _manifolds = new List<Manifold>();
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new Problem instance.
|
||||
/// </summary>
|
||||
public Problem()
|
||||
{
|
||||
_handle = ProblemHandle.Create();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new Problem instance with the specified options.
|
||||
/// </summary>
|
||||
public Problem(ProblemOptions options)
|
||||
{
|
||||
if (options == null)
|
||||
throw new ArgumentNullException(nameof(options));
|
||||
|
||||
_handle = ProblemHandle.CreateWithOptions(options.Handle.DangerousGetHandle());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the native handle.
|
||||
/// </summary>
|
||||
internal ProblemHandle Handle => _handle;
|
||||
|
||||
// ============================================================================
|
||||
// Parameter Block Management
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Adds a parameter block to the problem.
|
||||
/// </summary>
|
||||
/// <param name="parameters">The parameter array.</param>
|
||||
/// <param name="size">The size of the parameter block.</param>
|
||||
/// <exception cref="ArgumentNullException">Thrown when parameters is null.</exception>
|
||||
/// <exception cref="CeresException">Thrown when the operation fails.</exception>
|
||||
public void AddParameterBlock(double[] parameters, int size)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
if (size <= 0)
|
||||
throw new ArgumentException("Size must be positive", nameof(size));
|
||||
if (parameters.Length < size)
|
||||
throw new ArgumentException("Parameters array length must be at least size", nameof(parameters));
|
||||
|
||||
// CRITICAL: Pin array permanently for the lifetime of the problem
|
||||
// Ceres stores pointers to the arrays, so they must remain valid and pinned
|
||||
// This is the same approach used in AddResidualBlock
|
||||
var handle = PinArray(parameters);
|
||||
|
||||
try
|
||||
{
|
||||
var ptr = handle.AddrOfPinnedObject();
|
||||
var errorMessageBuffer = CreateErrorMessageBuffer();
|
||||
var errorCode = CeresNative.ceres_wrapper_problem_add_parameter_block(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
size,
|
||||
errorMessageBuffer,
|
||||
errorMessageBuffer.Capacity);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
// On error, free the handle before throwing
|
||||
if (handle.IsAllocated)
|
||||
handle.Free();
|
||||
var errorMessage = ExtractErrorMessage(errorMessageBuffer);
|
||||
throw new CeresException(errorCode, errorMessage);
|
||||
}
|
||||
|
||||
// Keep array pinned for the lifetime of the problem
|
||||
// Add to the dictionary keyed by pointer address (will be unpinned in Dispose)
|
||||
lock (_pinnedParameterBlocks)
|
||||
{
|
||||
_pinnedParameterBlocks[ptr] = handle;
|
||||
}
|
||||
}
|
||||
catch
|
||||
{
|
||||
// On exception, free the handle before re-throwing
|
||||
if (handle.IsAllocated)
|
||||
handle.Free();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets a parameter block to be constant (not optimized).
|
||||
/// </summary>
|
||||
public void SetParameterBlockConstant(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
CeresNative.ceres_wrapper_problem_set_parameter_block_constant(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets a parameter block to be variable (optimized).
|
||||
/// </summary>
|
||||
public void SetParameterBlockVariable(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
CeresNative.ceres_wrapper_problem_set_parameter_block_variable(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Removes a parameter block from the problem.
|
||||
/// </summary>
|
||||
/// <returns>True if the parameter block was removed, false if it was not found.</returns>
|
||||
/// <exception cref="ArgumentNullException">Thrown when parameters is null.</exception>
|
||||
/// <exception cref="CeresException">Thrown when the operation fails.</exception>
|
||||
public bool RemoveParameterBlock(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
var errorMessageBuffer = CreateErrorMessageBuffer();
|
||||
var errorCode = CeresNative.ceres_wrapper_problem_remove_parameter_block(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
errorMessageBuffer,
|
||||
errorMessageBuffer.Capacity);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
var errorMessage = ExtractErrorMessage(errorMessageBuffer);
|
||||
throw new CeresException(errorCode, errorMessage);
|
||||
}
|
||||
|
||||
// Free the GCHandle for the removed parameter block to unpin its memory
|
||||
lock (_pinnedParameterBlocks)
|
||||
{
|
||||
if (_pinnedParameterBlocks.TryGetValue(ptr, out var gcHandle))
|
||||
{
|
||||
if (gcHandle.IsAllocated)
|
||||
gcHandle.Free();
|
||||
_pinnedParameterBlocks.Remove(ptr);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
});
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Residual Block Management
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Adds a residual block to the problem.
|
||||
/// </summary>
|
||||
/// <param name="costFunction">The cost function.</param>
|
||||
/// <param name="lossFunction">The loss function (can be null for TrivialLoss).</param>
|
||||
/// <param name="parameterBlocks">Array of parameter block arrays.</param>
|
||||
/// <returns>The residual block ID.</returns>
|
||||
public IntPtr AddResidualBlock(
|
||||
CostFunction costFunction,
|
||||
LossFunction? lossFunction,
|
||||
double[][] parameterBlocks)
|
||||
{
|
||||
if (costFunction == null)
|
||||
throw new ArgumentNullException(nameof(costFunction));
|
||||
if (parameterBlocks == null || parameterBlocks.Length == 0)
|
||||
throw new ArgumentException("Parameter blocks cannot be null or empty", nameof(parameterBlocks));
|
||||
|
||||
// Pin parameter blocks - Ceres stores pointers to the arrays, so they must
|
||||
// remain valid and pinned for the lifetime of the problem
|
||||
// NOTE: These temporary collections are created per call but are necessary for proper
|
||||
// memory management. They will be cleaned up by GC after the method returns.
|
||||
var pinnedArrays = new List<GCHandle>(parameterBlocks.Length);
|
||||
var parameterBlockPtrs = new IntPtr[parameterBlocks.Length];
|
||||
|
||||
try
|
||||
{
|
||||
// Pin all parameter blocks
|
||||
for (int i = 0; i < parameterBlocks.Length; i++)
|
||||
{
|
||||
if (parameterBlocks[i] == null)
|
||||
throw new ArgumentException($"Parameter block {i} is null", nameof(parameterBlocks));
|
||||
|
||||
var handle = PinArray(parameterBlocks[i]);
|
||||
pinnedArrays.Add(handle);
|
||||
parameterBlockPtrs[i] = handle.AddrOfPinnedObject();
|
||||
}
|
||||
|
||||
// Execute native call with pinned pointer array
|
||||
// NOTE: WithPinnedArray uses 'fixed' statement which automatically unpins when done
|
||||
IntPtr residualBlockId = IntPtr.Zero;
|
||||
WithPinnedArray(parameterBlockPtrs, (ptrs) =>
|
||||
{
|
||||
// NOTE: StringBuilder is created per call but is lightweight and will be GC'd
|
||||
// This is acceptable overhead for error message handling
|
||||
var errorMessageBuffer = CreateErrorMessageBuffer();
|
||||
var errorCode = CeresNative.ceres_wrapper_problem_add_residual_block(
|
||||
_handle.DangerousGetHandle(),
|
||||
costFunction.Handle,
|
||||
lossFunction?.Handle ?? IntPtr.Zero,
|
||||
ptrs,
|
||||
parameterBlocks.Length,
|
||||
out residualBlockId,
|
||||
errorMessageBuffer,
|
||||
errorMessageBuffer.Capacity);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
var errorMsg = ExtractErrorMessage(errorMessageBuffer);
|
||||
throw new CeresException(errorCode, errorMsg);
|
||||
}
|
||||
});
|
||||
|
||||
// Keep arrays pinned for the lifetime of the problem
|
||||
// Add to the dictionary keyed by pointer address (will be unpinned in Dispose)
|
||||
// CRITICAL: Must add to dictionary atomically to avoid partial adds on exception
|
||||
lock (_pinnedParameterBlocks)
|
||||
{
|
||||
for (int i = 0; i < pinnedArrays.Count; i++)
|
||||
{
|
||||
_pinnedParameterBlocks[parameterBlockPtrs[i]] = pinnedArrays[i];
|
||||
}
|
||||
// Clear local list after successful add to prevent double-free in catch block
|
||||
// Handles are now owned by _pinnedParameterBlocks and will be freed in Dispose
|
||||
pinnedArrays.Clear();
|
||||
}
|
||||
|
||||
// Mark cost function and loss function as owned by this Problem so their native handles
|
||||
// won't be freed independently (Ceres will free them when Problem is destroyed)
|
||||
costFunction.MarkOwnedByProblem();
|
||||
lossFunction?.MarkOwnedByProblem();
|
||||
|
||||
// CRITICAL: Keep references to cost function and loss function to prevent GC collection
|
||||
// Native code may still call callbacks even after solve completes
|
||||
lock (_costFunctions)
|
||||
{
|
||||
_costFunctions.Add(costFunction);
|
||||
}
|
||||
if (lossFunction != null)
|
||||
{
|
||||
lock (_lossFunctions)
|
||||
{
|
||||
_lossFunctions.Add(lossFunction);
|
||||
}
|
||||
}
|
||||
|
||||
return residualBlockId;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// On error, unpin arrays that were pinned but not yet added to _pinnedParameterBlocks
|
||||
// This prevents memory leaks if exception occurs before AddRange completes
|
||||
// NOTE: After successful AddRange, pinnedArrays is cleared, so this only frees
|
||||
// handles that were pinned but not yet added to the list
|
||||
foreach (var handle in pinnedArrays)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (handle.IsAllocated)
|
||||
handle.Free();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors during cleanup - handle may already be freed or invalid
|
||||
}
|
||||
}
|
||||
// Clear the list to help GC
|
||||
pinnedArrays.Clear();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Removes a residual block from the problem.
|
||||
/// </summary>
|
||||
/// <returns>True if the residual block was removed, false if it was not found.</returns>
|
||||
/// <exception cref="ArgumentException">Thrown when residualBlockId is zero.</exception>
|
||||
/// <exception cref="CeresException">Thrown when the operation fails.</exception>
|
||||
public bool RemoveResidualBlock(IntPtr residualBlockId)
|
||||
{
|
||||
if (residualBlockId == IntPtr.Zero)
|
||||
throw new ArgumentException("Residual block ID cannot be zero", nameof(residualBlockId));
|
||||
|
||||
var errorMessageBuffer = CreateErrorMessageBuffer();
|
||||
var errorCode = CeresNative.ceres_wrapper_problem_remove_residual_block(
|
||||
_handle.DangerousGetHandle(),
|
||||
residualBlockId,
|
||||
errorMessageBuffer,
|
||||
errorMessageBuffer.Capacity);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
var errorMessage = ExtractErrorMessage(errorMessageBuffer);
|
||||
throw new CeresException(errorCode, errorMessage);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Manifold Management
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Sets a manifold for a parameter block.
|
||||
/// </summary>
|
||||
public void SetManifold(double[] parameters, Manifold manifold)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
if (manifold == null)
|
||||
throw new ArgumentNullException(nameof(manifold));
|
||||
|
||||
WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
CeresNative.ceres_wrapper_problem_set_manifold(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
manifold.Handle);
|
||||
});
|
||||
|
||||
// CRITICAL: Keep reference to manifold to prevent GC collection
|
||||
// Native code may still call manifold callbacks (Plus/Minus) even after solve completes,
|
||||
// especially for AutoDiffManifold with GCHandle callbacks, so we must keep the object alive
|
||||
// for the lifetime of the Problem
|
||||
lock (_manifolds)
|
||||
{
|
||||
_manifolds.Add(manifold);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Parameter Bounds
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Sets a lower bound for a parameter.
|
||||
/// </summary>
|
||||
/// <summary>
|
||||
/// Sets a lower bound for a parameter in a parameter block.
|
||||
/// </summary>
|
||||
public void SetParameterLowerBound(double[] parameters, int index, double lowerBound)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
if (index < 0 || index >= parameters.Length)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
|
||||
WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
CeresNative.ceres_wrapper_problem_set_parameter_lower_bound(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
index,
|
||||
lowerBound);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sets an upper bound for a parameter.
|
||||
/// </summary>
|
||||
public void SetParameterUpperBound(double[] parameters, int index, double upperBound)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
if (index < 0 || index >= parameters.Length)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
|
||||
WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
CeresNative.ceres_wrapper_problem_set_parameter_upper_bound(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
index,
|
||||
upperBound);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the lower bound for a parameter.
|
||||
/// </summary>
|
||||
public double GetParameterLowerBound(double[] parameters, int index)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
if (index < 0 || index >= parameters.Length)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_get_parameter_lower_bound(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
index);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the upper bound for a parameter.
|
||||
/// </summary>
|
||||
public double GetParameterUpperBound(double[] parameters, int index)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
if (index < 0 || index >= parameters.Length)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_get_parameter_upper_bound(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr,
|
||||
index);
|
||||
});
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Query Methods
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of parameter blocks in the problem.
|
||||
/// </summary>
|
||||
public int NumParameterBlocks =>
|
||||
CeresNative.ceres_wrapper_problem_num_parameter_blocks(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of residual blocks in the problem.
|
||||
/// </summary>
|
||||
public int NumResidualBlocks =>
|
||||
CeresNative.ceres_wrapper_problem_num_residual_blocks(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the total number of parameters in the problem.
|
||||
/// </summary>
|
||||
public int NumParameters =>
|
||||
CeresNative.ceres_wrapper_problem_num_parameters(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the total number of residuals in the problem.
|
||||
/// </summary>
|
||||
public int NumResiduals =>
|
||||
CeresNative.ceres_wrapper_problem_num_residuals(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Checks if a parameter block exists in the problem.
|
||||
/// </summary>
|
||||
public bool HasParameterBlock(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_has_parameter_block(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr) != 0;
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Checks if a parameter block is constant.
|
||||
/// </summary>
|
||||
public bool IsParameterBlockConstant(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_is_parameter_block_constant(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr) != 0;
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the size of a parameter block.
|
||||
/// </summary>
|
||||
public int GetParameterBlockSize(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_get_parameter_block_size(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the tangent size of a parameter block (with manifold).
|
||||
/// </summary>
|
||||
public int GetParameterBlockTangentSize(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_get_parameter_block_tangent_size(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr);
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Checks if a parameter block has a manifold.
|
||||
/// </summary>
|
||||
public bool HasManifold(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_has_manifold(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr) != 0;
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the manifold handle for a parameter block (returns IntPtr.Zero if no manifold).
|
||||
/// Note: This returns the native handle. To get a Manifold object, use HasManifold first.
|
||||
/// </summary>
|
||||
public IntPtr GetManifoldHandle(double[] parameters)
|
||||
{
|
||||
if (parameters == null)
|
||||
throw new ArgumentNullException(nameof(parameters));
|
||||
|
||||
return WithPinnedArray(parameters, (ptr) =>
|
||||
{
|
||||
return CeresNative.ceres_wrapper_problem_get_manifold(
|
||||
_handle.DangerousGetHandle(),
|
||||
ptr);
|
||||
});
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Solve
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Solves the problem with the given options and returns a summary.
|
||||
/// </summary>
|
||||
/// <exception cref="CeresException">Thrown if the solve operation fails.</exception>
|
||||
public SolverSummary Solve(SolverOptions options)
|
||||
{
|
||||
if (options == null)
|
||||
throw new ArgumentNullException(nameof(options));
|
||||
|
||||
// Check if disposed
|
||||
if (_disposed)
|
||||
{
|
||||
throw new ObjectDisposedException(nameof(Problem), "Cannot solve a disposed problem");
|
||||
}
|
||||
|
||||
// Validate _handle
|
||||
if (_handle == null)
|
||||
{
|
||||
throw new InvalidOperationException("Problem handle is null");
|
||||
}
|
||||
|
||||
if (_handle.IsInvalid)
|
||||
{
|
||||
throw new InvalidOperationException("Problem handle is invalid");
|
||||
}
|
||||
|
||||
// Validate options.Handle
|
||||
if (options.Handle == null)
|
||||
{
|
||||
throw new InvalidOperationException("SolverOptions handle is null");
|
||||
}
|
||||
|
||||
if (options.Handle.IsInvalid)
|
||||
{
|
||||
throw new InvalidOperationException("SolverOptions handle is invalid");
|
||||
}
|
||||
|
||||
var summaryHandle = SolverSummaryHandle.Create();
|
||||
|
||||
if (summaryHandle == null)
|
||||
{
|
||||
throw new InvalidOperationException("Failed to create SolverSummaryHandle");
|
||||
}
|
||||
|
||||
if (summaryHandle.IsInvalid)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
throw new InvalidOperationException("SolverSummaryHandle is invalid");
|
||||
}
|
||||
|
||||
// Track ownership transfer to prevent double-free
|
||||
// Once SolverSummary is created, it owns summaryHandle and will dispose it
|
||||
bool ownershipTransferred = false;
|
||||
|
||||
try
|
||||
{
|
||||
// Extract handles before calling native function
|
||||
var problemHandle = _handle.DangerousGetHandle();
|
||||
var solverOptionsHandle = options.Handle.DangerousGetHandle();
|
||||
var summaryHandlePtr = summaryHandle.DangerousGetHandle();
|
||||
|
||||
// Validate handles are not null/zero
|
||||
if (problemHandle == IntPtr.Zero)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
throw new InvalidOperationException("Problem handle pointer is zero");
|
||||
}
|
||||
|
||||
if (solverOptionsHandle == IntPtr.Zero)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
throw new InvalidOperationException("SolverOptions handle pointer is zero");
|
||||
}
|
||||
|
||||
if (summaryHandlePtr == IntPtr.Zero)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
throw new InvalidOperationException("SolverSummary handle pointer is zero");
|
||||
}
|
||||
|
||||
var errorMessageBuffer = CreateErrorMessageBuffer(512);
|
||||
if (errorMessageBuffer == null)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
throw new InvalidOperationException("Failed to create error message buffer");
|
||||
}
|
||||
|
||||
var errorCode = CeresNative.ceres_wrapper_solve(
|
||||
problemHandle,
|
||||
solverOptionsHandle,
|
||||
summaryHandlePtr,
|
||||
errorMessageBuffer,
|
||||
errorMessageBuffer.Capacity);
|
||||
|
||||
if (errorCode != CeresErrorCode.Success)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
var message = ExtractErrorMessage(errorMessageBuffer);
|
||||
throw new CeresException(errorCode, message);
|
||||
}
|
||||
|
||||
// CRITICAL: Create SolverSummary and transfer ownership of summaryHandle
|
||||
// After SolverSummary is created successfully, ownership is transferred
|
||||
// and summaryHandle will be disposed when SolverSummary is disposed
|
||||
var summary = new SolverSummary(summaryHandle);
|
||||
// Ownership successfully transferred to SolverSummary
|
||||
ownershipTransferred = true;
|
||||
return summary;
|
||||
}
|
||||
catch (CeresException)
|
||||
{
|
||||
// Dispose summaryHandle only if ownership hasn't been transferred to SolverSummary
|
||||
// This prevents double-free if SolverSummary was created successfully
|
||||
if (!ownershipTransferred)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
}
|
||||
throw;
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Dispose summaryHandle only if ownership hasn't been transferred to SolverSummary
|
||||
// This prevents double-free if SolverSummary was created successfully
|
||||
if (!ownershipTransferred)
|
||||
{
|
||||
summaryHandle?.Dispose();
|
||||
}
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// IDisposable
|
||||
// ============================================================================
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
// Suppress finalizer to prevent crash in finalizer thread
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
private void Dispose(bool disposing)
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
// CRITICAL: Dispose native Problem handle FIRST
|
||||
// This ensures native code has finished using all callbacks and objects
|
||||
if (_handle != null)
|
||||
{
|
||||
_handle.Dispose();
|
||||
// Suppress finalization to prevent crash in finalizer thread
|
||||
GC.SuppressFinalize(_handle);
|
||||
}
|
||||
|
||||
// CRITICAL: Free GCHandles in CostFunctions/LossFunctions AFTER native Problem is freed
|
||||
// Native code (Ceres) owns the CostFunction/LossFunction objects and will delete them
|
||||
// when Problem is freed. However, the C# wrapper objects still hold GCHandles
|
||||
// (for callbacks and wrappers) that MUST be freed to prevent memory leaks.
|
||||
//
|
||||
// IMPORTANT: We call FreeGCHandlesAfterProblemDisposed() instead of Dispose() because:
|
||||
// 1. Native objects are already deleted by Ceres when Problem is freed
|
||||
// 2. CostFunctionHandle.ReleaseNativeHandle() is intentionally a no-op to prevent double-free
|
||||
// 3. But we MUST free GCHandles to prevent memory leaks (GCHandles pin memory)
|
||||
//
|
||||
// FreeGCHandlesAfterProblemDisposed() will:
|
||||
// - Free _callbackHandle and _nativeCallbackHandle in base CostFunction
|
||||
// - Free _wrapperHandle in derived classes (AutoDiffCostFunction, DynamicAutoDiffCostFunction, etc.)
|
||||
// - Dispose the SafeHandle (which is a no-op but ensures proper state)
|
||||
// - Mark as disposed to prevent double-free
|
||||
CostFunction[] costFunctionsToFree;
|
||||
LossFunction[] lossFunctionsToFree;
|
||||
Manifold[] manifoldsToFree;
|
||||
|
||||
lock (_costFunctions)
|
||||
{
|
||||
costFunctionsToFree = _costFunctions.ToArray();
|
||||
_costFunctions.Clear();
|
||||
}
|
||||
|
||||
lock (_lossFunctions)
|
||||
{
|
||||
lossFunctionsToFree = _lossFunctions.ToArray();
|
||||
_lossFunctions.Clear();
|
||||
}
|
||||
|
||||
lock (_manifolds)
|
||||
{
|
||||
manifoldsToFree = _manifolds.ToArray();
|
||||
_manifolds.Clear();
|
||||
}
|
||||
|
||||
// Free GCHandles in CostFunctions (outside lock to avoid deadlock)
|
||||
foreach (var costFunction in costFunctionsToFree)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Free GCHandles without trying to delete native handle (already freed by Ceres)
|
||||
costFunction.FreeGCHandlesAfterProblemDisposed();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors - may already be freed or disposed
|
||||
}
|
||||
}
|
||||
|
||||
// Note: LossFunctions and Manifolds don't have GCHandles that need explicit freeing.
|
||||
// Arrays will go out of scope after method returns, allowing GC to collect them.
|
||||
// No need to explicitly set to null.
|
||||
|
||||
// Unpin parameter blocks AFTER native Problem is freed
|
||||
// Use a copy to avoid issues during enumeration
|
||||
GCHandle[] handlesToFree;
|
||||
lock (_pinnedParameterBlocks)
|
||||
{
|
||||
handlesToFree = new GCHandle[_pinnedParameterBlocks.Count];
|
||||
_pinnedParameterBlocks.Values.CopyTo(handlesToFree, 0);
|
||||
_pinnedParameterBlocks.Clear();
|
||||
}
|
||||
|
||||
// Free handles outside the lock
|
||||
foreach (var handle in handlesToFree)
|
||||
{
|
||||
if (handle.IsAllocated) handle.Free();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Finalizer path - free native handle first, then GCHandles
|
||||
if (_handle != null)
|
||||
{
|
||||
_handle.Dispose();
|
||||
}
|
||||
// Free pinned GCHandles to prevent permanent memory leak
|
||||
// GCHandles are NOT automatically freed by GC - they must be explicitly freed.
|
||||
// At finalization time, native Problem is freed above, so it's safe to unpin.
|
||||
lock (_pinnedParameterBlocks)
|
||||
{
|
||||
foreach (var handle in _pinnedParameterBlocks.Values)
|
||||
{
|
||||
try { if (handle.IsAllocated) handle.Free(); } catch { }
|
||||
}
|
||||
_pinnedParameterBlocks.Clear();
|
||||
}
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
|
||||
~Problem()
|
||||
{
|
||||
// Finalizer - cleanup native handle and free pinned GCHandles
|
||||
// GCHandles are NOT automatically freed by GC, so we must free them explicitly
|
||||
Dispose(false);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,409 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using CeresSharp.Enums;
|
||||
using CeresSharp.Exceptions;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Options for configuring the Ceres Solver.
|
||||
/// </summary>
|
||||
public sealed class SolverOptions : IDisposable
|
||||
{
|
||||
private readonly SolverOptionsHandle _handle;
|
||||
private System.Runtime.InteropServices.GCHandle? _iterationCallbackHandle;
|
||||
private System.Runtime.InteropServices.GCHandle? _iterationNativeCallbackHandle;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a new SolverOptions instance with default settings.
|
||||
/// </summary>
|
||||
public SolverOptions()
|
||||
{
|
||||
_handle = SolverOptionsHandle.Create();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the native handle.
|
||||
/// </summary>
|
||||
internal SolverOptionsHandle Handle => _handle;
|
||||
|
||||
// ============================================================================
|
||||
// Basic Options
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the linear solver type.
|
||||
/// </summary>
|
||||
public LinearSolverType LinearSolverType
|
||||
{
|
||||
get => (LinearSolverType)CeresNative.ceres_wrapper_solver_options_get_linear_solver_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_linear_solver_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the minimizer type.
|
||||
/// </summary>
|
||||
public MinimizerType MinimizerType
|
||||
{
|
||||
get => (MinimizerType)CeresNative.ceres_wrapper_solver_options_get_minimizer_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_minimizer_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum number of iterations.
|
||||
/// </summary>
|
||||
public int MaxNumIterations
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_num_iterations(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_num_iterations(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the number of threads to use.
|
||||
/// </summary>
|
||||
public int NumThreads
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_num_threads(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_num_threads(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Tolerances
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the function tolerance.
|
||||
/// </summary>
|
||||
public double FunctionTolerance
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_function_tolerance(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_function_tolerance(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the gradient tolerance.
|
||||
/// </summary>
|
||||
public double GradientTolerance
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_gradient_tolerance(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_gradient_tolerance(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the parameter tolerance.
|
||||
/// </summary>
|
||||
public double ParameterTolerance
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_parameter_tolerance(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_parameter_tolerance(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Trust Region Options
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the initial trust region radius.
|
||||
/// </summary>
|
||||
public double InitialTrustRegionRadius
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_initial_trust_region_radius(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_initial_trust_region_radius(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum trust region radius.
|
||||
/// </summary>
|
||||
public double MaxTrustRegionRadius
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_trust_region_radius(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_trust_region_radius(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the minimum trust region radius.
|
||||
/// </summary>
|
||||
public double MinTrustRegionRadius
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_min_trust_region_radius(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_min_trust_region_radius(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the trust region strategy type.
|
||||
/// </summary>
|
||||
public TrustRegionStrategyType TrustRegionStrategyType
|
||||
{
|
||||
get => (TrustRegionStrategyType)CeresNative.ceres_wrapper_solver_options_get_trust_region_strategy_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_trust_region_strategy_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the dogleg type.
|
||||
/// </summary>
|
||||
public DoglegType DoglegType
|
||||
{
|
||||
get => (DoglegType)CeresNative.ceres_wrapper_solver_options_get_dogleg_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_dogleg_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Preconditioner Options
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the preconditioner type.
|
||||
/// </summary>
|
||||
public PreconditionerType PreconditionerType
|
||||
{
|
||||
get => (PreconditionerType)CeresNative.ceres_wrapper_solver_options_get_preconditioner_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_preconditioner_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Other Options
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets whether to use non-monotonic steps.
|
||||
/// </summary>
|
||||
public bool UseNonmonotonicSteps
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_use_nonmonotonic_steps(_handle.DangerousGetHandle()) != 0;
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_use_nonmonotonic_steps(_handle.DangerousGetHandle(), value ? 1 : 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum consecutive non-monotonic steps.
|
||||
/// </summary>
|
||||
public int MaxConsecutiveNonmonotonicSteps
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_consecutive_nonmonotonic_steps(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_consecutive_nonmonotonic_steps(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum number of consecutive invalid steps.
|
||||
/// </summary>
|
||||
public int MaxNumConsecutiveInvalidSteps
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_num_consecutive_invalid_steps(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_num_consecutive_invalid_steps(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the minimum relative decrease.
|
||||
/// </summary>
|
||||
public double MinRelativeDecrease
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_min_relative_decrease(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_min_relative_decrease(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the logging type.
|
||||
/// </summary>
|
||||
public LoggingType LoggingType
|
||||
{
|
||||
get => (LoggingType)CeresNative.ceres_wrapper_solver_options_get_logging_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_logging_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets whether to output minimizer progress to stdout.
|
||||
/// </summary>
|
||||
public bool MinimizerProgressToStdout
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_minimizer_progress_to_stdout(_handle.DangerousGetHandle()) != 0;
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_minimizer_progress_to_stdout(_handle.DangerousGetHandle(), value ? 1 : 0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Line Search Options
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the line search type.
|
||||
/// </summary>
|
||||
public LineSearchType LineSearchType
|
||||
{
|
||||
get => (LineSearchType)CeresNative.ceres_wrapper_solver_options_get_line_search_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_line_search_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the line search direction type.
|
||||
/// </summary>
|
||||
public LineSearchDirectionType LineSearchDirectionType
|
||||
{
|
||||
get => (LineSearchDirectionType)CeresNative.ceres_wrapper_solver_options_get_line_search_direction_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_line_search_direction_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the nonlinear conjugate gradient type.
|
||||
/// </summary>
|
||||
public NonlinearConjugateGradientType NonlinearConjugateGradientType
|
||||
{
|
||||
get => (NonlinearConjugateGradientType)CeresNative.ceres_wrapper_solver_options_get_nonlinear_conjugate_gradient_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_nonlinear_conjugate_gradient_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum L-BFGS rank.
|
||||
/// </summary>
|
||||
public int MaxLbfgsRank
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_lbfgs_rank(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_lbfgs_rank(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Linear Solver Options
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the sparse linear algebra library type.
|
||||
/// </summary>
|
||||
public SparseLinearAlgebraLibraryType SparseLinearAlgebraLibraryType
|
||||
{
|
||||
get => (SparseLinearAlgebraLibraryType)CeresNative.ceres_wrapper_solver_options_get_sparse_linear_algebra_library_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_sparse_linear_algebra_library_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the dense linear algebra library type.
|
||||
/// </summary>
|
||||
public DenseLinearAlgebraLibraryType DenseLinearAlgebraLibraryType
|
||||
{
|
||||
get => (DenseLinearAlgebraLibraryType)CeresNative.ceres_wrapper_solver_options_get_dense_linear_algebra_library_type(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_dense_linear_algebra_library_type(_handle.DangerousGetHandle(), (int)value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum linear solver iterations.
|
||||
/// </summary>
|
||||
public int MaxLinearSolverIterations
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_linear_solver_iterations(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_linear_solver_iterations(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the minimum linear solver iterations.
|
||||
/// </summary>
|
||||
public int MinLinearSolverIterations
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_min_linear_solver_iterations(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_min_linear_solver_iterations(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the linear solver tolerance.
|
||||
/// </summary>
|
||||
public double LinearSolverTolerance
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_linear_solver_tolerance(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_linear_solver_tolerance(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Inner Iterations
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets whether to use inner iterations.
|
||||
/// </summary>
|
||||
public bool UseInnerIterations
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_use_inner_iterations(_handle.DangerousGetHandle()) != 0;
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_use_inner_iterations(_handle.DangerousGetHandle(), value ? 1 : 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the inner iteration tolerance.
|
||||
/// </summary>
|
||||
public double InnerIterationTolerance
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_inner_iteration_tolerance(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_inner_iteration_tolerance(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Timing
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the maximum solver time in seconds.
|
||||
/// </summary>
|
||||
public double MaxSolverTimeInSeconds
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_max_solver_time_in_seconds(_handle.DangerousGetHandle());
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_max_solver_time_in_seconds(_handle.DangerousGetHandle(), value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets whether to update state every iteration.
|
||||
/// </summary>
|
||||
public bool UpdateStateEveryIteration
|
||||
{
|
||||
get => CeresNative.ceres_wrapper_solver_options_get_update_state_every_iteration(_handle.DangerousGetHandle()) != 0;
|
||||
set => CeresNative.ceres_wrapper_solver_options_set_update_state_every_iteration(_handle.DangerousGetHandle(), value ? 1 : 0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Validation
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Validates the solver options.
|
||||
/// </summary>
|
||||
/// <returns>True if valid, false otherwise.</returns>
|
||||
public bool IsValid(out string? errorMessage)
|
||||
{
|
||||
var sb = new StringBuilder(256);
|
||||
var isValid = CeresNative.ceres_wrapper_solver_options_is_valid(_handle.DangerousGetHandle(), sb, sb.Capacity) != 0;
|
||||
errorMessage = isValid ? null : sb.ToString();
|
||||
return isValid;
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// IDisposable
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Internal method to track iteration callback handles for cleanup.
|
||||
/// </summary>
|
||||
internal void SetIterationCallbackHandles(
|
||||
System.Runtime.InteropServices.GCHandle callbackHandle,
|
||||
System.Runtime.InteropServices.GCHandle nativeCallbackHandle)
|
||||
{
|
||||
// Free previous handles if any
|
||||
if (_iterationCallbackHandle.HasValue && _iterationCallbackHandle.Value.IsAllocated)
|
||||
_iterationCallbackHandle.Value.Free();
|
||||
if (_iterationNativeCallbackHandle.HasValue && _iterationNativeCallbackHandle.Value.IsAllocated)
|
||||
_iterationNativeCallbackHandle.Value.Free();
|
||||
|
||||
_iterationCallbackHandle = callbackHandle;
|
||||
_iterationNativeCallbackHandle = nativeCallbackHandle;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
// Cleanup callback handles
|
||||
if (_iterationCallbackHandle.HasValue && _iterationCallbackHandle.Value.IsAllocated)
|
||||
_iterationCallbackHandle.Value.Free();
|
||||
if (_iterationNativeCallbackHandle.HasValue && _iterationNativeCallbackHandle.Value.IsAllocated)
|
||||
_iterationNativeCallbackHandle.Value.Free();
|
||||
|
||||
_handle?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,206 @@
|
||||
using System;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using CeresSharp.Enums;
|
||||
using CeresSharp.Native;
|
||||
using CeresSharp.Native.SafeHandles;
|
||||
|
||||
namespace CeresSharp;
|
||||
|
||||
/// <summary>
|
||||
/// Summary of the solver execution results.
|
||||
/// </summary>
|
||||
public sealed class SolverSummary : IDisposable
|
||||
{
|
||||
private readonly SolverSummaryHandle _handle;
|
||||
private bool _disposed;
|
||||
|
||||
internal SolverSummary(SolverSummaryHandle handle)
|
||||
{
|
||||
_handle = handle ?? throw new ArgumentNullException(nameof(handle));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates a SolverSummary from a native handle (for callbacks).
|
||||
/// </summary>
|
||||
internal SolverSummary(IntPtr handle, bool ownsHandle)
|
||||
{
|
||||
_handle = new SolverSummaryHandle(handle, ownsHandle);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the native handle.
|
||||
/// </summary>
|
||||
internal SolverSummaryHandle Handle => _handle;
|
||||
|
||||
// ============================================================================
|
||||
// Termination Information
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the termination type.
|
||||
/// </summary>
|
||||
public TerminationType TerminationType =>
|
||||
(TerminationType)CeresNative.ceres_wrapper_solver_summary_get_termination_type(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the termination message.
|
||||
/// </summary>
|
||||
public string Message
|
||||
{
|
||||
get
|
||||
{
|
||||
var sb = new StringBuilder(512);
|
||||
CeresNative.ceres_wrapper_solver_summary_get_message(_handle.DangerousGetHandle(), sb, sb.Capacity);
|
||||
return sb.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Cost Information
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the initial cost.
|
||||
/// </summary>
|
||||
public double InitialCost =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_initial_cost(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the final cost.
|
||||
/// </summary>
|
||||
public double FinalCost =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_final_cost(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the cost change (final - initial).
|
||||
/// </summary>
|
||||
public double CostChange =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_cost_change(_handle.DangerousGetHandle());
|
||||
|
||||
// ============================================================================
|
||||
// Iteration Information
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the total number of iterations.
|
||||
/// </summary>
|
||||
public int Iterations =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_iterations(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of successful steps.
|
||||
/// </summary>
|
||||
public int NumSuccessfulSteps =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_successful_steps(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of unsuccessful steps.
|
||||
/// </summary>
|
||||
public int NumUnsuccessfulSteps =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_unsuccessful_steps(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of inner iteration steps.
|
||||
/// </summary>
|
||||
public int NumInnerIterationSteps =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_inner_iteration_steps(_handle.DangerousGetHandle());
|
||||
|
||||
// ============================================================================
|
||||
// Timing Information
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the total time in seconds.
|
||||
/// </summary>
|
||||
public double TotalTimeInSeconds =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_total_time_in_seconds(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the preprocessor time in seconds.
|
||||
/// </summary>
|
||||
public double PreprocessorTimeInSeconds =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_preprocessor_time_in_seconds(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the minimizer time in seconds.
|
||||
/// </summary>
|
||||
public double MinimizerTimeInSeconds =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_minimizer_time_in_seconds(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the postprocessor time in seconds.
|
||||
/// </summary>
|
||||
public double PostprocessorTimeInSeconds =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_postprocessor_time_in_seconds(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the linear solver time in seconds.
|
||||
/// </summary>
|
||||
public double LinearSolverTimeInSeconds =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_linear_solver_time_in_seconds(_handle.DangerousGetHandle());
|
||||
|
||||
// ============================================================================
|
||||
// Statistics
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of parameter blocks.
|
||||
/// </summary>
|
||||
public int NumParameterBlocks =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_parameter_blocks(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of parameters.
|
||||
/// </summary>
|
||||
public int NumParameters =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_parameters(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of effective parameters.
|
||||
/// </summary>
|
||||
public int NumEffectiveParameters =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_effective_parameters(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of residual blocks.
|
||||
/// </summary>
|
||||
public int NumResidualBlocks =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_residual_blocks(_handle.DangerousGetHandle());
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of residuals.
|
||||
/// </summary>
|
||||
public int NumResiduals =>
|
||||
CeresNative.ceres_wrapper_solver_summary_get_num_residuals(_handle.DangerousGetHandle());
|
||||
|
||||
// ============================================================================
|
||||
// Full Report
|
||||
// ============================================================================
|
||||
|
||||
/// <summary>
|
||||
/// Gets the full solver report as a string.
|
||||
/// </summary>
|
||||
public string FullReport
|
||||
{
|
||||
get
|
||||
{
|
||||
var sb = new StringBuilder(4096);
|
||||
CeresNative.ceres_wrapper_solver_summary_get_full_report(_handle.DangerousGetHandle(), sb, sb.Capacity);
|
||||
return sb.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// IDisposable
|
||||
// ============================================================================
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_handle?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user