public void Make2DMatrixTest()
{
int vSize = 15;
Vector <float> V = Vector <float> .Build.Random(vSize);
int iter = 100000;
Matrix <float> M;
var watch = System.Diagnostics.Stopwatch.StartNew();
for (int i = 0; i < iter; i++)
{
M = UMatrix.Make2DMatrix(V, 5, 3);
}
watch.Stop();
var elapsedMs = watch.ElapsedMilliseconds;
Debug.Log("Time Col mode: " + elapsedMs);
watch = System.Diagnostics.Stopwatch.StartNew();
for (int i = 0; i < iter; i++)
{
M = UMatrix.Make2DMatrix(V, 5, 3, false);
}
watch.Stop();
elapsedMs = watch.ElapsedMilliseconds;
Debug.Log("Time Row mode: " + elapsedMs);
M = UMatrix.Make2DMatrix(V, 5, 3);
Debug.Log(V);
Debug.Log(M);
}
private void UpdateCovariance(Matrix robotCov)
{
lock (locker)
{
UMatrix abc = new UMatrix(6, 6);
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
abc[i, j] = robotCov[i, j]; // this is a heck !!!!!
}
if (abc[i, i] < 0)
{
abc[i, i] = Math.Abs(abc[i, i]);
}
}
if (abc[2, 2] == 0)
{
abc[2, 2] = 0.01;
}
if (abc[4, 4] == 0)
{
abc[4, 4] = 0.01;
}
if (abc[5, 5] == 0)
{
abc[5, 5] = 0.01;
}
covRobotPose = abc;
}
}
private void LoadTXTFormatGraph(string graphFile)
{
FileStream ifs = new FileStream(graphFile, FileMode.Open);
StreamReader sr = new StreamReader(ifs);
string line = "";
string[] tokens = null;
line = sr.ReadLine();
line = line.Trim();
while (line.StartsWith("c"))
{
line = sr.ReadLine();
line = line.Trim();
}
tokens = line.Split(' ');
int numVertices = int.Parse(tokens[1]); // Convert.ToInt32
int numEdges = int.Parse(tokens[2]);
if (numVertices < 0 || numEdges < 0)
{
throw new Exception("Number nodes or edges is a negative");
}
data = new VerticesList(numVertices);
residualNetwork = new UMatrix(numVertices);
while (!string.IsNullOrEmpty(line = sr.ReadLine()))
{
line = line.Trim();
tokens = line.Split(' ');
int incidentFromIndex = int.Parse(tokens[0]);
int incidentToIndex = int.Parse(tokens[1]);
ushort capacity = ushort.Parse(tokens[2]);
if (incidentFromIndex < 1 || incidentFromIndex > numVertices || incidentToIndex < 1 ||
incidentToIndex > numVertices)
{
throw new Exception("The vertex parameter is not in the range 0...this.numVertices");
}
data.SetValue(incidentFromIndex, incidentToIndex, capacity);
residualNetwork.SetValue(incidentFromIndex - 1, incidentToIndex - 1, capacity);
}
sr.Close();
ifs.Close();
NumberVertices = numVertices;
NumberEdges = numEdges;
}
private void UpdateCovarianceQ(Matrix state)
{
UMatrix abc = new UMatrix(7, 7);
for (int i = 0; i < 7; i++)
{
for (int j = 0; j < 7; j++)
{
abc[i, j] = state[i, j];
}
}
covRobotPoseQ = abc;
}
/*RaycastHit hit;
* Ray downRay;*/
void BuildCustomWeights(Vector <float> individual)
{
tmpBuildCustomWeights.Clear();
for (int i = 0; i < shapes.Count - 1; i++)
{
if (i == 0)
{
tmpBuildCustomWeights.Add(UMatrix.Make2DMatrix(individual.SubVector(0, (shapes[i] + 1) * shapes[i + 1]), shapes[i + 1], shapes[i] + 1));
}
else
{
tmpBuildCustomWeights.Add(UMatrix.Make2DMatrix(individual.SubVector((shapes[i - 1] + 1) * shapes[i], (shapes[i] + 1) * shapes[i + 1]), shapes[i + 1], shapes[i] + 1));
}
}
}
void BuildCustomWeights(List <Matrix <float> > newWeights, List <int> shapes, Vector <float> individual)
{
int bias = 1;
for (int i = 0; i < shapes.Count - 1; i++)
{
if (i == shapes.Count - 2)
{
bias = 0;
}
if (i == 0)
{
// Debug.Log(newWeights[i]);
// Debug.Log(individual.SubVector(0, (shapes[i] + 1) * (shapes[i + 1] + bias)));
UMatrix.Make2DMatrix(newWeights[i], individual.SubVector(0, (shapes[i] + 1) * (shapes[i + 1] + bias)), (shapes[i + 1] + bias), shapes[i] + 1);
}
else
{
UMatrix.Make2DMatrix(newWeights[i], individual.SubVector((shapes[i - 1] + 1) * shapes[i], (shapes[i] + 1) * (shapes[i + 1] + bias)), (shapes[i + 1] + bias), shapes[i] + 1);
}
}
}
/// <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, int scannerID, PoseFilterState currentRobotPose, SensorPose lidarPose, List <Polygon> dynamicObstacles)
{
if (robotID == 1)
{
MAXRANGESick = 7.0;
}
else if (robotID == 3)
{
MAXRANGESick = 30.0;
}
if (lidarPose == null)
{
lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
}
if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
{
if (laser2RobotTransMatrixDictionary[robotID].ContainsKey(scannerID))
{
JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID][scannerID];
}
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[robotID].Add(scannerID, laserToRobotDCM);
jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
}
}
else
{
laser2RobotTransMatrixDictionary.Add(robotID, new Dictionary <int, UMatrix>());
jacobianLaserPoseDictionary.Add(robotID, new Dictionary <int, UMatrix>());
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[robotID].Add(scannerID, new UMatrix(laserToRobotDCM));
jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
}
// calculate robot2global transformation matrix
if (currentRobotPose == null)
{
return;
}
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;
}
UMatrix JTpr = ComputeJacobianQ(currentRobotPose.q1, currentRobotPose.q2, currentRobotPose.q3, currentRobotPose.q4);
List <UMatrix> JfPrCubixLaserToRobotDCM = new List <UMatrix>(6);
List <UMatrix> RobotToGlocalDCMJfPlCubix = new List <UMatrix>(7);
for (int i = 0; i < 7; 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);
if (i == 7)
{
continue; // same as break
}
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
UpdateCovarianceQ(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 (scannerID == 1)
{
if (p.RThetaPoint.R >= MAXRANGESick || p.RThetaPoint.R <= MINRANGESick)
{
continue;
}
}
else if (scannerID == 2)
{
if (p.RThetaPoint.R >= MAXRANGEHokuyo || p.RThetaPoint.R <= MINRANGEHokuyo || laserIndex < hokuyoStartIdx || laserIndex > hokuyoEndIdx)
{
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 = 0;
if (scannerID == 2) // hokuyo
{
thetaDegIndex = (int)Math.Round((deg + laserHalfAngleHokuyo) * 2.84); // % 360;
}
else if (scannerID == 1) // sick
{
thetaDegIndex = (int)Math.Round((deg + laserHalfAngleSick) * 2) % 360;
}
double cosTheta = 0, sinTheta = 0;
if (scannerID == 1)
{
cosTheta = cosLookupSick[thetaDegIndex];
sinTheta = sinLookupSick[thetaDegIndex];
}
else if (scannerID == 2)
{
cosTheta = cosLookupHokuyo[thetaDegIndex];
sinTheta = sinLookupHokuyo[thetaDegIndex];
}
// initial laser points
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, 7);
UMatrix Jfpl = new UMatrix(3, 6);
for (int i = 0; i < 7; 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];
}
UMatrix JfprQprJfprT = new UMatrix(3, 3);
UMatrix JfplQplJfplT = new UMatrix(3, 3);
UMatrix JfrQrJfrT = new UMatrix(3, 3);
JfprQprJfprT = (Jfpr * covRobotPoseQ) * Jfpr.Transpose();
//JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose(); // not doing because covariance of laser point is so small
JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();
// add above variables together and get the covariance
UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
//-----------------------------//
// 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)
if ((laserPointIndexX < 0 || laserPointIndexX >= numCellX) || (laserPointIndexY < 0 || laserPointIndexY >= numCellY))
{
continue;
}
int rangeToApplyX = (int)Math.Round(Math.Sqrt(covRHatENU[0, 0]) / (pij_sum.ResolutionX * 2)) * 2;
int rangeToApplyY = (int)Math.Round(Math.Sqrt(covRHatENU[1, 1]) / (pij_sum.ResolutionY * 2)) * 2;
//-----------------------------------------//
// COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
//-----------------------------------------//
UMatrix pijCell = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
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++) // row
for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
{
//for (int j = -rangeToApplyX; j <= rangeToApplyX; j++) // column
for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
{
if (laserPointIndexX + i < 0 || laserPointIndexX + i >= numCellX || laserPointIndexY + j < 0 || laserPointIndexY + j >= numCellY)
{
continue;
}
// 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 + rangeToApplyY, i + rangeToApplyX] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdxUnsafe(laserPointIndexX + i, laserPointIndexY + j) + 1);
}
}
//---------------------------//
// COMPUTE HEIGHT ESTIMATION //
//---------------------------//
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(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
UMatrix sigUHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 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 = -rangeToApplyX; i <= rangeToApplyX; i++) // row
{
for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
{
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 + rangeToApplyY, i + rangeToApplyX] = r_hat_ENU[2, 0] + u_hat_product;
sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = covRHatENU[2, 2] - sig_u_hat_product;
}
}
//-------------------------------------------//
// ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
//-------------------------------------------//
for (int i = -rangeToApplyX; i <= rangeToApplyX; i++)
{
for (int j = -rangeToApplyY; j <= rangeToApplyY; 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))
{
continue;
}
pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] + pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] * sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
(pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)));
double largeU = (pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
double largeSig = (psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) - largeU * largeU;
if (pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) > 1)
{
thresholdedHeightMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU); //pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
}
uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
//sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU + 2 * Math.Sqrt(largeSig));
if (indexMap.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) != 1.0)
{
Index index = new Index(indexXToUpdate, indexYToUpdate);
indicesDictionary.Add(index, indicesDictionary.Count);
indexMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, 1.0);
}
}
}
} // 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
}
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 void Decompose(List<DataPoint> stream, List<DataPoint> main, List<DataPoint> wave, double epsilon, int maxDistance = int.MaxValue)
{
var sequel = new SequelMatrix(stream, S, R);
var u = new UMatrix(stream, S, R);
for (var i = 0; i < stream.Count; ++i)
{
var b = 0d;
if (i % 2 == 0 && i >= 2 * (S + 1) && i <= 2 * R)
{
b = - u[i, i - 2] * stream[i - 2].Y - u[i, i - 1] * stream[i - 1].Y + stream[i].Y;
}
if (i % 2 == 1 && i >= 2 * (S + 1) && i <= 2 * (R - 1))
{
b = u.Coeff(i) * wave[i - 1].Y;
}
wave.Add(new DataPoint(stream[i].X, b));
if (i > 2 * S && i < 2 * R && i % 2 == 1)
continue;
var a = 0d;
if (i <= 2 * S)
a = stream[i].Y;
if (i > 2 * S && i <= 2 * R)
{
var offset = (i - 2 * S - 2) / 2;
a = stream[2 * S + 2 * offset].Y * sequel[2 * S + 1 + offset, 2 * S + 2 * offset]
+ stream[2 * S + 1 + 2 * offset].Y * sequel[2 * S + 1 + offset, 2 * S + 1 + 2 * offset];
}
if (i > 2 * R)
a = stream[i].Y;
main.Add(new DataPoint(stream[i].X, a));
}
}
public RobotTwoWheelCommand GetCommand()
{
double g = 3;
double zeta = .7; //parameters for STC
double dt = .4; // adjust to change speed
double deltadist = .10; //incremental distance for looking ahead
// deltadist/dt = nominal speed along path
//get trajectory // this step is performed outside of this function by BehavioralDirector
//discretize trajectory into [xr, yr]
//generate reference velocities at each trajectory point [vr, wr, thetar] if new path
if (!pathsame)
{
this.GenrefVWHuniformdt(pathCurrentlyTracked, deltadist, dt);
}
//use current timestamp to find corresponding index in reference vectors
//double currenttime = TimeSync.CurrentTime;
double currenttime = (DateTime.Now.Ticks - 621355968000000000) / 10000000.0;
int indextotrack = (int)(Math.Ceiling((currenttime - timestamps[0]) / dt));
if (indextotrack >= timestamps.Length)
{
indextotrack = timestamps.Length - 1;
}
Console.WriteLine("indextotrack = {0}", indextotrack);
Console.WriteLine("time = {0}", currenttime);
//unwrap thetar against thetarold, ignore if null (1337)
if (!endpointsame)
{
thetarold = 1337;
}
if (thetarold != 1337)
{
while (hr[indextotrack] - thetarold >= Math.PI)
{
hr[indextotrack] -= 2 * Math.PI;;
}
while (hr[indextotrack] - thetarold <= -Math.PI)
{
hr[indextotrack] += 2 * Math.PI;
}
}
//set thetarold to thetar
thetarold = hr[indextotrack];
double[] qrefdata = { xr[indextotrack], yr[indextotrack], hr[indextotrack] };
UMatrix qref = new UMatrix(3, 1, qrefdata);
//print to qref to screen // use only for debugging
//string qrefstring = qref.ToString();
//Console.WriteLine("QREF\n {0}", qrefstring);
//establish pose vector q = [x, y, theta];
double x, y, theta;
x = this.currentPoint.x;
y = this.currentPoint.y;
theta = this.currentPoint.yaw; // in radians
double[] qdata = { x, y, theta };
UMatrix q = new UMatrix(3, 1, qdata);
//unwrap pose against qref
while (q[2, 0] - qref[2, 0] >= Math.PI)
{
q[2, 0] -= 2 * Math.PI;;
}
while (q[2, 0] - qref[2, 0] <= -Math.PI)
{
q[2, 0] += 2 * Math.PI;
}
//establish kinematic state error
UMatrix delq = new UMatrix(3, 1);
delq = qref - q;
//if (!endpointsame) { delqthetarold = 1337; }
//if (delqthetarold != 1337)
//{
// while (delq[2,0] - delqthetarold >= Math.PI)
// {
// delq[2,0] -= 2 * Math.PI; ;
// }
// while (delq[2,0] - delqthetarold <= -Math.PI)
// {
// delq[2,0] += 2 * Math.PI;
// }
//}
////set thetarold to thetar
//delqthetarold = delq[2,0];
//print delq to screen // use only for debugging
string delqstring = delq.ToString();
//Console.WriteLine("ERROR\n {0}", delqstring);
//Console.ReadLine();
//create rotation matrix
double[] Rotdata = { Math.Cos(theta), Math.Sin(theta), 0, -Math.Sin(theta), Math.Cos(theta), 0, 0, 0, 1 };
UMatrix Rot = new UMatrix(3, 3, Rotdata);
//convert to robot frame
UMatrix e = new UMatrix(3, 1);
e = Rot * delq; //error in the robot reference frame: first element is along track error, second element is cross track error, last element is angle error
string estring = e.ToString();
Console.WriteLine("ERROR\n {0}", estring);
// calculate Uf = [vf, wf]
double vf = vr[indextotrack] * Math.Cos(e[2, 0]); // in meters/sec
double wf = wr[indextotrack] * 180 / Math.PI; // in degrees/sec
Console.WriteLine("vf = {0}, wf = {1}", vf, wf);
//calculate wn = sqrt(wr^2 + g*vr^2)
double wn = Math.Sqrt(Math.Pow(wr[indextotrack], 2) + g * (Math.Pow(vr[indextotrack], 2)));
//calculte STC gain matrix
double k1, k2, k3;
k1 = 2 * zeta * wn;
k3 = k1;
k2 = g * vr[indextotrack];
double[] Kdata = { k1, 0, 0, 0, k2, k3 };
UMatrix K = new UMatrix(2, 3, Kdata);
//find control values Ub = K*e
UMatrix Ub = new UMatrix(2, 1);
Ub = K * e;
Console.WriteLine("vb = {0}, wb = {1}", Ub[0, 0], Ub[1, 0]);
//find total control u = Uf + Ub
double vcommand = (Ub[0, 0] + vf) * 3.6509; // multiply by 3.6509 to get "segway units"
double wcommand = (Ub[1, 0] * 180 / Math.PI + wf) * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec)
RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand);
if (Math.Abs(wcommand) >= 600)
{
Console.WriteLine("W saturation");
}
//create text file of information
if (indextotrack < timestamps.Length && counter < 15)
{
if (indextotrack == timestamps.Length - 1)
{
counter = counter + 1;
}
tw2.WriteLine("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} ", indextotrack, timestamps[indextotrack], x, y, e[0, 0], e[1, 0], e[2, 0], vf, wf, Ub[0, 0], Ub[1, 0]);
}
else
{
tw2.Close();
}
return(command);
}
/// <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
}
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>();
}
