public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.state = state;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.state = state;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public void Enqueue(PoseFilterState state)
{
lock (this)
{
if (filterQueue.Count >= queueLength)
filterQueue.Dequeue();
filterQueue.Enqueue(state);
poseInterpolator.Add(state);
}
}
public void Enqueue(PoseFilterState state)
{
lock (this)
{
if (filterQueue.Count >= queueLength)
{
filterQueue.Dequeue();
}
filterQueue.Enqueue(state);
poseInterpolator.Add(state);
}
}
public void Add(PoseFilterState state)
{
lock (this)
{
//if the buffer fills up knock of the last one
if (stateBuffer.Count >= bufferSize)
{
stateBuffer.RemoveAt(0);
oldestTime = stateBuffer[0].timestamp;
}
if (stateBuffer.Count == 0)
{
stateBuffer.Add(state);
oldestTime = stateBuffer[0].timestamp;
newestTime = stateBuffer[0].timestamp;
}
else
{
int index = 0;
for (int i = stateBuffer.Count - 1; i >= 0; i--)
{
if (state.timestamp > stateBuffer[i].timestamp)
{
index = i + 1;
break;
}
}
if (index == stateBuffer.Count)
newestTime = state.timestamp;
if (index == 0)
oldestTime = state.timestamp;
stateBuffer.Insert(index, state);
}
}
}
public NewStateAvailableEventArgs(PoseFilterState state)
{
this.state = state;
}
public FilterStateMessage(int robotID, PoseFilterState state)
{
this.robotID = robotID; this.state = state;
}
/// <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 FilterStateMessage(int robotID, PoseFilterState state)
{
this.robotID = robotID; this.state = state;
}
public NewStateAvailableEventArgs(PoseFilterState state)
{
this.state = state;
}
/// <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 PoseFilterState PoseAtTime(double timestamp)
{
if (timestamp < oldestTime || timestamp > newestTime)
{
throw new ArgumentOutOfRangeException("Requested time is outside range of buffer");
}
PoseFilterState q0;
PoseFilterState q1;
//Find the two pose states whose time stamps lie on either side of the desired time
lock (this)
{
PoseFilterState tempState = new PoseFilterState(0, 0, 0, 0, 0, 0, timestamp);
int index = stateBuffer.BinarySearch(tempState);
if (index < 0)
{
index = ~index;
q0 = stateBuffer[index - 1];
q1 = stateBuffer[index];
}
else
{
return stateBuffer[index];
}
}
return QuaternionInterpolant(q0, q1, timestamp);
}
private PoseFilterState QuaternionInterpolant(PoseFilterState q0, PoseFilterState q1, double timestamp)
{
double a1;
double a2;
double u = (timestamp - q0.timestamp) / (q1.timestamp - q0.timestamp);
double q01 = q0.q1; double q02 = q0.q2; double q03 = q0.q3; double q04 = q0.q4;
double q11 = q1.q1; double q12 = q1.q2; double q13 = q1.q3; double q14 = q1.q4;
//double qSquareSum = q01 * q01 + q02 * q02 + q03 * q03 + q04 * q04;
//double qp1 = (-1 * q01 * q14 - q02 * q13 + q03 * q12 + q04 * q11) / qSquareSum;
//double qp2 = (q01 * q13 - q02 * q14 - q03 * q11 + q04 * q12) / qSquareSum;
//double qp3 = (-1 * q01 * q12 + q02 * q11 - q03 * q14 + q04 * q13) / qSquareSum;
//double qp4 = (q01 * q11 + q02 * q12 + q03 * q13 + q04 * q14) / qSquareSum;
//double omega = Math.Acos(qp4);
//qp1 = qp1 * Math.Sin(u * omega);
//qp2 = qp2 * Math.Sin(u * omega);
//qp3 = qp3 * Math.Sin(u * omega);
//qp4 = Math.Cos(u * omega);
//double qu1 = (-1 * q01 * qp4 - q02 * qp3 + q03 * qp2 + q04 * qp1);
//double qu2 = (q01 * qp3 - q02 * qp4 - q03 * qp1 + q04 * qp2);
//double qu3 = (-1 * q01 * qp2 + q02 * qp1 - q03 * qp4 + q04 * qp3);
//double qu4 = (q01 * qp1 + q02 * qp2 + q03 * qp3 + q04 * qp4);
double theta = Math.Acos(q01 * q11 + q02 * q12 + q03 * q13 + q04 * q14);
//Correct for possible loss of numerical precision on q1,q2
if (Double.IsNaN(theta) || theta == 0)
{
theta = 0;
a1 = 1-u;
a2 = u;
}
else
{
a1 = Math.Sin((1 - u) * theta) / Math.Sin(theta);
a2 = Math.Sin(u * theta) / Math.Sin(theta);
}
double qu1 = a1 * q01 + a2 * q11;
double qu2 = a1 * q02 + a2 * q12;
double qu3 = a1 * q03 + a2 * q13;
double qu4 = a1 * q04 + a2 * q14;
if (Double.IsNaN(qu1) || Double.IsNaN(qu2) || Double.IsNaN(qu3) || Double.IsNaN(qu4))
throw new ArithmeticException("Bad");
if (Math.Abs(qu1) > 1 || Math.Abs(qu2) > 1 || Math.Abs(qu3) > 1 || Math.Abs(qu4) > 1)
throw new ArithmeticException("Bigger than 1");
double xu = u * q1.x + (1 - u) * q0.x;
double yu = u * q1.y + (1 - u) * q0.y;
double zu = u * q1.z + (1 - u) * q0.z;
Matrix P = q0.Covariance * (1 - u) + q1.Covariance * u;
PoseFilterState poseToReturn = new PoseFilterState(xu, yu, zu, qu1, qu2, qu3, qu4, timestamp, P);
poseToReturn.vx = u * q1.vx + (1 - u) * q0.vx;
poseToReturn.vy = u * q1.vy + (1 - u) * q0.vy;
poseToReturn.vz = u * q1.vz + (1 - u) * q0.vz;
poseToReturn.wx = u * q1.wx + (1 - u) * q0.wx;
poseToReturn.wy = u * q1.wy + (1 - u) * q0.wy;
poseToReturn.wz = u * q1.wz + (1 - u) * q0.wz;
return poseToReturn;
}
