private void BuildHistogram() { if (scan.Points.Count != 361) { Console.WriteLine("Scan count incorrect. It is " + scan.Points.Count); return; } for (int i = 0; i < 361; i++) { histogram[i] = 0; } for (int i = 0; i < 361; i++) { ILidar2DPoint p = scan.Points.ElementAt(i); if (i - 2 >= 0) { histogram[i - 2] += 0.25 * 80 / p.RThetaPoint.R; } if (i - 1 >= 0) { histogram[i - 1] += 0.5 * 80 / p.RThetaPoint.R; } histogram[i] += 80 / p.RThetaPoint.R; if (i + 1 < 361) { histogram[i + 1] += 0.5 * 80 / p.RThetaPoint.R; } if (i + 2 < 361) { histogram[i + 2] += 0.5 * 80 / p.RThetaPoint.R; } } Console.Clear(); for (int i = 0; i < 361; i += 4) { Console.Write((int)histogram[i] + " "); } Console.WriteLine(""); }
public void Draw(Renderer r) { if (options.Show == false) { return; } if (scan == null) { return; //nothing to do } GLUtility.DisableNiceLines(); GLUtility.GoToSensorPose(laserToBody); lock (this.drawLock) { //foreach (ILidar2DPoint sp in scan.Points) for (int i = startIdx; i <= endIdx; i++) { PointF p = PointF.Empty; ILidar2DPoint sp = scan.Points[i]; if (options.IsUpsideDown) { p = new PointF((float)(sp.RThetaPoint.R * Math.Cos(-sp.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(sp.RThetaPoint.R * Math.Sin(-sp.RThetaPoint.theta + robotPose.yaw) + robotPose.y)); } else { p = new PointF((float)(sp.RThetaPoint.R * Math.Cos(sp.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(sp.RThetaPoint.R * Math.Sin(sp.RThetaPoint.theta + robotPose.yaw) + robotPose.y)); } GLUtility.DrawCross(new GLPen(options.RenderColor, 1), Vector2.FromPointF(p), .1f); if (options.ShowIndex) { GLUtility.DrawString("Index: " + i, Color.Black, p); } } } if (options.ShowTS) { GLUtility.DrawString("Time: " + scan.Timestamp.ToString(), Color.Black, new PointF(0, 0)); } if (options.ShowOrigin) { GLUtility.DrawCircle(new GLPen(Color.Red, 1), new PointF(0, 0), .075f); } if (options.ShowBoundary) { PointF p = PointF.Empty, p2 = PointF.Empty; PointF pose = PointF.Empty; ILidar2DPoint spInitial = scan.Points[0]; ILidar2DPoint spEnd = scan.Points[scan.Points.Count - 1]; if (options.IsUpsideDown) { p = new PointF((float)(spInitial.RThetaPoint.R * Math.Cos(-spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spInitial.RThetaPoint.R * Math.Sin(-spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.y)); p2 = new PointF((float)(spEnd.RThetaPoint.R * Math.Cos(-spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spEnd.RThetaPoint.R * Math.Sin(-spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.y)); } else { p = new PointF((float)(spInitial.RThetaPoint.R * Math.Cos(spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spInitial.RThetaPoint.R * Math.Sin(spInitial.RThetaPoint.theta + robotPose.yaw) + robotPose.y)); p2 = new PointF((float)(spEnd.RThetaPoint.R * Math.Cos(spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.x), (float)(spEnd.RThetaPoint.R * Math.Sin(spEnd.RThetaPoint.theta + robotPose.yaw) + robotPose.y)); } pose.X = (float)robotPose.x; pose.Y = (float)robotPose.y; GLUtility.DrawLine(new GLPen(options.BoundaryColor, options.BoundaryWidth), p, pose); GLUtility.DrawLine(new GLPen(options.BoundaryColor, options.BoundaryWidth), p2, pose); } GLUtility.ComeBackFromSensorPose(); GLUtility.EnableNiceLines(); }
/// <summary> /// Update tracker with given input. It will do simple data association with given data (comparing distance between existing targets). /// </summary> /// <param name="uPixel">x-pixel in a camera coordinate</param> /// <param name="vPixel">y-pixel in a camera coordinate</param> /// <param name="range">laser range reading</param> /// <param name="currentPose"></param> /// <returns>target index = targetID</returns> public int Update(double uPixel, double vPixel, RobotPose currentPose, SensorPose lidarPose, TargetTypes type, ILidar2DPoint lidarPt, string cameraType, int numCamera) { Matrix zSCR, Xx2; double distance = 0; int targetIdx = 0; lock (locker) { // Compute X and Y position of the target based on the passed lidar point //Vector2 xyPos = TargetTrackingImpl.FindPosCoord(screenSize, range, uPixel, vPixel, currentPose, null); // this X Y coordinate = E N double yaw = currentPose.yaw - Math.PI / 2; double pitch = currentPose.pitch; double roll = currentPose.roll; Matrix R_ENU2R = new Matrix(Math.Cos(yaw), Math.Sin(yaw), 0, -Math.Sin(yaw), Math.Cos(yaw), 0, 0, 0, 1) * new Matrix(1, 0, 0, 0, Math.Cos(pitch), -Math.Sin(pitch), 0, Math.Sin(pitch), Math.Cos(pitch)) * new Matrix(Math.Cos(roll), 0, Math.Sin(roll), 0, 1, 0, -Math.Sin(roll), 0, Math.Cos(roll)); Matrix localPt = new Matrix(3, 1); localPt[0, 0] = -lidarPt.RThetaPoint.ToVector2().Y - lidarPose.y; localPt[1, 0] = lidarPt.RThetaPoint.ToVector2().X + lidarPose.x; Matrix globalPt = R_ENU2R.Inverse * localPt; Vector2 xyPos = new Vector2(globalPt[0, 0] + currentPose.x, globalPt[1, 0] + currentPose.y); this.xyPosList.Add(xyPos); #region debugging - go away // find the closest target //if (currentTargetPose.Count == 0) //{ // distance = double.MaxValue; // targetIdx = 0; //} //else //{ // targetIdx = FindClosestTarget(xyPos, ref distance, type); //} //if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI) //{ // if (distance > staticDistanceThreshold) // if the distance is larger than the threshold, add as a new target // { // if (currentTargetPose.Count == currentTargetPose.Capacity) // currentTargetPose.RemoveAt(0); // currentTargetPose.Add(xyPos); // implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType)); // targetIdx = currentTargetPose.Count - 1; // } //} //else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI) //{ // if (distance > dynamicDistanceThreshold) // if the distance is larger than the threshold, add as a new target // { // if (currentTargetPose.Count == currentTargetPose.Capacity) // currentTargetPose.RemoveAt(0); // currentTargetPose.Add(xyPos); // implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType)); // targetIdx = currentTargetPose.Count - 1; // } //} #endregion zSCR = new Matrix(3, 1); zSCR[0, 0] = uPixel; zSCR[1, 0] = vPixel; zSCR[2, 0] = lidarPt.RThetaPoint.R; Xx2 = new Matrix(9, 1); Xx2[0, 0] = currentPose.x; Xx2[1, 0] = currentPose.y; /* Xx2[2, 0] = currentPose.z; */ Xx2[2, 0] = 0; Xx2[3, 0] = currentPose.roll; Xx2[4, 0] = currentPose.pitch; Xx2[5, 0] = currentPose.yaw; //Xx2[3, 0] = 0; Xx2[4, 0] = 0; Xx2[5, 0] = currentPose.yaw; Xx2[6, 0] = 0; Xx2[7, 0] = 0; Xx2[8, 0] = 0; // modification for 3 cameras if (numCamera == 3) { if (uPixel > 0 && uPixel <= 320) { Xx2[6, 0] = 50 * Math.PI / 180; } else if (uPixel > 320 && uPixel <= 320 * 2) { zSCR[0, 0] = uPixel - 320; zSCR[1, 0] = vPixel; Xx2[6, 0] = 1.5 * Math.PI / 180; } else if (uPixel > 320 * 2) { zSCR[0, 0] = uPixel - 320 * 2; zSCR[1, 0] = vPixel; Xx2[6, 0] = -47 * Math.PI / 180; } } // find the most associated target Matrix Sxx2 = new Matrix(9, 9); Sxx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0])); Sxx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1])); Sxx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2])); Sxx2[3, 3] = Math.Sqrt(0.01); // roll Sxx2[4, 4] = Math.Sqrt(0.01); // pitch Sxx2[5, 5] = Math.Sqrt(0.03); // yaw Sxx2[6, 6] = Math.Sqrt(0.01); // pan Sxx2[7, 7] = Math.Sqrt(0.01); // tilt Sxx2[8, 8] = Math.Sqrt(0.01); // scan //targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCR); Matrix zSCRForTest = new Matrix(2, 1); zSCRForTest[0, 0] = lidarPt.RThetaPoint.R; zSCRForTest[1, 0] = lidarPt.RThetaPoint.theta; targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCRForTest); if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI) { if (targetIdx == -1) // if no good associated target found, then add a new one { implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType)); targetIdx = implList.Count - 1; } } else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI) { if (targetIdx == -1) { implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType)); targetIdx = implList.Count - 1; } } Matrix xhatPOI = new Matrix(7, 1); xhatPOI.Zero(); xhatPOI[0, 0] = xyPos.X; xhatPOI[1, 0] = xyPos.Y; xhatPOI[2, 0] = 0; if (!implList[targetIdx].IsInitialized) { implList[targetIdx].SetInitialPOIInfo(xhatPOI); } } Matrix Sx2 = new Matrix(9, 9); Sx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0])); Sx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1])); Sx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2])); //Sx2[0, 0] = Math.Sqrt(0.1); //Sx2[1, 1] = Math.Sqrt(0.1); //Sx2[2, 2] = Math.Sqrt(0.01); Sx2[3, 3] = Math.Sqrt(0.01); // roll Sx2[4, 4] = Math.Sqrt(0.01); // pitch Sx2[5, 5] = Math.Sqrt(0.01); // yaw Sx2[6, 6] = Math.Sqrt(0.01); // pan Sx2[7, 7] = Math.Sqrt(0.01); // tilt Sx2[8, 8] = Math.Sqrt(0.01); // scan // update with correct target implList[targetIdx].UpdateZSCR(zSCR); implList[targetIdx].UpdateVehicleState(Xx2, currentPose.timestamp); implList[targetIdx].UpdateSx2(Sx2); if (implList[targetIdx].Type != TargetTypes.ConfirmedSOOI && implList[targetIdx].Type != TargetTypes.ConfirmedMOOI) { implList[targetIdx].SetTargetType(type); } implList[targetIdx].Update(); return(targetIdx); }
static public double FindTargetDistance(ILidarScan <ILidar2DPoint> lidarScan, double u, double v, Dictionary <int, int> colorPix, Vector2 TLCorner, Vector2 RBCorner, string cameraSize, string camType, RobotPose camPose, TargetTypes type, out ILidar2DPoint lidarPt, ref List <Vector2> ptInBox) { #region camera stuff Matrix DCM4D = new Matrix(4, 4, 1.0); double[] fc = new double[2]; double[] cc = new double[2]; if (cameraSize.Equals("320x240")) { //for 320 x 240 image with Unibrain Fire-i camera if (camType.Equals("Fire-i")) { fc[0] = 384.4507; fc[1] = 384.1266; cc[0] = 155.1999; cc[1] = 101.5641; } // Fire-Fly MV else if (camType.Equals("FireFly")) { fc[0] = 345.26498; fc[1] = 344.99438; cc[0] = 159.36854; cc[1] = 118.26944; } } else if (cameraSize.Equals("640x480")) { // for 640 x 480 image with Unibrain Fire-i camera if (camType.Equals("Fire-i")) { fc[0] = 763.5805; fc[1] = 763.8337; cc[0] = 303.0963; cc[1] = 215.9287; } // for Fire-Fly MV (Point Gray) else if (camType.Equals("FireFly")) { fc[0] = 691.09778; fc[1] = 690.70187; cc[0] = 324.07388; cc[1] = 234.22204; } } double alpha_c = 0; // camera matrix Matrix KK = new Matrix(fc[0], alpha_c * fc[0], cc[0], 0, fc[1], cc[1], 0, 0, 1); #endregion // update DCM for point transformation DCM4D[0, 3] = camPose.x; DCM4D[1, 3] = camPose.y; DCM4D[2, 3] = camPose.z; DCM4D[0, 0] = Math.Cos(camPose.yaw); DCM4D[1, 1] = Math.Cos(camPose.yaw); DCM4D[0, 1] = Math.Sin(camPose.yaw); DCM4D[1, 0] = -Math.Sin(camPose.yaw); List <Vector2> pixelList = new List <Vector2>(lidarScan.Points.Count); List <ILidar2DPoint> lidarScanInBox = new List <ILidar2DPoint>(); foreach (ILidar2DPoint pt in lidarScan.Points) { Matrix point = new Matrix(4, 1); point[0, 0] = -pt.RThetaPoint.ToVector4().Y; point[1, 0] = pt.RThetaPoint.ToVector4().X; point[2, 0] = pt.RThetaPoint.ToVector4().Z; point[3, 0] = 1; Matrix transPt = DCM4D * point; Matrix ptImgPlane = new Matrix(3, 1); ptImgPlane[0, 0] = transPt[0, 0] / transPt[1, 0]; ptImgPlane[1, 0] = -transPt[2, 0] / transPt[1, 0]; ptImgPlane[2, 0] = transPt[1, 0] / transPt[1, 0]; ptImgPlane = KK * ptImgPlane; pixelList.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0])); if (ptImgPlane[0, 0] >= TLCorner.X && ptImgPlane[0, 0] <= RBCorner.X && ptImgPlane[1, 0] >= TLCorner.Y && ptImgPlane[1, 0] <= RBCorner.Y) { if (colorPix.Count > 0) { if (colorPix.ContainsKey((int)ptImgPlane[0, 0]) && colorPix[(int)ptImgPlane[0, 0]] == 255) { lidarScanInBox.Add(pt); ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0])); } } else { lidarScanInBox.Add(pt); ptInBox.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0])); } } } if (lidarScanInBox.Count == 0) { lidarPt = null; return(-1); } lidarPt = FineTargetDistanceClusterBased(lidarScanInBox); if (lidarPt == null) { return(-1); } return(lidarPt.RThetaPoint.R); }
/// <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 }
/// <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 }