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; }