public Vector3 MatchData(Vector3 beginEstimateWorld, OccGridMapUtil gridMapUtil, DataContainer dataContainer, out Matrix4x4 covMatrix, int maxIterations)
{
covMatrix = Matrix4x4.Identity;
if (drawInterface != null)
{
drawInterface.SetScale(0.05f);
drawInterface.SetColor(0.0f, 1.0f, 0.0f);
drawInterface.DrawArrow(beginEstimateWorld);
Vector3 beginEstimateMap = gridMapUtil.GetMapCoordsPose(beginEstimateWorld);
DrawScan(beginEstimateMap, gridMapUtil, dataContainer);
drawInterface.SetColor(1.0, 0.0, 0.0);
}
if (dataContainer.Count != 0)
{
Vector3 beginEstimateMap = gridMapUtil.GetMapCoordsPose(beginEstimateWorld);
Vector3 estimate = beginEstimateMap;
EstimateTransformationLogLh(ref estimate, gridMapUtil, dataContainer);
int numIter = maxIterations;
for (int i = 0; i < numIter; ++i)
{
EstimateTransformationLogLh(ref estimate, gridMapUtil, dataContainer);
if (drawInterface != null)
{
float invNumIterf = 1.0f / numIter;
drawInterface.SetColor(i * invNumIterf, 0.0f, 0.0f);
drawInterface.DrawArrow(gridMapUtil.GetWorldCoordsPose(estimate));
}
if (debugInterface != null)
{
debugInterface.AddHessianMatrix(H);
}
}
if (drawInterface != null)
{
drawInterface.SetColor(0.0, 0.0, 1.0);
DrawScan(estimate, gridMapUtil, dataContainer);
}
estimate.Z = Util.Util.NormalizeAngle(estimate.Z);
covMatrix = H;
return(gridMapUtil.GetWorldCoordsPose(estimate));
}
return(beginEstimateWorld);
}
