/// <summary>
/// Get difference of two occupancy map
/// </summary>
/// <param name="globalMulti">ocGrid1</param>
/// <param name="globalSingle">ocGrid2</param>
/// <param name="currentPose">current position</param>
/// <param name="extentX">x-length of the comparison</param>
/// <param name="extentY">y-length of the comparison</param>
/// <returns>list of index classes - has ocGrid1 value</returns>
public UpdateMapDataMessage Diff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
{
if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID)) return null;
OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
//OccupancyGrid2D globalSingle = globalOcGridByEachRobot[robotID];
//List<Index> diffIndexToSend = new List<Index>();
List<Position> diffPositionToSend = new List<Position>();
//List<float> heightList = new List<float>();
//List<float> covList = new List<float>();
//List<float> pijList = new List<float>();
List<float> pijSum = new List<float>();
List<float> puHat = new List<float>();
List<float> puHatSquare = new List<float>();
List<float> pSigUhateSquare = new List<float>();
int numCellXHalf = (int)(extentX / globalMulti.ResolutionX);
int numCellYHalf = (int)(extentY / globalMulti.ResolutionY);
int currentCellX, currentCellY;
globalMulti.GetIndicies(currentPose.x, currentPose.y, out currentCellX, out currentCellY);
int comparisonCellX, comparisonCellY;
for (int i = 0; i < numCellYHalf * 2; i++) // [i, j] = [column, row]
{
for (int j = 0; j < numCellXHalf * 2; j++)
{
comparisonCellX = currentCellX - numCellXHalf + j;
comparisonCellY = currentCellY - numCellYHalf + i;
if (globalMulti.GetCellByIdx(comparisonCellX, comparisonCellY) != globalSingle.GetCellByIdx(comparisonCellX, comparisonCellY))
{
double x, y; globalMulti.GetReals(comparisonCellX, comparisonCellY, out x, out y);
diffPositionToSend.Add(new Position((float)x, (float)y));
//heightList.Add((float)gaussianMixMapAlgorithm.UhatGM.GetCellByIdx(j, i));
//covList.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCellByIdx(j, i));
//pijList.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCellByIdx(j, i));
pijSum.Add((float)gaussianMixMapAlgorithm.Pij_sum.GetCell(x, y));
puHat.Add((float)gaussianMixMapAlgorithm.Pu_hat.GetCell(x, y));
puHatSquare.Add((float)gaussianMixMapAlgorithm.Pu_hat_square.GetCell(x, y));
pSigUhateSquare.Add((float)gaussianMixMapAlgorithm.Psig_u_hat_square.GetCell(x, y));
}
}
}
return new UpdateMapDataMessage(robotID, diffPositionToSend, pijSum, puHat, puHatSquare, pSigUhateSquare);
}