/// <summary>
/// Constructor
/// </summary>
/// <param name="mapResolution"></param>
/// <param name="mapSizeX"></param>
/// <param name="mapSizeY"></param>
/// <param name="startCoords"></param>
/// <param name="multiResSize"></param>
/// <param name="drawInterface"></param>
/// <param name="debugInterface"></param>
public HectorSLAMProcessor(float mapResolution, int mapSizeX, int mapSizeY, Vector2 startCoords, int multiResSize, IDrawInterface drawInterface = null, IHectorDebugInfo debugInterface = null)
{
this.drawInterface = drawInterface;
this.debugInterface = debugInterface;
MapRep = new MapRepMultiMap(mapResolution, mapSizeX, mapSizeY, multiResSize, startCoords, drawInterface, debugInterface);
Reset();
MinDistanceDiffForMapUpdate = 0.4f * 1.0f;
MinAngleDiffForMapUpdate = 0.13f * 1.0f;
}
public MapRepMultiMap(float mapResolution, int mapSizeX, int mapSizeY, int numDepth, Vector2 startCoords, IDrawInterface drawInterface, IHectorDebugInfo debugInterface)
{
//unsigned int numDepth = 3;
Point resolution = new Point(mapSizeX, mapSizeY);
float totalMapSizeX = mapResolution * mapSizeX;
float mid_offset_x = totalMapSizeX * startCoords.X;
float totalMapSizeY = mapResolution * mapSizeY;
float mid_offset_y = totalMapSizeY * startCoords.Y;
mapContainer = new List <MapProcContainer>();
dataContainers = new List <DataContainer>(numDepth - 1);
for (int i = 0; i < numDepth; ++i)
{
System.Diagnostics.Debug.WriteLine($"HectorSM map lvl {i}: cellLength: {mapResolution} res x: {resolution.X} res y: {resolution.Y}");
GridMap gridMap = new GridMap(mapResolution, resolution, new Vector2(mid_offset_x, mid_offset_y));
OccGridMapUtilConfig gridMapUtil = new OccGridMapUtilConfig(gridMap);
ScanMatcher scanMatcher = new ScanMatcher(drawInterface, debugInterface);
mapContainer.Add(new MapProcContainer(gridMap, gridMapUtil, scanMatcher));
resolution = new Point(resolution.X / 2, resolution.Y / 2);
mapResolution *= 2.0f;
}
}
public MapRepSingleMap(float mapResolution, IDrawInterface drawInterface, IHectorDebugInfo debugInterface)
{
gridMap = new GridMap(mapResolution, new Point(1024, 1024), new Vector2(20.0f, 20.0f));
gridMapUtil = new OccGridMapUtilConfig(gridMap);
scanMatcher = new ScanMatcher(drawInterface, debugInterface);
}
public ScanMatcher(IDrawInterface drawInterface = null, IHectorDebugInfo debugInterface = null)
{
this.drawInterface = drawInterface;
this.debugInterface = debugInterface;
}
