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