public OccupancyGrid2DLogOdds(IOccupancyGrid2D inputGrid, double decayFactor, double gridDecay)
{
this.currentOccupancyGrid = inputGrid;
this.decayFactor = decayFactor;
this.gridDecay = gridDecay;
this.outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
}
public OccupancyGrid2DLogOdds(IOccupancyGrid2D inputGrid, double decayFactor, double gridDecay)
{
this.currentOccupancyGrid = inputGrid;
this.decayFactor = decayFactor;
this.gridDecay = gridDecay;
this.outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
}
public void UpdateLidarScan(ILidarScan <ILidar2DPoint> s)
{
lock (dataLocker)
{
Stopwatch sw = new Stopwatch();
sw.Start();
curScan2D = s;
if (curRobotPose != null && curScan2D != null /*&& curRobotPose.timestamp != priorTimeStamp && curRobotPose.timestamp != 0*/)
{
//Get the laser to body coordinate system (this is I for now)
Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody);
//Get the body to global transformation
Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose);
//Get a vector from the current lidar pose
Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z);
//Transform the sensor position in body coordinates to the sensor position in global coordinates
Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody);
//Get the current grid indicies
int xLidarPoseIndex, yLidarPoseIndex;
currentOccupancyGrid.GetIndicies(lidarInGlobal.X, lidarInGlobal.Y, out xLidarPoseIndex, out yLidarPoseIndex);
//Find the cells corresponding to each LIDAR return and make a list of the cells that are clear from the sensor to that point
Dictionary <Vector2, Boolean> occupiedCellsThisScan = new Dictionary <Vector2, Boolean>(curScan2D.Points.Count());
Dictionary <Vector2, Boolean> clearCellsThisScan = new Dictionary <Vector2, Boolean>();
//Process each lidar return
foreach (ILidar2DPoint pt in curScan2D.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
//Find the index of the laser return
int xLaserIndex, yLaserIndex;
currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex);
//Add to the list of occupied cells
if (currentOccupancyGrid.CheckValidIdx(xLaserIndex, yLaserIndex))
{
occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex - 1)] = true;
//occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex - 1)] = true;
}
}
}
//Process each lidar return
foreach (ILidar2DPoint pt in curScan2D.Points)
{
if (pt.RThetaPoint.R < lidarMaxPracticalRange)
{
//Extract the lidar point in XYZ (laser coordinates)
Vector3 v3 = pt.RThetaPoint.ToVector3();
//Convert laser to body coordinate system
Vector3 vBody = Tlaser2body.TransformPoint(v3);
//Convert body to global cooridnate system
Vector3 vGlobal = Tbody2global.TransformPoint(vBody);
//Find the index of the laser return
int xLaserIndex, yLaserIndex;
currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex);
//Ray trace between the two points performing the update
Raytrace(xLidarPoseIndex, yLidarPoseIndex, xLaserIndex, yLaserIndex, occupiedCellsThisScan, clearCellsThisScan);
}
}
//decay the whole grid
for (int i = 0; i < currentOccupancyGrid.NumCellX; i++)
{
for (int j = 0; j < currentOccupancyGrid.NumCellY; j++)
{
double value = currentOccupancyGrid.GetCellByIdx(i, j);
currentOccupancyGrid.SetCellByIdx(i, j, value *= gridDecay);
}
}
foreach (Vector2 cellIdx in occupiedCellsThisScan.Keys)
{
UpdateCellOccupied((int)cellIdx.X, (int)cellIdx.Y);
}
foreach (Vector2 cellIdx in clearCellsThisScan.Keys)
{
UpdateCellClear((int)cellIdx.X, (int)cellIdx.Y);
}
//Copy for the timestamp for the next iteration
priorTimeStamp = (double)curRobotPose.timestamp;
}
// Console.WriteLine("OG Took: " + sw.ElapsedMilliseconds);
}//lock
if (outputOccupancyGrid != null)
{
outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
if (curRobotPose != null)
{
if (NewGridAvailable != null)
{
NewGridAvailable(this, new NewOccupancyGrid2DAvailableEventArgs(GetOccupancyGrid(), curRobotPose.timestamp));
}
}
}
}
//Constructor
public OccupancyGrid2DLogOdds(IOccupancyGrid2D inputOccupancyGrid)
{
//Assign the occupancy grid we're going to process from the input occupancy grid
currentOccupancyGrid = inputOccupancyGrid;
outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
}
/// <summary>
/// Update OccupancyGrid
/// </summary>
/// <param name="occupancyGrid">OccupancyGrid to be assigned</param>
public void UpdateOccupancyGrid(IOccupancyGrid2D occupancyGrid)
{
this.occupancyGrid = (IOccupancyGrid2D)occupancyGrid.DeepCopy();
}
//Constructor
public OccupancyGrid2DLogOdds(IOccupancyGrid2D inputOccupancyGrid)
{
//Assign the occupancy grid we're going to process from the input occupancy grid
currentOccupancyGrid = inputOccupancyGrid;
outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy();
}
/// <summary>
/// Update OccupancyGrid
/// </summary>
/// <param name="occupancyGrid">OccupancyGrid to be assigned</param>
public void UpdateOccupancyGrid(IOccupancyGrid2D occupancyGrid)
{
this.occupancyGrid = (IOccupancyGrid2D)occupancyGrid.DeepCopy();
}