public unsafe void SetCellsFast(Bitmap bmp, RobotPose pose, IOccupancyGrid2D og, int setDiameter)
{
BitmapData data = bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height),
ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
byte *imgPtr = (byte *)(data.Scan0);
byte red, green, blue;
int poseIdxX, poseIdxY;
og.GetIndicies(pose.x, pose.y, out poseIdxX, out poseIdxY);
imgPtr += ((poseIdxY - setDiameter / 2) * data.Stride) + (poseIdxX - setDiameter / 2) * 3;
for (int i = 0; i < setDiameter; i++)
{
for (int j = 0; j < setDiameter; j++)
{
blue = imgPtr[0];
green = imgPtr[1];
red = imgPtr[2];
occupancyMap[j, i] = (double)blue;
imgPtr += 3;
}
imgPtr += (data.Width - setDiameter) * 3;
}
bmp.UnlockBits(data);
}
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();
}
/// <summary>
/// Constructor of OccupancyGrid2Polygons class
/// </summary>
/// <param name="ocGridReceived"></param>
/// <param name="polygonThreshold"></param>
public OcGrid2Poly(IOccupancyGrid2D ocGridReceived, double polygonThreshold)
{
this.occupancyGrid = ocGridReceived;
this.polygonThreshold = polygonThreshold;
indexInPolygonDictionary = new Dictionary <Vector2, Polygon>(ocGridReceived.NumCellX * ocGridReceived.NumCellY);
polygonList = new List <Polygon>(100);
polygonList.Add(new Polygon());
allPointsInPolygons = new Dictionary <Vector2, int>(occupancyGrid.NumCellX * occupancyGrid.NumCellY);
}
/// <summary>
/// Constructor of OccupancyGrid2Polygons class
/// </summary>
/// <param name="ocGridReceived"></param>
/// <param name="polygonThreshold"></param>
public OcGrid2Poly(IOccupancyGrid2D ocGridReceived, double polygonThreshold)
{
this.occupancyGrid = ocGridReceived;
this.polygonThreshold = polygonThreshold;
indexInPolygonDictionary = new Dictionary<Vector2, Polygon>(ocGridReceived.NumCellX * ocGridReceived.NumCellY);
polygonList = new List<Polygon>(100);
polygonList.Add(new Polygon());
allPointsInPolygons = new Dictionary<Vector2, int>(occupancyGrid.NumCellX * occupancyGrid.NumCellY);
}
public Poly2OcGrid(double resX, double resY, double extentX, double extentY)
{
this.resX = resX;
this.resY = resY;
this.extentX = extentX;
this.extentY = extentY;
og = new OccupancyGrid2D(resolutionX, resolutionY, extentX, extentY);
obstacles = new List<Polygon>();
}
public Poly2OcGrid(double resX, double resY, double extentX, double extentY)
{
this.resX = resX;
this.resY = resY;
this.extentX = extentX;
this.extentY = extentY;
og = new OccupancyGrid2D(resolutionX, resolutionY, extentX, extentY);
obstacles = new List <Polygon>();
}
public OccupancyGridRenderer(string name, IOccupancyGrid2D grid, float lowHeight, float highHeight, bool logOdds, bool newRenderingMethod, float sample)
{
this.newRenderingMethod = newRenderingMethod;
this.logOdds = logOdds;
this.name = name;
this.lowHeight = lowHeight;
this.highHeight = highHeight;
this.grid = grid;
this.sample = sample;
}
public OccupancyGridRenderer(string name, IOccupancyGrid2D grid, float lowHeight, float highHeight, bool logOdds, bool newRenderingMethod, float sample)
{
this.newRenderingMethod = newRenderingMethod;
this.logOdds = logOdds;
this.name = name;
this.lowHeight = lowHeight;
this.highHeight = highHeight;
this.grid = grid;
this.sample = sample;
}
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>();
}
public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
: this(ocToUpdate, laserRelativePositionToRover, 10)
{
}
public void UpdateOccupancyGrid(IOccupancyGrid2D ocgrid)
{
grid = ocgrid;
}
//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();
}
public PDFMessage(IOccupancyGrid2D pdf)
{
this.pdf = pdf;
}
public OccupancyGrid2DMessage(int robotID, IOccupancyGrid2D occupancyGrid2D)
{
this.robotID = robotID;
this.grid = occupancyGrid2D;
}
public OccupancyGridRenderer(string name, IOccupancyGrid2D grid, float lowHeight, float highHeight, bool logOdds)
: this(name, grid, lowHeight, highHeight, logOdds, false, 2.0f)
{
}
public void UpdateOccupancyGrid(IOccupancyGrid2D ocgrid)
{
grid = ocgrid;
}
public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
: this(ocToUpdate, laserRelativePositionToRover, 10)
{
}
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>();
}
public PDFMessage(IOccupancyGrid2D pdf)
{
this.pdf = pdf;
}
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));
}
}
public unsafe void SetCellsFast(Bitmap bmp, RobotPose pose, IOccupancyGrid2D og, int setDiameter)
{
BitmapData data = bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height),
ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
byte* imgPtr = (byte*)(data.Scan0);
byte red, green, blue;
int poseIdxX, poseIdxY;
og.GetIndicies(pose.x, pose.y, out poseIdxX, out poseIdxY);
imgPtr += ((poseIdxY - setDiameter / 2) * data.Stride) + (poseIdxX - setDiameter / 2) * 3;
for (int i = 0; i < setDiameter; i++)
{
for (int j = 0; j < setDiameter; j++)
{
blue = imgPtr[0];
green = imgPtr[1];
red = imgPtr[2];
occupancyMap[j, i] = (double)blue;
imgPtr += 3;
}
imgPtr += (data.Width - setDiameter) * 3;
}
bmp.UnlockBits(data);
}
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 OccupancyGridRenderer(string name, IOccupancyGrid2D grid, float lowHeight, float highHeight, bool logOdds)
: this(name, grid, lowHeight, highHeight, logOdds, false,2.0f)
{
}
//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();
}
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 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));
}
}
}
}
public HeightMapRenderer(IOccupancyGrid2D ocg)
{
grid = ocg;
}
/// <summary>
/// Update OccupancyGrid
/// </summary>
/// <param name="occupancyGrid">OccupancyGrid to be assigned</param>
public void UpdateOccupancyGrid(IOccupancyGrid2D occupancyGrid)
{
this.occupancyGrid = (IOccupancyGrid2D)occupancyGrid.DeepCopy();
}
/// <summary>
/// Update OccupancyGrid
/// </summary>
/// <param name="occupancyGrid">OccupancyGrid to be assigned</param>
public void UpdateOccupancyGrid(IOccupancyGrid2D occupancyGrid)
{
this.occupancyGrid = (IOccupancyGrid2D)occupancyGrid.DeepCopy();
}
public HeightMapRenderer(IOccupancyGrid2D ocg)
{
grid = ocg;
}
public OccupancyGrid2DMessage(int robotID, IOccupancyGrid2D occupancyGrid2D)
{
this.robotID = robotID;
this.grid = occupancyGrid2D;
}
public void UpdateOG(IOccupancyGrid2D grid)
{
this.grid = grid;
}
public NewOccupancyGrid2DAvailableEventArgs(IOccupancyGrid2D occupancyGrid, double timestamp)
{
this.occupancyGrid = occupancyGrid;
this.timestamp = timestamp;
}
public void UpdateOG(IOccupancyGrid2D grid)
{
this.grid = grid;
}
public NewOccupancyGrid2DAvailableEventArgs(IOccupancyGrid2D occupancyGrid, double timestamp)
{
this.occupancyGrid = occupancyGrid;
this.timestamp = timestamp;
}
