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 MapProcContainer(GridMap gridMap, OccGridMapUtilConfig gridMapUtil, ScanMatcher scanMatcher)
{
GridMap = GridMap;
GridMapUtil = gridMapUtil;
ScanMatcher = scanMatcher;
MapMutex = null;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="mapResolution">Meters per pixel</param>
/// <param name="mapSize">Map size in pixels</param>
/// <param name="startPose">Start pose (X and Y in meters, Z in degrees)</param>
/// <param name="numDepth">Number of maps</param>
/// <param name="numThreads">Number of processing threads</param>
public HectorSLAMProcessor(float mapResolution, Point mapSize, Vector3 startPose, int numDepth, int numThreads, ILogger logger = null)
{
this.logger = logger;
this.startPose = startPose;
MapRep = new MapRepMultiMap(mapResolution, mapSize, numDepth, Vector2.Zero);
scanMatcher = new ScanMatcher(numThreads, logger);
// Set initial poses
MatchPose = startPose;
LastMapUpdatePose = new Vector3(float.MinValue, float.MinValue, float.MinValue);
}
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 Vector3 MatchData(Vector3 beginEstimateWorld, DataContainer dataContainer, out Matrix4x4 covMatrix, int maxIterations)
{
return(ScanMatcher.MatchData(beginEstimateWorld, GridMapUtil, dataContainer, out covMatrix, maxIterations));
}
