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)); }