void gaussMixMapClient_MsgReceived(object sender, MsgReceivedEventArgs <GlobalMapCell> e)
{
isWorking = true;
if (globalHeightMap == null)
{
double globalMapResolution = e.message.DataList[0].largeU; // first index has grid information
double globalMapExtent = e.message.DataList[0].largeSig;
globalHeightMap = new OccupancyGrid2D(globalMapResolution, globalMapResolution, globalMapExtent, globalMapExtent);
globalSigMap = new OccupancyGrid2D(globalHeightMap);
globalPijMap = new OccupancyGrid2D(globalHeightMap);
laserHitMap = new OccupancyGrid2D(globalHeightMap);
thresholdGlobalHeightMap = new OccupancyGrid2D(globalHeightMap);
}
// if message is received, update the 3 gridmap
GlobalGaussianMixMap.ConvertToOcGrid(ref globalHeightMap, ref globalSigMap, ref globalPijMap, ref laserHitMap,
e.message.DataList);
for (int i = 0; i < globalHeightMap.NumCellX; i++)
{
for (int j = 0; j < globalHeightMap.NumCellY; j++)
{
if (globalPijMap.GetCellByIdx(i, j) > 1)
{
thresholdGlobalHeightMap.SetCellByIdx(i, j, globalHeightMap.GetCellByIdx(i, j));
}
else
{
thresholdGlobalHeightMap.SetCellByIdx(i, j, 0);
}
}
}
}
/// <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));
}
public List <Polygon> GetClosed()
{
List <Polygon> pixels = new List <Polygon>();
for (int i = 0; i < closed.Width; i++)
{
for (int j = 0; j < closed.Height; j++)
{
if (closed.GetCellByIdx(i, j) != 0)
{
pixels.Add(DrawPixel((i - extentX / resX) * resX, (j - extentY / resY) * resY));
}
}
}
return(pixels);
}
/// <summary>
/// Returns arrays to send out through messaging
/// </summary>
/// <param name="largeUDiff"></param>
/// <param name="largeSigDiff"></param>
/// <param name="pijDiff"></param>
/// <param name="colIdx"></param>
/// <param name="rowIdx"></param>
public void GetArraysToSend(out List <Index> indexList, out List <float> heightList, out List <float> covList, out List <float> pijSumList, out List <float> laserHitList)
{
int index = 0;
if (indicesDictionary == null)
{
indexList = new List <Index>();
heightList = new List <float>();
covList = new List <float>();
pijSumList = new List <float>();
laserHitList = new List <float>();
return;
}
else
{
indexList = new List <Index>(indicesDictionary.Count);
heightList = new List <float>(indicesDictionary.Count);
covList = new List <float>(indicesDictionary.Count);
pijSumList = new List <float>(indicesDictionary.Count);
laserHitList = new List <float>(indicesDictionary.Count);
}
lock (locker)
{
foreach (KeyValuePair <Index, int> pair in indicesDictionary)
{
indexList.Add(pair.Key);
heightList.Add((float)uhatGM.GetCellByIdx(pair.Key.Col, pair.Key.Row));
covList.Add((float)sigSqrGM.GetCellByIdx(pair.Key.Col, pair.Key.Row));
pijSumList.Add((float)pij_sum.GetCellByIdx(pair.Key.Col, pair.Key.Row));
laserHitList.Add((float)laserHit.GetCellByIdx(pair.Key.Col, pair.Key.Row));
}
indicesDictionary.Clear();
}
//indexList = new List<Index>(this.indicesList);
//heightList = new List<float>(this.heightList);
//covList = new List<float>(this.covList);
//pijSumList = new List<float>(this.pijSumList);
//this.indicesList.Clear();
//this.heightList.Clear();
//this.covList.Clear();
//this.pijSumList.Clear();
//this.indicesDictionary.Clear();
}
public List <Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
{
List <Waypoint> path = new List <Waypoint>();
//added by aaron (sort of a hack)
if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
{
path.Add(new Waypoint(start.Coordinate, true, 0));
success = true;
return(path);
}
int xIdx, yIdx;
success = true;
Vector2[] NESWVector = new Vector2[4];
Vector2[] diagVector = new Vector2[4];
bool[] NESW = new bool[4];
Vector2 startV = start.Coordinate; // Start Vector2
Vector2 goalV = goal.Coordinate; // Goal Vector2
PriorityQueue open = new PriorityQueue();
closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
opened = new OccupancyGrid2D(resX, resY, extentX, extentY);
GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
startV = new Vector2(xIdx, yIdx);
GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
goalV = new Vector2(xIdx, yIdx);
Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);
Node current = root;
open.Push(current);
// Do the spreading/discovering stuff until we discover a path.
while (current.xy != startV)
{
if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
{
Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
success = false;
break;
}
current = open.Pop();
NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);
diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);
for (int i = 0; i < 4; i++)
{
if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
NESW[i] = true;
if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
+ og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
}
}
}
}
for (int i = 0; i < 4; i++)
{
if (NESW[i % 4] && NESW[(i + 1) % 4])
{
if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
+ og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
}
}
}
}
}
for (int i = 0; i < 4; i++)
{
NESW[i] = false;
}
closed.SetCellByIdx((int)current.xy.X, (int)current.xy.Y, 1);
}
// Build a path using the discovered path.
double x, y;
Waypoint waypoint;
// First waypoint is a user waypoint
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
// Middle waypoints are path waypoints
while (current != root && current != null)
{
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, false, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
}
// Last waypoint is a user waypoint
if (current != null)
{
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
}
return(path);
}
/// <summary>
/// Update OccupancyGrid based on lidarScan and robotPose received
/// </summary>
/// <param name="lidarScan"></param>
/// <param name="currentRobotPose"></param>
public void UpdateOccupancyGrid(ILidarScan <ILidar2DPoint> lidarScan, int robotID, RobotPose currentRobotPose, SensorPose lidarPose, List <Polygon> dynamicObstacles)
{
if (lidarPose == null)
{
lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
}
if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
{
JTpl = jacobianLaserPoseDictionary[robotID];
laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID];
}
else
{
Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
laser2RobotTransMatrixDictionary.Add(robotID, laserToRobotDCM);
jacobianLaserPoseDictionary.Add(robotID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
JTpl = jacobianLaserPoseDictionary[robotID];
}
// calculate robot2global transformation matrix
Matrix4 robot2GlocalDCM = Matrix4.FromPose(currentRobotPose);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
}
}
if (lidarScan == null)
{
return;
}
Stopwatch sw1 = new Stopwatch();
Stopwatch sw2 = new Stopwatch();
Stopwatch sw3 = new Stopwatch();
Stopwatch sw4 = new Stopwatch();
Stopwatch sw5 = new Stopwatch();
Stopwatch sw6 = new Stopwatch();
UMatrix JTpr = ComputeJacobian(currentRobotPose.yaw, currentRobotPose.pitch, currentRobotPose.roll);
List <UMatrix> JfPrCubixLaserToRobotDCM = new List <UMatrix>(6);
List <UMatrix> RobotToGlocalDCMJfPlCubix = new List <UMatrix>(6);
for (int i = 0; i < 6; i++)
{
//derivative of the robot transform matrtix w.r.t. some element of the robot psoe
UMatrix j = new UMatrix(4, 4);
j[0, 0] = JTpr[0, i]; j[1, 0] = JTpr[1, i]; j[2, 0] = JTpr[2, i]; j[3, 0] = JTpr[3, i];
j[0, 1] = JTpr[4, i]; j[1, 1] = JTpr[5, i]; j[2, 1] = JTpr[6, i]; j[3, 1] = JTpr[7, i];
j[0, 2] = JTpr[8, i]; j[1, 2] = JTpr[9, i]; j[2, 2] = JTpr[10, i]; j[3, 2] = JTpr[11, i];
j[0, 3] = JTpr[12, i]; j[1, 3] = JTpr[13, i]; j[2, 3] = JTpr[14, i]; j[3, 3] = JTpr[15, i];
JfPrCubixLaserToRobotDCM.Add(j * laserToRobotDCM);
UMatrix tempJacobianPl = new UMatrix(4, 4);
tempJacobianPl[0, 0] = JTpl[0, i]; tempJacobianPl[1, 0] = JTpl[1, i]; tempJacobianPl[2, 0] = JTpl[2, i]; tempJacobianPl[3, 0] = JTpl[3, i];
tempJacobianPl[0, 1] = JTpl[4, i]; tempJacobianPl[1, 1] = JTpl[5, i]; tempJacobianPl[2, 1] = JTpl[6, i]; tempJacobianPl[3, 1] = JTpl[7, i];
tempJacobianPl[0, 2] = JTpl[8, i]; tempJacobianPl[1, 2] = JTpl[9, i]; tempJacobianPl[2, 2] = JTpl[10, i]; tempJacobianPl[3, 2] = JTpl[11, i];
tempJacobianPl[0, 3] = JTpl[12, i]; tempJacobianPl[1, 3] = JTpl[13, i]; tempJacobianPl[2, 3] = JTpl[14, i]; tempJacobianPl[3, 3] = JTpl[15, i];
RobotToGlocalDCMJfPlCubix.Add(robotToGlocalDCM * tempJacobianPl);
}
UMatrix laserToENU = robotToGlocalDCM * laserToRobotDCM;
UMatrix pijCell = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
// update covariance
UpdateCovariance(currentRobotPose.covariance);
//SickPoint p = new SickPoint(new RThetaCoordinate(1.0f, 0.0f));
for (int laserIndex = 0; laserIndex < lidarScan.Points.Count; laserIndex++)
{
lock (locker)
{
ILidar2DPoint p = lidarScan.Points[laserIndex];
if (p.RThetaPoint.R >= MAXRANGE)
{
continue;
}
bool hitDynamicObstacles = false;
// figure out if this lidar point is hitting other robot
// find laser points in 3D space
// first define 2D point on the laser plane
UMatrix laserPoint = new UMatrix(4, 1);
double deg = (p.RThetaPoint.theta * 180.0 / Math.PI);
int thetaDegIndex = (int)Math.Round((deg + 90.0) * 2.0) % 360;
double cosTheta = cosLookup[thetaDegIndex];
double sinTheta = sinLookup[thetaDegIndex];
laserPoint[0, 0] = p.RThetaPoint.R * cosTheta;
laserPoint[1, 0] = p.RThetaPoint.R * sinTheta;
laserPoint[2, 0] = 0;
laserPoint[3, 0] = 1;
//calculate r_hat_ENU
UMatrix r_hat_ENU = laserToENU * laserPoint;
foreach (Polygon dp in dynamicObstacles)
{
if (dp.IsInside(new Vector2(r_hat_ENU[0, 0], r_hat_ENU[1, 0])))
{
hitDynamicObstacles = true;
break;
}
}
if (hitDynamicObstacles)
{
continue;
}
//-------------------------------//
// COVARIANCE UMatrix CALCULATION //
//-------------------------------//
UMatrix JRr = new UMatrix(4, 2);
JRr.Zero();
JRr[0, 0] = cosTheta;
JRr[0, 1] = -p.RThetaPoint.R * sinTheta;
JRr[1, 0] = sinTheta;
JRr[1, 1] = p.RThetaPoint.R * cosTheta;
UMatrix Jfr = new UMatrix(3, 2); // 3x2
Jfr = (laserToENU * JRr).Submatrix(0, 2, 0, 1); // 4x4 * 4x4 * 4x2
UMatrix Jfpr = new UMatrix(3, 6);
UMatrix Jfpl = new UMatrix(3, 6);
sw1.Reset();
sw1.Start();
for (int i = 0; i < 6; i++) //for each state var (i.e. x,y,z,y,p,r)
{
UMatrix JfprTemp = (JfPrCubixLaserToRobotDCM[i]) * laserPoint; // 4 by 1 UMatrix
Jfpr[0, i] = JfprTemp[0, 0];
Jfpr[1, i] = JfprTemp[1, 0];
Jfpr[2, i] = JfprTemp[2, 0];
//UMatrix JfplTemp = (RobotToGlocalDCMJfPlCubix[i]) * laserPoint; // 4 by 1 UMatrix
//Jfpl[0, i] = JfplTemp[0, 0];
//Jfpl[1, i] = JfplTemp[1, 0];
//Jfpl[2, i] = JfplTemp[2, 0];
}
sw1.Stop();
sw2.Reset();
sw2.Start();
UMatrix JfprQprJfprT = new UMatrix(3, 3);
UMatrix JfplQplJfplT = new UMatrix(3, 3);
UMatrix JfrQrJfrT = new UMatrix(3, 3);
JfprQprJfprT = (Jfpr * covRobotPose) * Jfpr.Transpose();
//JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose();
JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();
// add above variables together and get the covariance
UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
sw2.Stop();
sw3.Reset();
sw3.Start();
//-----------------------------//
// FIND WHICH CELLS TO COMPUTE //
//-----------------------------//
// find out cells around this laser point
int laserPointIndexX = 0;
int laserPointIndexY = 0;
//this is used just to do the transformation from reaal to grid and visa versa
psig_u_hat_square.GetIndicies(r_hat_ENU[0, 0], r_hat_ENU[1, 0], out laserPointIndexX, out laserPointIndexY); // get cell (r_hat_ENU_X, r_hat_ENU_y)
sw3.Stop();
sw4.Reset();
sw4.Start();
//-----------------------------------------//
// COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
//-----------------------------------------//
double sigX = Math.Sqrt(covRHatENU[0, 0]);
double sigY = Math.Sqrt(covRHatENU[1, 1]);
double rho = covRHatENU[1, 0] / (sigX * sigY);
double logTerm = Math.Log(2 * Math.PI * sigX * sigY * Math.Sqrt(1 - (rho * rho)));
double xTermDenom = (1 - (rho * rho));
for (int i = -rangeToApply; i <= rangeToApply; i++) // column
{
for (int j = -rangeToApply; j <= rangeToApply; j++) // row
{
// estimate using Bivariate Normal Distribution
double posX = 0; double posY = 0;
psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
posX += psig_u_hat_square.ResolutionX / 2;
posY += psig_u_hat_square.ResolutionY / 2;
double x = posX - r_hat_ENU[0, 0];
double y = posY - r_hat_ENU[1, 0];
double z = (x * x) / (sigX * sigX) -
(2 * rho * x * y / (sigX * sigY)) +
(y * y) / (sigY * sigY);
double xTerm = -0.5 * z / xTermDenom;
// chi2 test
//if ((2 * (1 - (rho * rho))) * ((x * x) / (sigX * sigX) + (y * y) / (sigY * sigY) - (2 * rho * x * y) / (sigX * sigY)) > 15.2)
// continue;
pijCell[j + rangeToApply, i + rangeToApply] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdx(laserPointIndexX + i, laserPointIndexY + j) + 1);
}
}
sw4.Stop();
sw5.Reset();
sw5.Start();
//---------------------------//
// COMPUTE HEIGHT ESTIMATION //
//---------------------------//
// Matrix2 PEN = new Matrix2(covRHatENU[0, 0], covRHatENU[0, 1], covRHatENU[1, 0], covRHatENU[1, 1]);
UMatrix PEN = covRHatENU.Submatrix(0, 1, 0, 1);
UMatrix PENInv = PEN.Inverse2x2;
UMatrix PuEN = new UMatrix(1, 2);
UMatrix PENu = new UMatrix(2, 1);
UMatrix PuENPENInv = PuEN * PENInv;
UMatrix uHatMatrix = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
UMatrix sigUHatMatrix = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
PuEN[0, 0] = covRHatENU[2, 0];
PuEN[0, 1] = covRHatENU[2, 1];
PENu[0, 0] = covRHatENU[0, 2];
PENu[1, 0] = covRHatENU[1, 2];
double sig_u_hat_product = (PuENPENInv * PENu)[0, 0]; // output = 1x1 UMatrix
for (int i = -rangeToApply; i <= rangeToApply; i++) // column
{
for (int j = -rangeToApply; j <= rangeToApply; j++) // row
{
UMatrix ENmr_EN = new UMatrix(2, 1);
double posX = 0; double posY = 0;
psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
ENmr_EN[0, 0] = posX - r_hat_ENU[0, 0];
ENmr_EN[1, 0] = posY - r_hat_ENU[1, 0];
double u_hat_product = (PuENPENInv * (ENmr_EN))[0, 0]; // output = 1x1 UMatrix
uHatMatrix[j + rangeToApply, i + rangeToApply] = r_hat_ENU[2, 0] + u_hat_product;
sigUHatMatrix[j + rangeToApply, i + rangeToApply] = covRHatENU[2, 2] - sig_u_hat_product;
}
}
sw5.Stop();
sw6.Reset();
sw6.Start();
//-------------------------------------------//
// ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
//-------------------------------------------//
for (int i = -rangeToApply; i <= rangeToApply; i++)
{
for (int j = -rangeToApply; j <= rangeToApply; j++)
{
int indexXToUpdate = laserPointIndexX + i;
int indexYToUpdate = laserPointIndexY + j;
// if the cell to update is out of range, continue
if (!psig_u_hat_square.CheckValidIdx(indexXToUpdate, indexYToUpdate))
{
//Console.WriteLine("Laser points out of the occupancy grid map");
continue;
}
pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApply, i + rangeToApply] + pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate));
pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate));
psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApply, i + rangeToApply] * sigUHatMatrix[j + rangeToApply, i + rangeToApply] + psig_u_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate));
uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
(pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate)));
normalisedPijSum.SetCellByIdx(indexXToUpdate, indexYToUpdate, pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdx(indexXToUpdate, indexYToUpdate));
double largeU = (pu_hat.GetCellByIdx(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
double largeSig = (psig_u_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdx(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate) - largeU * largeU;
uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeSig);
Index index = new Index(indexXToUpdate, indexYToUpdate);
if (!indicesDictionary.ContainsKey(index))
{
indicesDictionary.Add(index, indicesDictionary.Count);
}
/*
* if (indicesDictionary.ContainsKey(index))
* {
* int indexOfIndices = indicesDictionary[index];
* heightList[indexOfIndices] = (float)largeU;
* covList[indexOfIndices] = (float)largeSig;
* pijSumList[indexOfIndices] = (float)pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate);
* }
* else
* {
* indicesDictionary.Add(index, indicesDictionary.Count);
* indicesList.Add(new Index(indexXToUpdate, indexYToUpdate));
* heightList.Add((float)largeU);
* covList.Add((float)largeSig);
* pijSumList.Add((float)pij_sum.GetCellByIdx(indexXToUpdate, indexYToUpdate));
* }
*/
}
}
sw6.Stop();
} // end foreach
//Console.WriteLine("1: " + sw1.ElapsedMilliseconds +
// " 2: " + sw2.ElapsedMilliseconds +
// " 3: " + sw3.ElapsedMilliseconds +
// " 4: " + sw4.ElapsedMilliseconds +
// " 5: " + sw5.ElapsedMilliseconds +
// " 6: " + sw6.ElapsedMilliseconds +
// " TOTAL: " + (sw1.ElapsedMilliseconds + sw2.ElapsedMilliseconds + sw3.ElapsedMilliseconds + sw4.ElapsedMilliseconds + sw5.ElapsedMilliseconds + sw6.ElapsedMilliseconds).ToString());
} // end function
}
