/// <summary>
/// Dispose of any resources.
/// </summary>
public override void Dispose()
{
if (handle.Handle != IntPtr.Zero)
{
ISAM2Lib.deletenavigator(handle);
handle = new HandleRef(this, IntPtr.Zero);
}
}
/// <summary>
/// Get the map estimate.
/// </summary>
/// <param name="plmodel">Side model that contains the covariances of the
/// pose-landmark pairs projected into the measurement space.
/// Takes the form E = JSJ^T + R, where
/// J is the measurement jacobian in both the pose and landmark directions,
/// S is the joint covariance of the best estimate current pose and the landmark and
/// R is the measurement noise.
/// </param>
/// <returns>Map estimate as points with certain covariance (gaussians).</returns>
private unsafe IndexedMap GetMapModel(out IndexedMap plmodel)
{
int length;
Gaussian component;
IndexedMap mapmodel = new IndexedMap(3);
plmodel = new IndexedMap(3);
double *ptrmapmodel = (double *)ISAM2Lib.getmapmodel(handle, out length);
double *ptrmapcov = (double *)ISAM2Lib.getmapcovariances(handle, out length);
double *ptrplcov = (double *)ISAM2Lib.getplcovariances(handle, out length);
// main model: mean and covariance
for (int i = 0, k = 0, h = 0; i < length; i++, k += 3, h += 9)
{
double[] mean = new double[3];
double[][] covariance = { new double[3], new double[3], new double[3] };
mean[0] = ptrmapmodel[k + 0];
mean[1] = ptrmapmodel[k + 1];
mean[2] = ptrmapmodel[k + 2];
covariance[0][0] = ptrmapcov[h + 0];
covariance[0][1] = ptrmapcov[h + 1];
covariance[0][2] = ptrmapcov[h + 2];
covariance[1][0] = ptrmapcov[h + 3];
covariance[1][1] = ptrmapcov[h + 4];
covariance[1][2] = ptrmapcov[h + 5];
covariance[2][0] = ptrmapcov[h + 6];
covariance[2][1] = ptrmapcov[h + 7];
covariance[2][2] = ptrmapcov[h + 8];
component = new Gaussian(mean, covariance, 1.0);
mapmodel.Add(component);
}
// side model: covariance projected into measurement space
// includes pose uncertainty and measurement noise
for (int i = 0, h = 0; i < length; i++, h += 9)
{
double[][] covariance = { new double[3], new double[3], new double[3] };
covariance[0][0] = ptrplcov[h + 0];
covariance[0][1] = ptrplcov[h + 1];
covariance[0][2] = ptrplcov[h + 2];
covariance[1][0] = ptrplcov[h + 3];
covariance[1][1] = ptrplcov[h + 4];
covariance[1][2] = ptrplcov[h + 5];
covariance[2][0] = ptrplcov[h + 6];
covariance[2][1] = ptrplcov[h + 7];
covariance[2][2] = ptrplcov[h + 8];
component = new Gaussian(new double[3], covariance, 1.0);
plmodel.Add(component);
}
return(mapmodel);
}
/// <summary>
/// Update the solver estimates from unmanaged code.
/// </summary>
/// <param name="newstate">Updated state after odometry as [tx, ty, tz, qw, qx, qy, qz].</param>
/// <param name="measurements">Vectorized measurement list as [x1, y1, z1, x2, y2, z2, ...].</param>
/// <param name="labels">Data association labels, one per measurement.</param>
/// <param name="nmeasurements">Number of measurements.</param>
/// <returns>Error code: 0 = ok, 1 = divergence, 2 = duplicate var, -1 = general error.</returns>
private unsafe int Update(double[] newstate, double[] measurements, int[] labels, int nmeasurements)
{
int retvalue = -1;
fixed(double *ptrNewState = newstate)
{
fixed(double *ptrMeasurements = measurements)
{
fixed(int *ptrLabels = labels)
{
retvalue = ISAM2Lib.update(handle, (IntPtr)ptrNewState, (IntPtr)ptrMeasurements, (IntPtr)ptrLabels, nmeasurements, OnlyMapping);
}
}
}
return(retvalue);
}
/// <summary>
/// Get the trajectory estimate from unmanaged code.
/// </summary>
/// <returns>Trajectory estimate.</returns>
private unsafe List <double[]> GetTrajectory()
{
int length;
List <double[]> trajectory = new List <double[]>();
double *ptrtrajectory = (double *)ISAM2Lib.gettrajectory(handle, out length);
for (int i = 0, k = 0; i < length; i++)
{
double[] point = new double[StateSize];
for (int h = 0; h < StateSize; h++, k++)
{
point[h] = ptrtrajectory[k];
}
trajectory.Add(point);
}
return(trajectory);
}
/// <summary>
/// Construct a new ISAM2Navigator from unmanaged code.
/// </summary>
/// <param name="initPose">Initial pose.</param>
/// <param name="measurementNoise">Vectorized measurement noise.</param>
/// <param name="motionNoise">Vectorized motion noise.</param>
/// <param name="mparams">Measurer configuration parameters.</param>
/// <returns>New navigator.</returns>
private unsafe HandleRef NewNavigator(double[] initPose, double[] measurementNoise,
double[] motionNoise, double[] mparams)
{
IntPtr ptr = IntPtr.Zero;
fixed(double *ptrInitPose = initPose)
{
fixed(double *ptrMeasurementNoise = measurementNoise)
{
fixed(double *ptrMotionNoise = motionNoise)
{
fixed(double *ptrMParams = mparams)
{
ptr = ISAM2Lib.newnavigator((IntPtr)ptrInitPose, (IntPtr)ptrMeasurementNoise,
(IntPtr)ptrMotionNoise, (IntPtr)ptrMParams);
}
}
}
}
return(new HandleRef(this, ptr));
}