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);
}
}
}
}
public object DeepCopy()
{
OccupancyGrid2D copy = new OccupancyGrid2D(this.resolutionX, this.resolutionY, this.extentX, this.extentY);
//Copy the occupancyMap
Array.Copy(this.occupancyMap, copy.occupancyMap, this.occupancyMap.Length);
return(copy);
}
public string ToLog()
{
string toReturn = "";
OccupancyGrid2D copy = new OccupancyGrid2D(this);
for (int i = 0; i < NumCellY; i++)
{
for (int j = 0; j < NumCellX; j++)
{
toReturn += this.GetCellByIdx(j, i).ToString() + " ";
}
toReturn += "\n";
}
return(toReturn);
}
public GlobalGaussianMixMap(OccupancyGrid2D globalOcGrid)
{
addressProvider = new HardcodedAddressProvider();
//robotPoseClient = new GenericMulticastClient<RobotPoseMessage>(addressProvider.GetAddressByName("RobotPose"), new CSharpMulticastSerializer<RobotPoseMessage>(true));
//lidarScanClient = new GenericMulticastClient<LidarScanMessage>(addressProvider.GetAddressByName("LidarScan"), new CSharpMulticastSerializer<LidarScanMessage>(true));
//sensorPoseClient = new GenericMulticastClient<SensorPoseMessage>(addressProvider.GetAddressByName("SensorPose"), new CSharpMulticastSerializer<SensorPoseMessage>(true));
lidarPosePackageClient = new GenericMulticastClient<LidarPosePackageMessage>(addressProvider.GetAddressByName("LidarPosePackage"), new CSharpMulticastSerializer<LidarPosePackageMessage>(true));
simLidarPosePackageClient = new GenericMulticastClient<SimMessage<LidarPosePackageMessage>>(addressProvider.GetAddressByName("SimLidarPosePackage"), new CSharpMulticastSerializer<SimMessage<LidarPosePackageMessage>>(true));
globalGaussianMixMapServer = new GenericMulticastServer<GlobalMapCell>(addressProvider.GetAddressByName("GlobalGaussianMixMap"), new CSharpMulticastSerializer<GlobalMapCell>(true));
globalGaussianMixMapServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
robotIDToPose = new Dictionary<int, RobotPose>();
robotIDToScan = new Dictionary<int, ILidarScan<ILidar2DPoint>>();
robotIDToSensorPose = new Dictionary<int, SensorPose>();
robotIDToTimestamp = new Dictionary<int, double>();
robotIDToPastTimestamp = new Dictionary<int, double>();
globalOcGridByEachRobot = new Dictionary<int, OccupancyGrid2D>();
robotIDToDynamicObstacles = new Dictionary<int, List<Polygon>>();
this.globalOcGrid = new OccupancyGrid2D(globalOcGrid);
laserToRobot = new SensorPose(0, 0, 0.5, 0, 0 / 180.0, 0, 0);
gaussianMixMapAlgorithm = new GaussianMixMapping(globalOcGrid, laserToRobot);
globalOcGridByEachRobotAlgorithm = new Dictionary<int, GaussianMixMapping>();
lidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
simLidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
lidarPosePackageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LidarPosePackageMessage>>(lidarPosePackageClient_MsgReceived);
simLidarPosePackageClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<SimMessage<LidarPosePackageMessage>>>(simLidarPosePackageClient_MsgReceived);
// local map request from robots
localMapRequestClient = new GenericMulticastClient<LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer<LocalMapRequestMessage>(true));
localMapRequestClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
localMapRequestClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<LocalMapRequestMessage>>(localMapRequestClient_MsgReceived);
// local map update sender
localMapReponseServer = new GenericMulticastServer<UpdateMapDataMessage>(addressProvider.GetAddressByName("LocalMapResponse"), new CSharpMulticastSerializer<UpdateMapDataMessage>(true));
localMapReponseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
Thread t = new Thread(new ParameterizedThreadStart(UpdateGlobalMap));
t.Start();
//Thread t2 = new Thread(new ParameterizedThreadStart(SendGlobalUpdate));
//t2.Start();
}
public static OccupancyGrid2D operator +(OccupancyGrid2D a, OccupancyGrid2D b)
{
OccupancyGrid2D toReturn = new OccupancyGrid2D(a.ResolutionX, a.resolutionY, a.extentX, a.ExtentY);
if (!(a.ExtentX == b.ExtentX && a.ExtentY == b.ExtentY && a.ResolutionX == b.ResolutionX && a.ResolutionY == b.ResolutionY))
{
throw new Exception("OccupancyGrid size does not match");
}
else
{
for (int i = 0; i < a.Width; i++)
{
for (int j = 0; j < a.Height; j++)
{
toReturn.SetCellByIdx(i, j, a.GetCellByIdx(i, j) + b.GetCellByIdx(i, j));
}
}
}
return(toReturn);
}
//void localMapUpdateClient_MsgReceived(object sender, MsgReceivedEventArgs<UpdateMapDataMessage> e)
//{
// lock (locker)
// {
// //UpdateCurrentLocalMap(ref heightMap, ref covMap, ref pijMap, e.message);
// UpdateCurrentLocalMap(ref heightMap, e.message);
// }
//}
void UpdateCurrentLocalMap(ref OccupancyGrid2D currentLocalHeightMap, ref OccupancyGrid2D currentLocalCovMap,
ref OccupancyGrid2D currentLocalPijMap, UpdateMapDataMessage updateMessage)
{
try
{
//check if the size matches
foreach (UpdateMapDataMessage.UpdateMapDataCell cell in updateMessage.CellData)
{
if (cell.X == cell.Y && cell.Y == 0) continue; // the first packet
double x = cell.X; double y = cell.Y;
//currentLocalHeightMap.SetCell(x, y, cell.Height);
//currentLocalCovMap.SetCell(x, y, cell.Cov);
//currentLocalPijMap.SetCell(x, y, cell.Pij);
}
}
catch
{
throw new IndexOutOfRangeException("The update message and current map are not in the same size");
}
}
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;
}
public static OccupancyGrid2D operator +(OccupancyGrid2D a, OccupancyGrid2D b)
{
OccupancyGrid2D toReturn = new OccupancyGrid2D(a.ResolutionX, a.resolutionY, a.extentX, a.ExtentY);
if (!(a.ExtentX == b.ExtentX && a.ExtentY == b.ExtentY && a.ResolutionX == b.ResolutionX && a.ResolutionY == b.ResolutionY))
{
throw new Exception("OccupancyGrid size does not match");
}
else
{
for (int i = 0; i < a.Width; i++)
{
for (int j = 0; j < a.Height; j++)
{
toReturn.SetCellByIdx(i, j, a.GetCellByIdx(i, j) + b.GetCellByIdx(i, j));
}
}
}
return toReturn;
}
public object DeepCopy()
{
OccupancyGrid2D copy = new OccupancyGrid2D(this.resolutionX, this.resolutionY, this.extentX, this.extentY);
//Copy the occupancyMap
Array.Copy(this.occupancyMap, copy.occupancyMap, this.occupancyMap.Length);
return copy;
}
/// <summary>
/// this is a copy constructor. You can use deepcopy, but this is better.
/// </summary>
/// <param name="gridToCopy"></param>
public OccupancyGrid2D(OccupancyGrid2D gridToCopy)
: this(gridToCopy.resolutionX, gridToCopy.resolutionY, gridToCopy.extentX, gridToCopy.extentY)
{
//Copy the occupancyMap
Array.Copy(gridToCopy.occupancyMap, this.occupancyMap, gridToCopy.occupancyMap.Length);
}
/// <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);
}
/// <summary>
/// Return only height map difference
/// </summary>
/// <param name="robotID">Robot ID</param>
/// <param name="globalMulti">global Map to compare</param>
/// <param name="currentPose">robot's current position</param>
/// <param name="extentX">extent x</param>
/// <param name="extentY">extent y</param>
/// <returns>UpdateMapDataMessage with only height information</returns>
//public UpdateMapDataMessage HeightDiff(int robotID, OccupancyGrid2D globalMulti, RobotPose currentPose, double extentX, double extentY)
//{
// if (!globalOcGridByEachRobotAlgorithm.ContainsKey(robotID)) return null;
// OccupancyGrid2D globalSingle = globalOcGridByEachRobotAlgorithm[robotID].UhatGM;
// //List<Index> diffIndexToSend = new List<Index>();
// List<Position> diffPositionToSend = new List<Position>();
// List<float> heightList = 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 (comparisonCellX < 0 || comparisonCellX > globalSingle.NumCellX || comparisonCellY < 0 || comparisonCellY > globalSingle.NumCellY)
// continue;
// 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(comparisonCellX, comparisonCellY));
// }
// }
// }
// return new UpdateMapDataMessage(robotID, diffPositionToSend, heightList);
//}
/// <summary>
/// Converts GlobalMapCell data into a occupancy grid for global map
/// </summary>
/// <param name="globalHeightMap">returned global height map</param>
/// <param name="globalSigMap">returned global covariance map</param>
/// <param name="globalPijMap">returned laser hitting (kinda) map</param>
/// <param name="globalMapDataList">incoming message from network</param>
public static void ConvertToOcGrid(ref OccupancyGrid2D globalHeightMap, ref OccupancyGrid2D globalSigMap, ref OccupancyGrid2D globalPijMap, ref OccupancyGrid2D laserHitMap, List<GlobalMapCell.GlobalMapCellData> globalMapDataList)
{
foreach (GlobalMapCell.GlobalMapCellData cell in globalMapDataList)
{
if (cell.Equals(globalMapDataList[0]))
continue;
int col = cell.colIdx; int row = cell.rowIdx;
globalHeightMap.SetCellByIdx(col, row, cell.largeU);
globalSigMap.SetCellByIdx(col, row, cell.largeSig);
globalPijMap.SetCellByIdx(col, row, cell.pij);
laserHitMap.SetCellByIdx(col, row, cell.laserHit);
}
}
public void UpdateOG(OccupancyGrid2D og)
{
lock (ogLock)
{
this.og = og;
}
}
public void UpdateObstacles(List<Polygon> obstacles, double r, out List<Polygon> bloated)
{
if (pose == null)
{
bloated = default(List<Polygon>);
return;
}
lock (bloatedObstacleLock)
{
bloatedObstacles = new List<Polygon>();
foreach (Polygon p in obstacles)
bloatedObstacles.Add(Polygon.ConvexMinkowskiConvolution(Polygon.VehiclePolygonWithRadius(r), Polygon.GrahamScan(p.points, 1e-3)));
}
bloated = bloatedObstacles;
// Following is deprecated for new height map planning
Bitmap bmp = new Bitmap((int)Math.Round(2 * extentX / resX), (int)Math.Round(2 * extentY / resY));
Graphics canvas = Graphics.FromImage(bmp);
Pen pen = new Pen(Color.Blue, 1);
PointF[] points;
lock (bloatedObstacleLock)
{
foreach (Polygon p in bloatedObstacles)
{
points = new PointF[p.points.Count];
for (int i = 0; i < p.points.Count; i++)
{
points[i] = new PointF((float)(p.points.ElementAt(i).X / resX + extentX / resX),
(float)(p.points.ElementAt(i).Y / resY + extentY / resY));
}
canvas.DrawPolygon(pen, points);
}
}
bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
//bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
//bmp = Blur(bmp, pose, (int)(10 / resX) * 2);
//bmp = Blur(bmp);
//bmp = Blur(bmp);
lock (ogLock)
{
og = new OccupancyGrid2D(resX, resY, extentX, extentY);
og.SetCellsFast(bmp);//, pose, og, 10);
}
}
public GaussianMixMapping(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
{
//currentLaserPoseToRobot = laserRelativePositionToRover;
Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
laserToRobotDCM = new UMatrix(4, 4);
robotToGlocalDCM = new UMatrix(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
covRobotPose = new UMatrix(6, 6);
covLaserPose = new UMatrix(6, 6);
covLaserScan = new UMatrix(2, 2);
pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
normalisedPijSum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
#region Initial setup
// initial setup
// these matrices were obtained from Jon using Vicon with Pioneer and HOKUYO laser
// these matrices need to be redefined
// these matrices are in row-wise
//double[] Qp_robot = new double[36]{0.0026, 0.0011, -0.0008, -0.0019, 0.0125, -0.0047,
// 0.0011, 0.0082, -0.0054, 0.0098, 0.0116, -0.0330,
// -0.0008, -0.0054, 0.0132, -0.0173, -0.0154, 0.0115,
// -0.0019, 0.0098, -0.0173, 0.0542, 0.0076, -0.0319,
// 0.0125, 0.0116, -0.0154, 0.0076, 0.0812, -0.0397,
// -0.0047, -0.0330, 0.0115, -0.0319, -0.0397, 0.1875};
//double[] Qp_laser = new double[36]{0.0077, -0.0009, -0.0016, -0.0127, -0.0415, -0.0011,
// -0.0009, 0.0051, -0.0013, -0.0035, 0.0045, 0.0197,
// -0.0016, -0.0013, 0.0101, -0.0167, 0.0189, 0.0322,
// -0.0127, -0.0035, -0.0167, 0.2669, -0.0548, -0.2940,
// -0.0415, 0.0045, 0.0189, -0.0548, 0.8129, 0.3627,
// -0.0011, 0.0197, 0.0322, -0.2940, 0.3627, 0.7474};
double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0,
0, 0, 0.001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001};
double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };
// assign to our UMatrix
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
}
}
covLaserScan[0, 0] = Qr[0];
covLaserScan[0, 1] = 0;
covLaserScan[1, 0] = 0;
covLaserScan[1, 1] = Qr[3];
#endregion
//JTpl = ComputeJacobian(currentLaserPoseToRobot.yaw, currentLaserPoseToRobot.pitch, currentLaserPoseToRobot.roll);
// int thetaDegIndex = (int)((p.RThetaPoint.theta * 180.0/Math.PI) * 2) + 180;
int k = 0;
for (double i = -90; i <= 90; i += .5)
{
sinLookup[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookup[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
// initialize arrays
//pijToSend = new float[361 * rangeLong * rangeLong];
//largeUToSend = new float[361 * rangeLong * rangeLong];
//largeSigToSend = new float[361 * rangeLong * rangeLong];
//colIdxChange = new int[361 * rangeLong * rangeLong];
//rowIdxChange = new int[361 * rangeLong * rangeLong];
indicesList = new List<Index>();
heightList = new List<float>();
covList = new List<float>();
pijSumList = new List<float>();
laser2RobotTransMatrixDictionary = new Dictionary<int, UMatrix>();
jacobianLaserPoseDictionary = new Dictionary<int, UMatrix>();
indicesDictionary = new Dictionary<Index, int>();
}
public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover, int cellToUpdate)
{
//currentLaserPoseToRobot = laserRelativePositionToRover;
this.rangeToApply = cellToUpdate;
Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
laserToRobotDCM = new UMatrix(4, 4);
robotToGlocalDCM = new UMatrix(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
covRobotPose = new UMatrix(6, 6);
covLaserPose = new UMatrix(6, 6);
covLaserScan = new UMatrix(2, 2);
pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
thresholdedHeightMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
resetCountMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
indexMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
this.numCellX = ocToUpdate.NumCellX;
this.numCellY = ocToUpdate.NumCellY;
#region Initial setup
double[] Qp_robot = new double[36]{0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
double[] Qp_laser = new double[36]{0.001, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0,
0, 0, 0.001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001};
double[] Qr = new double[4] { .01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0) };
// assign to our UMatrix
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
covRobotPose[j, i] = Qp_robot[j * 6 + i];// *1e-6;
covLaserPose[j, i] = Qp_laser[j * 6 + i];// *1e-6;
}
}
covLaserScan[0, 0] = Qr[0];
covLaserScan[0, 1] = 0;
covLaserScan[1, 0] = 0;
covLaserScan[1, 1] = Qr[3];
#endregion
int k = 0;
sinLookupHokuyo = new double[682];
cosLookupHokuyo = new double[682];
for (double i = -120; i <= 120; i += 0.35190615835777126099706744868035)
{
sinLookupHokuyo[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookupHokuyo[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
laserHalfAngleHokuyo = 120;
MAXRANGEHokuyo = 5.0;
MINRANGEHokuyo = 0.5;
k = 0;
sinLookupSick = new double[361];
cosLookupSick = new double[361];
for (double i = -90; i <= 90; i += .5)
{
sinLookupSick[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookupSick[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
laserHalfAngleSick = 90;
MAXRANGESick = 30.0;
MINRANGESick = 0.5;
hokuyoStartIdx = 100;
hokuyoEndIdx = 500;
indicesList = new List<Index>();
heightList = new List<float>();
covList = new List<float>();
pijSumList = new List<float>();
laser2RobotTransMatrixDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();
jacobianLaserPoseDictionary = new Dictionary<int, Dictionary<int, UMatrix>>();
indicesDictionary = new Dictionary<Index, int>();
}
/// <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> laserHit)
{
int index = 0;
if (indicesDictionary == null)
{
indexList = new List<Index>();
heightList = new List<float>();
covList = new List<float>();
pijSumList = new List<float>();
laserHit = 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);
laserHit = new List<float>(indicesDictionary.Count);
}
lock (locker)
{
foreach (KeyValuePair<Index, int> pair in indicesDictionary)
{
indexList.Add(pair.Key);
heightList.Add((float)uhatGM.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
//heightList.Add((float)thresholdedHeightMap.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
covList.Add((float)sigSqrGM.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
pijSumList.Add((float)pij_sum.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
laserHit.Add((float)LaserHit.GetCellByIdxUnsafe(pair.Key.Col, pair.Key.Row));
indexMap.SetCellByIdx(pair.Key.Col, pair.Key.Row, 0.0); // reset the index map
}
indicesDictionary.Clear();
}
}
/// <summary>
/// this is a copy constructor. You can use deepcopy, but this is better.
/// </summary>
/// <param name="gridToCopy"></param>
public OccupancyGrid2D(OccupancyGrid2D gridToCopy)
: this(gridToCopy.resolutionX, gridToCopy.resolutionY, gridToCopy.extentX, gridToCopy.extentY)
{
//Copy the occupancyMap
Array.Copy(gridToCopy.occupancyMap, this.occupancyMap, gridToCopy.occupancyMap.Length);
}
public string ToLog()
{
string toReturn = "";
OccupancyGrid2D copy = new OccupancyGrid2D(this);
for (int i = 0; i < NumCellY; i++)
{
for (int j = 0; j < NumCellX; j++)
{
toReturn += this.GetCellByIdx(j, i).ToString() + " ";
}
toReturn += "\n";
}
return toReturn;
}