public LidarPoseTargetMessage(int robotID, int targetID, RobotPose detectionPose, ILidarScan<ILidar2DPoint> scan) { this.robotID = robotID; this.targetID = targetID; this.detectionPose = detectionPose; this.scan = scan; }
public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan) { this.robotID = robotID; this.state = state; this.lidarScan = scan; this.sensorPose = sensorPose; }
/// <summary> /// Find targets above grounds and return in global and robot coordinate /// </summary> /// <param name="robotPose"></param> /// <param name="laserScan"></param> /// <param name="localPoints"></param> /// <param name="globalPoints"></param> public void DetectTarget(RobotPose robotPose, ILidarScan <ILidar2DPoint> laserScan, out List <Vector3> laserPoints, out List <Vector3> localPoints, bool lidarUpsideDown) { // initialization localPoints = new List <Vector3>(); laserPoints = new List <Vector3>(); for (int i = 0; i < laserScan.Points.Count; i++) { Vector4 localPt = laserToRobot * laserScan.Points[i].RThetaPoint.ToVector4(); Vector4 newLocalPt; if (lidarUpsideDown) { newLocalPt = new Vector4(-localPt.Y, localPt.X, localPt.Z, localPt.W); } else { newLocalPt = new Vector4(localPt.Y, localPt.X, localPt.Z, localPt.W); } localPoints.Add(new Vector3(newLocalPt.X, newLocalPt.Y, newLocalPt.Z)); if (lidarUpsideDown) { laserPoints.Add(new Vector3(laserScan.Points[i].RThetaPoint.R, laserScan.Points[i].RThetaPoint.theta, laserScan.Points[i].RThetaPoint.thetaDeg)); } else { laserPoints.Add(new Vector3(laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.R, laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.theta, laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.thetaDeg)); } } }
public LidarPoseTargetMessage(int robotID, int targetID, RobotPose detectionPose, ILidarScan <ILidar2DPoint> scan) { this.robotID = robotID; this.targetID = targetID; this.detectionPose = detectionPose; this.scan = scan; }
public LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan) { this.robotID = robotID; this.pose = pose; this.lidarScan = scan; this.sensorPose = sensorPose; }
public void SetScan(ILidarScan <ILidar2DPoint> scan) { lock (this.drawLock) { this.scan = scan; this.time = scan.Timestamp; this.numPoints = scan.Points.Count; } }
public void SetScan(ILidarScan <ILidar2DPoint> scan, RobotPose robotPose, int startIdx, int endIdx) { lock (this.drawLock) { this.scan = scan; this.time = scan.Timestamp; this.numPoints = endIdx - startIdx + 1; this.robotPose = robotPose; this.startIdx = startIdx; this.endIdx = endIdx; } }
public void SetScan(ILidarScan <ILidar2DPoint> scan, RobotPose robotPose) { lock (this.drawLock) { this.scan = scan; this.time = scan.Timestamp; this.numPoints = scan.Points.Count; this.robotPose = robotPose; this.startIdx = 0; this.endIdx = scan.Points.Count - 1; } }
public TargetListNoImageMessage(int robotID, List<Vector2> pixelLTCorner, List<Vector2> pixelRBCorner, List<int> targetIDs, ILidarScan<ILidar2DPoint> lidarScan, double timeStamp) { int numTarget = targetIDs.Count; //this.pixelLTCorner = new List<Vector2>(); this.pixelRBCorner = new List<Vector2>(); //for (int i = 0; i < numTarget; i++) //{ // this.pixelLTCorner.Add(pixelLTCorner[i]); // this.pixelRBCorner.Add(pixelRBCorner[i]); //} this.robotID = robotID; this.pixelLTCorner = pixelLTCorner; this.pixelRBCorner = pixelRBCorner; this.timeStamp = timeStamp; this.targetID = targetIDs; this.lidarScan = lidarScan; }
public void Handle2DLidarData(int robotID, ILidarScan <ILidar2DPoint> scan) { if (robots.ContainsKey(robotID) == false) { Console.WriteLine("warning: receiving lidar data for unknown robot : " + robotID); return; } if (scan.ScannerID == 1) { robots[robotID].LidarRendererFront.SetScan(scan); } if (scan.ScannerID == 0) { robots[robotID].LidarRendererFront.SetScan(scan); } }
public TargetListNoImageMessage(int robotID, List <Vector2> pixelLTCorner, List <Vector2> pixelRBCorner, List <int> targetIDs, ILidarScan <ILidar2DPoint> lidarScan, double timeStamp) { int numTarget = targetIDs.Count; //this.pixelLTCorner = new List<Vector2>(); this.pixelRBCorner = new List<Vector2>(); //for (int i = 0; i < numTarget; i++) //{ // this.pixelLTCorner.Add(pixelLTCorner[i]); // this.pixelRBCorner.Add(pixelRBCorner[i]); //} this.robotID = robotID; this.pixelLTCorner = pixelLTCorner; this.pixelRBCorner = pixelRBCorner; this.timeStamp = timeStamp; this.targetID = targetIDs; this.lidarScan = lidarScan; }
private void ScanToTextFile(ILidarScan <ILidar2DPoint> LidarScan) { TextWriter Raw = new StreamWriter("C:\\Users\\labuser\\Desktop\\RawData.txt"); TextWriter Pts = new StreamWriter("C:\\Users\\labuser\\Desktop\\PointData.txt"); Raw.WriteLine(); Pts.WriteLine(); for (int i = 0; i < LidarScan.Points.Count; i++) { Raw.Write(LidarScan.Points[i].RThetaPoint.R); Raw.Write(" "); Raw.WriteLine(LidarScan.Points[i].RThetaPoint.theta); Pts.WriteLine(LidarScan.Points[i].RThetaPoint.ToVector2().ToString()); } Raw.Close(); Pts.Close(); }
public Vector2[] TransformToGlobal(ILidarScan <ILidar2DPoint> scan, Matrix4 sensorDcm) { Vector2[] globalScan = new Vector2[scan.Points.Count]; Vector4 point; Matrix4 transformDcm = Matrix4.FromPose(pose) * sensorDcm; int n = scan.Points.Count; for (int i = 0; i < n; i++) { point = transformDcm * scan.Points[i].RThetaPoint.ToVector4(); if (point.Z >= 0.05) { globalScan[i] = new Vector2(point.X, point.Y); } } return(globalScan); }
/// <summary> /// Method to fill object variables /// </summary> /// <param name="lidarscandata">lidar scan data in the form of a List of Vector2s</param> public void RunDetector(ILidarScan <ILidar2DPoint> LidarScan, RobotPose pose) { //ScanToTextFile(LidarScan); //export scan data to analyze in matlab List <Vector2> lidarscandata = new List <Vector2>(LidarScan.Points.Count); for (int ii = 0; ii < LidarScan.Points.Count; ii++) { if (LidarScan.Points[ii].RThetaPoint.R < rangetoconsider) { lidarscandata.Add(LidarScan.Points[ii].RThetaPoint.ToVector2(isSickUpsideDown)); } } if (isSickUpsideDown) { lidarscandata.Reverse(); } polygons = ClusterScanData(lidarscandata); //PolygonsToTextFile(); //export polygon data to analyze in matlab AllGaps = FindAllGaps(polygons, pose); CheckGaps(AllGaps); }
/// <summary> /// Find targets above grounds and return in global and robot coordinate /// </summary> /// <param name="robotPose"></param> /// <param name="laserScan"></param> /// <param name="localPoints"></param> /// <param name="globalPoints"></param> public void DetectTarget(RobotPose robotPose, ILidarScan<ILidar2DPoint> laserScan, out List<Vector3> laserPoints, out List<Vector3> localPoints, bool lidarUpsideDown) { // initialization localPoints = new List<Vector3>(); laserPoints = new List<Vector3>(); for (int i = 0; i < laserScan.Points.Count; i++) { Vector4 localPt = laserToRobot * laserScan.Points[i].RThetaPoint.ToVector4(); Vector4 newLocalPt; if (lidarUpsideDown) newLocalPt = new Vector4(-localPt.Y, localPt.X, localPt.Z, localPt.W); else newLocalPt = new Vector4(localPt.Y, localPt.X, localPt.Z, localPt.W); localPoints.Add(new Vector3(newLocalPt.X, newLocalPt.Y, newLocalPt.Z)); if (lidarUpsideDown) laserPoints.Add(new Vector3(laserScan.Points[i].RThetaPoint.R, laserScan.Points[i].RThetaPoint.theta, laserScan.Points[i].RThetaPoint.thetaDeg)); else laserPoints.Add(new Vector3(laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.R, laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.theta, laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.thetaDeg)); } }
public void Handle2DLidarData(int robotID, ILidarScan<ILidar2DPoint> scan) { if (robots.ContainsKey(robotID) == false) { Console.WriteLine("warning: receiving lidar data for unknown robot : " + robotID); return; } if (scan.ScannerID == 1) robots[robotID].LidarRendererFront.SetScan(scan); if (scan.ScannerID == 0) robots[robotID].LidarRendererFront.SetScan(scan); }
public Vector2[] TransformToGlobal(ILidarScan<ILidar2DPoint> scan, Matrix4 sensorDcm) { Vector2[] globalScan = new Vector2[scan.Points.Count]; Vector4 point; Matrix4 transformDcm = Matrix4.FromPose(pose) * sensorDcm; int n = scan.Points.Count; for (int i = 0; i < n; i++) { point = transformDcm * scan.Points[i].RThetaPoint.ToVector4(); if (point.Z >= 0.05) globalScan[i] = new Vector2(point.X, point.Y); } return globalScan; }
public RobotTwoWheelCommand GetCommand(ILidarScan<ILidar2DPoint> LidarScan, int angleTolerance, double threshold, double vmax, double wmax, out List<Polygon> polys) { // convert laser data into polar direction and magnitude //ScanToTextFile(LidarScan); //export scan data to analyze in matlab hysteresismin = threshold * .75; double[] magnitudes, betas, gammas; int n = LidarScan.Points.Count; magnitudes = new double[n]; betas = new double[n]; gammas = new double[n]; for (int ii = 0; ii < n; ii++) { if (LidarScan.Points[ii].RThetaPoint.R < dmax) { magnitudes[ii] = (a - Math.Pow(LidarScan.Points[ii].RThetaPoint.R, 2) * b); if (LidarScan.Points[ii].RThetaPoint.R < Rbloat) gammas[ii] = Math.PI / 2; else gammas[ii] = Math.Asin(Rbloat / LidarScan.Points[ii].RThetaPoint.R); } else { magnitudes[ii] = 0; gammas[ii] = Math.Asin(Rbloat / dmax); } double betaii = LidarScan.Points[ii].RThetaPoint.thetaDeg; while (betaii < 0) { betaii = betaii + 360; } while (betaii > 360) { betaii = betaii - 360; } betas[ii] = betaii; } // create polar histogram double[] hk = new double[numk]; double[] angles = new double[numk]; //corresponding angle for (int z = 0; z < numk; z++) { hk[z] = 0; angles[z] = z * alpha; } for (int j = 0; j < magnitudes.Count(); j++) { //int kmin = (int)(((betas[j] - gammas[j] + 360) % 360) / alpha); //int kmax = (int)(((betas[j] + gammas[j] + 360) % 360) / alpha); int k = (int)(betas[j] / alpha); //for (int k = kmin; k <= kmax; k++) hk[k] = hk[k] + magnitudes[j]; } for (int k = 0; k < numk; k++) { if (hkold[k] >= threshold && (hk[k] > hysteresismin && hk[k] < threshold)) hk[k] = hkold[k]; } hkold = hk; // smooth the histogram double[] hkprime = Smoother(hk, 5); /*for (int i = 0; i < numk; i++) Console.WriteLine(angles[i] + "," + hk[i] + "," + hkprime[i]); Console.WriteLine("END +++++++++++++++++++++++");*/ // Temporary building of polygons //List<Vector2> points = new List<Vector2>(); polys = new List<Polygon>(); /*for (int i = 0; i < numk; i++) { points.Add(new Vector2(hkprime[i] * Math.Cos(angles[i] * Math.PI / 180), hkprime[i] * Math.Sin(angles[i] * Math.PI / 180))); } polys.Add(new Polygon(points));*/ // find candidate valleys double goalangle = 180 * (goalpoint.ArcTan) / Math.PI; ktarget = (int)(Math.Round(goalangle / alpha)); List<double[]> valleys = FindValleys(hkprime); if (!trapped) { // pick out best candidate valley double[] valley = PickBestValley(valleys); // calculate steering angle double steerangle = FindSteerAngle(valley); //degrees // calculate speed commands double vmin = 0; // (m/s) //double vmax = .2; // (m/s) //double wmax = 25; // (deg/s) double w = 0; double v = 0; double steerwindow = angleTolerance; if (Math.Abs(steerangle) > steerwindow) { w = wmax * Math.Sign(steerangle); } else { w = wmax * (steerangle / steerwindow); } /*points = new List<Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerangle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerangle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List<Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1 * Math.Cos(goalangle * Math.PI / 180), threshold * 1 * Math.Sin(goalangle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List<Vector2>(); for (int i = 0; i < 180; i++) { points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180))); } polys.Add(new Polygon(points));*/ double hc = Math.Min(hk[0], threshold); double vp = vmax * (1 - hc / threshold); v = vp * (1 - Math.Abs(w) / wmax) + vmin; double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units" double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec) //RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0); RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand); return command; } else // trapped (no valleys were found), so stop, or lift threshold { RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0); return command; } }
void lidar_ScanReceived(object sender, ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > e) { currentData = e.Scan; Messaging.lidarScanServer.SendUnreliably(new LidarScanMessage(Messaging.robotID, e.Scan)); }
public static List<Vector2> ExtractGlobalFeature(ILidarScan<ILidar2DPoint> scan, SensorPose sensorPose, RobotPose currentPose, double wheelRadius) { Stopwatch sw = new Stopwatch(); sw.Start(); List<Vector2> features = new List<Vector2>(); List<ILidar2DPoint> filteredScan = new List<ILidar2DPoint>(); //foreach (ILidar2DPoint pt in scan.Points) for (int i = 0; i < scan.Points.Count; i++) { if (scan.Points[i].RThetaPoint.R > 0.20) filteredScan.Add(scan.Points[i]); } Matrix4 laser2robotDCM = new Matrix4(1, 0, 0, 0, 0, Math.Cos(sensorPose.pitch), Math.Sin(sensorPose.pitch), 0, 0, -Math.Sin(sensorPose.pitch), Math.Cos(sensorPose.pitch), sensorPose.z, 0, 0, 0, 1); int neighbor = 3; for (int i = neighbor; i < filteredScan.Count - neighbor; i++) { // AGAIN, these codes are written respect to ENU coordinate frame double currentX = -filteredScan[i].RThetaPoint.R * Math.Sin(filteredScan[i].RThetaPoint.theta) - sensorPose.y; double currentY = filteredScan[i].RThetaPoint.R * Math.Cos(filteredScan[i].RThetaPoint.theta) + sensorPose.x; double lastX = -filteredScan[i - neighbor].RThetaPoint.R * Math.Sin(filteredScan[i - neighbor].RThetaPoint.theta) - sensorPose.y; double lastY = filteredScan[i - neighbor].RThetaPoint.R * Math.Cos(filteredScan[i - neighbor].RThetaPoint.theta) + sensorPose.x; double nextX = -filteredScan[i + neighbor].RThetaPoint.R * Math.Sin(filteredScan[i + neighbor].RThetaPoint.theta) - sensorPose.y; double nextY = filteredScan[i + neighbor].RThetaPoint.R * Math.Cos(filteredScan[i + neighbor].RThetaPoint.theta) + sensorPose.x; Matrix localPt = new Matrix(4, 1), lastLocalPt = new Matrix(4, 1), nextLocalPt = new Matrix(4, 1); localPt[0, 0] = currentX; localPt[1, 0] = currentY; localPt[3, 0] = 1; lastLocalPt[0, 0] = lastX; lastLocalPt[1, 0] = lastY; lastLocalPt[3, 0] = 1; nextLocalPt[0, 0] = nextX; nextLocalPt[1, 0] = nextY; nextLocalPt[3, 0] = 1; Vector4 currentPt = laser2robotDCM * localPt; Vector4 lastPt = laser2robotDCM * lastLocalPt; Vector4 nextPt = laser2robotDCM * nextLocalPt; // check if the z position of the laser return is higher than 10 cm of current z-state //if (currentPt.Z > currentPose.z + 0.1) // features.Add(new Vector2(currentPt.X, currentPt.Y)); // height comparison Vector2 neighborPt = FindNeighborToCompare(filteredScan, sensorPose, i, 0.1); Matrix neighbotPtM = new Matrix(4, 1); neighbotPtM[0, 0] = neighborPt.X; neighbotPtM[1, 0] = neighborPt.Y; neighbotPtM[3, 0] = 1; Vector4 neighborPtV = laser2robotDCM * neighbotPtM; double yaw = currentPose.yaw - Math.PI / 2; double heightDiff = neighborPtV.Z - currentPt.Z; if (heightDiff > 0.1) { features.Add(new Vector2(neighborPtV.X * Math.Cos(yaw) - neighborPtV.Y * Math.Sin(yaw) + currentPose.x, neighborPtV.X * Math.Sin(yaw) + neighborPtV.Y * Math.Cos(yaw) + currentPose.y)); } else if (heightDiff < -0.10) { features.Add(new Vector2(currentPt.X * Math.Cos(yaw) - currentPt.Y * Math.Sin(yaw) + currentPose.x, currentPt.X * Math.Sin(yaw) + currentPt.Y * Math.Cos(yaw) + currentPose.y)); } } Console.WriteLine("Jump(?) Extraction Time:" + sw.ElapsedMilliseconds); return features; }
public void UpdateLidarScan(ILidarScan<ILidar2DPoint> s) { lock (dataLocker) { Stopwatch sw = new Stopwatch(); sw.Start(); curScan2D = s; if (curRobotPose != null && curScan2D != null /*&& curRobotPose.timestamp != priorTimeStamp && curRobotPose.timestamp != 0*/) { //Get the laser to body coordinate system (this is I for now) Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody); //Get the body to global transformation Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose); //Get a vector from the current lidar pose Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z); //Transform the sensor position in body coordinates to the sensor position in global coordinates Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody); //Get the current grid indicies int xLidarPoseIndex, yLidarPoseIndex; currentOccupancyGrid.GetIndicies(lidarInGlobal.X, lidarInGlobal.Y, out xLidarPoseIndex, out yLidarPoseIndex); //Find the cells corresponding to each LIDAR return and make a list of the cells that are clear from the sensor to that point Dictionary<Vector2, Boolean> occupiedCellsThisScan = new Dictionary<Vector2, Boolean>(curScan2D.Points.Count()); Dictionary<Vector2, Boolean> clearCellsThisScan = new Dictionary<Vector2, Boolean>(); //Process each lidar return foreach (ILidar2DPoint pt in curScan2D.Points) { if (pt.RThetaPoint.R < lidarMaxPracticalRange) { //Extract the lidar point in XYZ (laser coordinates) Vector3 v3 = pt.RThetaPoint.ToVector3(); //Convert laser to body coordinate system Vector3 vBody = Tlaser2body.TransformPoint(v3); //Convert body to global cooridnate system Vector3 vGlobal = Tbody2global.TransformPoint(vBody); //Find the index of the laser return int xLaserIndex, yLaserIndex; currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex); //Add to the list of occupied cells if (currentOccupancyGrid.CheckValidIdx(xLaserIndex, yLaserIndex)) { occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex)] = true; //occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex)] = true; //occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex - 1)] = true; //occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex - 1)] = true; } } } //Process each lidar return foreach (ILidar2DPoint pt in curScan2D.Points) { if (pt.RThetaPoint.R < lidarMaxPracticalRange) { //Extract the lidar point in XYZ (laser coordinates) Vector3 v3 = pt.RThetaPoint.ToVector3(); //Convert laser to body coordinate system Vector3 vBody = Tlaser2body.TransformPoint(v3); //Convert body to global cooridnate system Vector3 vGlobal = Tbody2global.TransformPoint(vBody); //Find the index of the laser return int xLaserIndex, yLaserIndex; currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex); //Ray trace between the two points performing the update Raytrace(xLidarPoseIndex, yLidarPoseIndex, xLaserIndex, yLaserIndex, occupiedCellsThisScan, clearCellsThisScan); } } //decay the whole grid for (int i = 0; i < currentOccupancyGrid.NumCellX; i++) { for (int j = 0; j < currentOccupancyGrid.NumCellY; j++) { double value = currentOccupancyGrid.GetCellByIdx(i, j); currentOccupancyGrid.SetCellByIdx(i, j, value *= gridDecay); } } foreach (Vector2 cellIdx in occupiedCellsThisScan.Keys) { UpdateCellOccupied((int)cellIdx.X, (int)cellIdx.Y); } foreach (Vector2 cellIdx in clearCellsThisScan.Keys) { UpdateCellClear((int)cellIdx.X, (int)cellIdx.Y); } //Copy for the timestamp for the next iteration priorTimeStamp = (double)curRobotPose.timestamp; } // Console.WriteLine("OG Took: " + sw.ElapsedMilliseconds); }//lock if (outputOccupancyGrid != null) { outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy(); if (curRobotPose != null) if (NewGridAvailable != null) NewGridAvailable(this, new NewOccupancyGrid2DAvailableEventArgs(GetOccupancyGrid(), curRobotPose.timestamp)); } }
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); }
public void SetScan(ILidarScan<ILidar2DPoint> scan, RobotPose robotPose) { lock (this.drawLock) { this.scan = scan; this.time = scan.Timestamp; this.numPoints = scan.Points.Count; this.robotPose = robotPose; this.startIdx = 0; this.endIdx = scan.Points.Count - 1; } }
void lidar_ScanReceived(object sender, ILidarScanEventArgs<ILidarScan<ILidar2DPoint>> e) { currentData = e.Scan; Messaging.lidarScanServer.SendUnreliably(new LidarScanMessage(Messaging.robotID, e.Scan)); }
/// <summary> /// Returns lidar scan projection into a image plane /// </summary> /// <param name="lidarScan"></param> /// <param name="cameraSize">"320x240" or "640x480"</param> /// <param name="camType">"Fire-i" or "FireFly"</param> /// <param name="camPose">Your camera pose relative to the lidar</param> /// <returns>List of pixel values for each lidar point</returns> static public List <Vector2> FindLidarProjection(ILidarScan <ILidar2DPoint> lidarScan, string cameraSize, string camType, SensorPose camPose) { 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); // 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); 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])); } return(pixelList); }
/// <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 LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan) { this.robotID = robotID; this.pose = pose; this.lidarScan = scan; this.sensorPose = sensorPose; }
public List<int> fExtract(ILidarScan<ILidar2DPoint> currentScan, double bAngle, double maxRange) { #region cluster basis variables ibook = new List<int>(); List<Vector2> xCart = new List<Vector2>(); List<Vector2> currentScanXY = new List<Vector2>(); List<double> iNonzero = new List<double>(); List<double> tPulse = new List<double>(); List<double> rCart = new List<double>(); List<double> rCartXY = new List<double>(); List<double> rMap = new List<double>(); List<int> iDotCorners = new List<int>(); #endregion #region cluster basis for (int i = 0; i < currentScan.Points.Count; i++) { currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2()); rCartXY.Add(currentScan.Points[i].RThetaPoint.R); if (currentScan.Points[i].RThetaPoint.R < maxRange) { xCart.Add(currentScanXY[i]); ibook.Add(i); tPulse.Add(currentScan.Points[i].RThetaPoint.theta); rCart.Add(currentScan.Points[i].RThetaPoint.R); } } iDotCorners = dotProductExtract(xCart, bAngle); List<int> ibookTemp = new List<int>(); List<double> tPulseTemp = new List<double>(); List<double> rCartTemp = new List<double>(); for (int i = 0; i < iDotCorners.Count; i++) { ibookTemp.Add(ibook[iDotCorners[i]]); tPulseTemp.Add(tPulse[iDotCorners[i]]); rCartTemp.Add(rCart[iDotCorners[i]]); } ibook = new List<int>(ibookTemp); tPulse = new List<double>(tPulseTemp); rMap = new List<double>(rCartTemp); #endregion #region cluster train variables List<bool> rPulse = new List<bool>(); List<bool> rState = new List<bool>(); List<int> iCluster = new List<int>(); List<Vector2> iClusterVec = new List<Vector2>(); List<Vector2> iClusterVecTemp = new List<Vector2>(); List<double> cMap = new List<double>(); List<int> stableClusters = new List<int>(); rPulse.Add(true); rState.Add(true); iCluster.Add(0); #endregion #region cluster train for (int i = 1; i < rMap.Count - 1; i++) { rPulse.Add((Math.Abs(rMap[i] - rMap[i - 1]) < 1.0) && (Math.Abs(tPulse[i] - tPulse[i - 1]) < 5.0 * (Math.PI / 180.0))); if (rPulse[i] == true) rState.Add(rState[i - 1]); else { rState.Add(!(rState[i - 1])); iCluster.Add(i - 1); iCluster.Add(i); } } #endregion #region stable (single-element) clusters if (iCluster.Count > 1) { #region instantiate clusters iCluster.Add(iDotCorners.Count - 1); for (int i = 0; i < iCluster.Count / 2; i++) iClusterVec.Add(new Vector2(iCluster[2 * i], iCluster[2 * i + 1])); for (int i = 0; i < iClusterVec.Count; i++) { double nC = 0.0; Vector2 qTemp = new Vector2(); qTemp.X = 0; qTemp.Y = 0; for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++) { qTemp = qTemp + currentScanXY[ibook[j]]; nC = nC + 1.0; } qTemp = qTemp / nC; cMap.Add(Math.Sqrt(Math.Pow(qTemp.X, 2) + Math.Pow(qTemp.Y, 2))); } #endregion #region propose stable clusters iClusterVecTemp.Clear(); for (int i = 1; i < cMap.Count - 1; i++) { if ((cMap[i] > cMap[i - 1]) && (cMap[i] > cMap[i + 1])) { } else { if (cMap[i] > 1.0) iClusterVecTemp.Add(iClusterVec[i]); } } iClusterVec = new List<Vector2>(iClusterVecTemp); #endregion #region cluster stability assessment stableClusters.Clear(); for (int i = 0; i < iClusterVec.Count; i++) { #region shadow-test-1 double st1 = rCart[iDotCorners[(int)iClusterVec[i].X] - 1]; double nC = 0.0; double rTemp = 0.0; for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++) { rTemp = rTemp + rCart[iDotCorners[j]]; nC = nC + 1.0; } double st2 = rTemp / nC; double st3 = rCart[iDotCorners[(int)iClusterVec[i].X] + 1]; Vector2 O = new Vector2(2.0, st2); Vector2 u = new Vector2(1.0, st1); Vector2 v = new Vector2(3.0, st3); Vector2 stX = u + v - 2 * O; #endregion #region shadow-test-2 st1 = rCart[iDotCorners[(int)iClusterVec[i].Y] - 1]; nC = 0.0; rTemp = 0.0; for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++) { rTemp = rTemp + rCart[iDotCorners[j]]; nC = nC + 1.0; } st2 = rTemp / nC; st3 = rCart[iDotCorners[(int)iClusterVec[i].Y] + 1]; O = new Vector2(2.0, st2); u = new Vector2(1.0, st1); v = new Vector2(3.0, st3); Vector2 stY = u + v - 2 * O; #endregion if (Math.Atan2(stX.Y, stX.X) > 0 && Math.Atan2(stY.Y, stY.X) > 0) stableClusters.Add(i); } iClusterVecTemp.Clear(); for (int i = 0; i < stableClusters.Count; i++) iClusterVecTemp.Add(iClusterVec[stableClusters[i]]); iClusterVec = new List<Vector2>(iClusterVecTemp); #endregion #region reject large clusters iClusterVecTemp.Clear(); for (int i = 0; i < iClusterVec.Count; i++) { if (Math.Abs((int)iClusterVec[i].X - (int)iClusterVec[i].Y) <= 1) iClusterVecTemp.Add(iClusterVec[i]); } iClusterVec = new List<Vector2>(iClusterVecTemp); ibookTemp.Clear(); if (ibook != null) { for (int i = 0; i < iClusterVec.Count; i++) { ibookTemp.Add(ibook[(int)iClusterVec[i].X]); } ibook = new List<int>(ibookTemp); } #endregion } else { ibook = null; } #endregion return ibook; }
/// <summary> /// Method to fill object variables /// </summary> /// <param name="lidarscandata">lidar scan data in the form of a List of Vector2s</param> public void RunDetector(ILidarScan<ILidar2DPoint> LidarScan, RobotPose pose) { //ScanToTextFile(LidarScan); //export scan data to analyze in matlab List<Vector2> lidarscandata = new List<Vector2>(LidarScan.Points.Count); for (int ii = 0; ii < LidarScan.Points.Count; ii++) { if (LidarScan.Points[ii].RThetaPoint.R < rangetoconsider) lidarscandata.Add(LidarScan.Points[ii].RThetaPoint.ToVector2(isSickUpsideDown)); } if (isSickUpsideDown) lidarscandata.Reverse(); polygons = ClusterScanData(lidarscandata); //PolygonsToTextFile(); //export polygon data to analyze in matlab AllGaps = FindAllGaps(polygons, pose); CheckGaps(AllGaps); }
/// <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 void SetScan(ILidarScan<ILidar2DPoint> scan) { lock (this.drawLock) { this.scan = scan; this.time = scan.Timestamp; this.numPoints = scan.Points.Count; } }
public List <int> fExtract(ILidarScan <ILidar2DPoint> currentScan, double bAngle, double maxRange) { #region cluster basis variables ibook = new List <int>(); List <Vector2> xCart = new List <Vector2>(); List <Vector2> currentScanXY = new List <Vector2>(); List <double> iNonzero = new List <double>(); List <double> tPulse = new List <double>(); List <double> rCart = new List <double>(); List <double> rCartXY = new List <double>(); List <double> rMap = new List <double>(); List <int> iDotCorners = new List <int>(); #endregion #region cluster basis for (int i = 0; i < currentScan.Points.Count; i++) { currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2()); rCartXY.Add(currentScan.Points[i].RThetaPoint.R); if (currentScan.Points[i].RThetaPoint.R < maxRange) { xCart.Add(currentScanXY[i]); ibook.Add(i); tPulse.Add(currentScan.Points[i].RThetaPoint.theta); rCart.Add(currentScan.Points[i].RThetaPoint.R); } } iDotCorners = dotProductExtract(xCart, bAngle); List <int> ibookTemp = new List <int>(); List <double> tPulseTemp = new List <double>(); List <double> rCartTemp = new List <double>(); for (int i = 0; i < iDotCorners.Count; i++) { ibookTemp.Add(ibook[iDotCorners[i]]); tPulseTemp.Add(tPulse[iDotCorners[i]]); rCartTemp.Add(rCart[iDotCorners[i]]); } ibook = new List <int>(ibookTemp); tPulse = new List <double>(tPulseTemp); rMap = new List <double>(rCartTemp); #endregion #region cluster train variables List <bool> rPulse = new List <bool>(); List <bool> rState = new List <bool>(); List <int> iCluster = new List <int>(); List <Vector2> iClusterVec = new List <Vector2>(); List <Vector2> iClusterVecTemp = new List <Vector2>(); List <double> cMap = new List <double>(); List <int> stableClusters = new List <int>(); rPulse.Add(true); rState.Add(true); iCluster.Add(0); #endregion #region cluster train for (int i = 1; i < rMap.Count - 1; i++) { rPulse.Add((Math.Abs(rMap[i] - rMap[i - 1]) < 1.0) && (Math.Abs(tPulse[i] - tPulse[i - 1]) < 5.0 * (Math.PI / 180.0))); if (rPulse[i] == true) { rState.Add(rState[i - 1]); } else { rState.Add(!(rState[i - 1])); iCluster.Add(i - 1); iCluster.Add(i); } } #endregion #region stable (single-element) clusters if (iCluster.Count > 1) { #region instantiate clusters iCluster.Add(iDotCorners.Count - 1); for (int i = 0; i < iCluster.Count / 2; i++) { iClusterVec.Add(new Vector2(iCluster[2 * i], iCluster[2 * i + 1])); } for (int i = 0; i < iClusterVec.Count; i++) { double nC = 0.0; Vector2 qTemp = new Vector2(); qTemp.X = 0; qTemp.Y = 0; for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++) { qTemp = qTemp + currentScanXY[ibook[j]]; nC = nC + 1.0; } qTemp = qTemp / nC; cMap.Add(Math.Sqrt(Math.Pow(qTemp.X, 2) + Math.Pow(qTemp.Y, 2))); } #endregion #region propose stable clusters iClusterVecTemp.Clear(); for (int i = 1; i < cMap.Count - 1; i++) { if ((cMap[i] > cMap[i - 1]) && (cMap[i] > cMap[i + 1])) { } else { if (cMap[i] > 1.0) { iClusterVecTemp.Add(iClusterVec[i]); } } } iClusterVec = new List <Vector2>(iClusterVecTemp); #endregion #region cluster stability assessment stableClusters.Clear(); for (int i = 0; i < iClusterVec.Count; i++) { #region shadow-test-1 double st1 = rCart[iDotCorners[(int)iClusterVec[i].X] - 1]; double nC = 0.0; double rTemp = 0.0; for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++) { rTemp = rTemp + rCart[iDotCorners[j]]; nC = nC + 1.0; } double st2 = rTemp / nC; double st3 = rCart[iDotCorners[(int)iClusterVec[i].X] + 1]; Vector2 O = new Vector2(2.0, st2); Vector2 u = new Vector2(1.0, st1); Vector2 v = new Vector2(3.0, st3); Vector2 stX = u + v - 2 * O; #endregion #region shadow-test-2 st1 = rCart[iDotCorners[(int)iClusterVec[i].Y] - 1]; nC = 0.0; rTemp = 0.0; for (int j = (int)iClusterVec[i].X; j < (int)iClusterVec[i].Y + 1; j++) { rTemp = rTemp + rCart[iDotCorners[j]]; nC = nC + 1.0; } st2 = rTemp / nC; st3 = rCart[iDotCorners[(int)iClusterVec[i].Y] + 1]; O = new Vector2(2.0, st2); u = new Vector2(1.0, st1); v = new Vector2(3.0, st3); Vector2 stY = u + v - 2 * O; #endregion if (Math.Atan2(stX.Y, stX.X) > 0 && Math.Atan2(stY.Y, stY.X) > 0) { stableClusters.Add(i); } } iClusterVecTemp.Clear(); for (int i = 0; i < stableClusters.Count; i++) { iClusterVecTemp.Add(iClusterVec[stableClusters[i]]); } iClusterVec = new List <Vector2>(iClusterVecTemp); #endregion #region reject large clusters iClusterVecTemp.Clear(); for (int i = 0; i < iClusterVec.Count; i++) { if (Math.Abs((int)iClusterVec[i].X - (int)iClusterVec[i].Y) <= 1) { iClusterVecTemp.Add(iClusterVec[i]); } } iClusterVec = new List <Vector2>(iClusterVecTemp); ibookTemp.Clear(); if (ibook != null) { for (int i = 0; i < iClusterVec.Count; i++) { ibookTemp.Add(ibook[(int)iClusterVec[i].X]); } ibook = new List <int>(ibookTemp); } #endregion } else { ibook = null; } #endregion return(ibook); }
public LidarScanMessage(int robotID, ILidarScan <ILidar2DPoint> scan) { this.scan = scan; this.robotID = robotID; }
public static List <Vector2> ExtractGlobalFeature(ILidarScan <ILidar2DPoint> scan, SensorPose sensorPose, RobotPose currentPose, double wheelRadius) { Stopwatch sw = new Stopwatch(); sw.Start(); List <Vector2> features = new List <Vector2>(); List <ILidar2DPoint> filteredScan = new List <ILidar2DPoint>(); //foreach (ILidar2DPoint pt in scan.Points) for (int i = 0; i < scan.Points.Count; i++) { if (scan.Points[i].RThetaPoint.R > 0.20) { filteredScan.Add(scan.Points[i]); } } Matrix4 laser2robotDCM = new Matrix4(1, 0, 0, 0, 0, Math.Cos(sensorPose.pitch), Math.Sin(sensorPose.pitch), 0, 0, -Math.Sin(sensorPose.pitch), Math.Cos(sensorPose.pitch), sensorPose.z, 0, 0, 0, 1); int neighbor = 3; for (int i = neighbor; i < filteredScan.Count - neighbor; i++) { // AGAIN, these codes are written respect to ENU coordinate frame double currentX = -filteredScan[i].RThetaPoint.R * Math.Sin(filteredScan[i].RThetaPoint.theta) - sensorPose.y; double currentY = filteredScan[i].RThetaPoint.R * Math.Cos(filteredScan[i].RThetaPoint.theta) + sensorPose.x; double lastX = -filteredScan[i - neighbor].RThetaPoint.R * Math.Sin(filteredScan[i - neighbor].RThetaPoint.theta) - sensorPose.y; double lastY = filteredScan[i - neighbor].RThetaPoint.R * Math.Cos(filteredScan[i - neighbor].RThetaPoint.theta) + sensorPose.x; double nextX = -filteredScan[i + neighbor].RThetaPoint.R * Math.Sin(filteredScan[i + neighbor].RThetaPoint.theta) - sensorPose.y; double nextY = filteredScan[i + neighbor].RThetaPoint.R * Math.Cos(filteredScan[i + neighbor].RThetaPoint.theta) + sensorPose.x; Matrix localPt = new Matrix(4, 1), lastLocalPt = new Matrix(4, 1), nextLocalPt = new Matrix(4, 1); localPt[0, 0] = currentX; localPt[1, 0] = currentY; localPt[3, 0] = 1; lastLocalPt[0, 0] = lastX; lastLocalPt[1, 0] = lastY; lastLocalPt[3, 0] = 1; nextLocalPt[0, 0] = nextX; nextLocalPt[1, 0] = nextY; nextLocalPt[3, 0] = 1; Vector4 currentPt = laser2robotDCM * localPt; Vector4 lastPt = laser2robotDCM * lastLocalPt; Vector4 nextPt = laser2robotDCM * nextLocalPt; // check if the z position of the laser return is higher than 10 cm of current z-state //if (currentPt.Z > currentPose.z + 0.1) // features.Add(new Vector2(currentPt.X, currentPt.Y)); // height comparison Vector2 neighborPt = FindNeighborToCompare(filteredScan, sensorPose, i, 0.1); Matrix neighbotPtM = new Matrix(4, 1); neighbotPtM[0, 0] = neighborPt.X; neighbotPtM[1, 0] = neighborPt.Y; neighbotPtM[3, 0] = 1; Vector4 neighborPtV = laser2robotDCM * neighbotPtM; double yaw = currentPose.yaw - Math.PI / 2; double heightDiff = neighborPtV.Z - currentPt.Z; if (heightDiff > 0.1) { features.Add(new Vector2(neighborPtV.X * Math.Cos(yaw) - neighborPtV.Y * Math.Sin(yaw) + currentPose.x, neighborPtV.X * Math.Sin(yaw) + neighborPtV.Y * Math.Cos(yaw) + currentPose.y)); } else if (heightDiff < -0.10) { features.Add(new Vector2(currentPt.X * Math.Cos(yaw) - currentPt.Y * Math.Sin(yaw) + currentPose.x, currentPt.X * Math.Sin(yaw) + currentPt.Y * Math.Cos(yaw) + currentPose.y)); } } Console.WriteLine("Jump(?) Extraction Time:" + sw.ElapsedMilliseconds); return(features); }
public RobotTwoWheelCommand GetCommand(ILidarScan <ILidar2DPoint> LidarScan, int angleTolerance, double threshold, double vmax, double wmax, out List <Polygon> polys) { // convert laser data into polar direction and magnitude //ScanToTextFile(LidarScan); //export scan data to analyze in matlab hysteresismin = threshold * .75; double[] magnitudes, betas, gammas; int n = LidarScan.Points.Count; magnitudes = new double[n]; betas = new double[n]; gammas = new double[n]; for (int ii = 0; ii < n; ii++) { if (LidarScan.Points[ii].RThetaPoint.R < dmax) { magnitudes[ii] = (a - Math.Pow(LidarScan.Points[ii].RThetaPoint.R, 2) * b); if (LidarScan.Points[ii].RThetaPoint.R < Rbloat) { gammas[ii] = Math.PI / 2; } else { gammas[ii] = Math.Asin(Rbloat / LidarScan.Points[ii].RThetaPoint.R); } } else { magnitudes[ii] = 0; gammas[ii] = Math.Asin(Rbloat / dmax); } double betaii = LidarScan.Points[ii].RThetaPoint.thetaDeg; while (betaii < 0) { betaii = betaii + 360; } while (betaii > 360) { betaii = betaii - 360; } betas[ii] = betaii; } // create polar histogram double[] hk = new double[numk]; double[] angles = new double[numk]; //corresponding angle for (int z = 0; z < numk; z++) { hk[z] = 0; angles[z] = z * alpha; } for (int j = 0; j < magnitudes.Count(); j++) { //int kmin = (int)(((betas[j] - gammas[j] + 360) % 360) / alpha); //int kmax = (int)(((betas[j] + gammas[j] + 360) % 360) / alpha); int k = (int)(betas[j] / alpha); //for (int k = kmin; k <= kmax; k++) hk[k] = hk[k] + magnitudes[j]; } for (int k = 0; k < numk; k++) { if (hkold[k] >= threshold && (hk[k] > hysteresismin && hk[k] < threshold)) { hk[k] = hkold[k]; } } hkold = hk; // smooth the histogram double[] hkprime = Smoother(hk, 5); /*for (int i = 0; i < numk; i++) * Console.WriteLine(angles[i] + "," + hk[i] + "," + hkprime[i]); * * Console.WriteLine("END +++++++++++++++++++++++");*/ // Temporary building of polygons //List<Vector2> points = new List<Vector2>(); polys = new List <Polygon>(); /*for (int i = 0; i < numk; i++) * { * points.Add(new Vector2(hkprime[i] * Math.Cos(angles[i] * Math.PI / 180), hkprime[i] * Math.Sin(angles[i] * Math.PI / 180))); * } * polys.Add(new Polygon(points));*/ // find candidate valleys double goalangle = 180 * (goalpoint.ArcTan) / Math.PI; ktarget = (int)(Math.Round(goalangle / alpha)); List <double[]> valleys = FindValleys(hkprime); if (!trapped) { // pick out best candidate valley double[] valley = PickBestValley(valleys); // calculate steering angle double steerangle = FindSteerAngle(valley); //degrees // calculate speed commands double vmin = 0; // (m/s) //double vmax = .2; // (m/s) //double wmax = 25; // (deg/s) double w = 0; double v = 0; double steerwindow = angleTolerance; if (Math.Abs(steerangle) > steerwindow) { w = wmax * Math.Sign(steerangle); } else { w = wmax * (steerangle / steerwindow); } /*points = new List<Vector2>(); * points.Add(new Vector2(0, 0)); * points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerangle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerangle * Math.PI / 180))); * polys.Add(new Polygon(points)); * * points = new List<Vector2>(); * points.Add(new Vector2(0, 0)); * points.Add(new Vector2(threshold * 1 * Math.Cos(goalangle * Math.PI / 180), threshold * 1 * Math.Sin(goalangle * Math.PI / 180))); * polys.Add(new Polygon(points)); * * points = new List<Vector2>(); * for (int i = 0; i < 180; i++) * { * points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180))); * } * polys.Add(new Polygon(points));*/ double hc = Math.Min(hk[0], threshold); double vp = vmax * (1 - hc / threshold); v = vp * (1 - Math.Abs(w) / wmax) + vmin; double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units" double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec) //RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0); RobotTwoWheelCommand command = new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand); return(command); } else // trapped (no valleys were found), so stop, or lift threshold { RobotTwoWheelCommand command = new RobotTwoWheelCommand(0, 0); return(command); } }
private void lidar_ScanReceived(object sender, ILidarScanEventArgs<ILidarScan<ILidar2DPoint>> e) { currentData = e.Scan; }
void sick_ScanReceived(object sender, ILidarScanEventArgs<ILidarScan<ILidar2DPoint>> e) { scan = e.Scan; BuildHistogram(); }
void SLAMEKFMainLoop(object o) { k = 0; while (running) { lock (dataLock) { if (currentScan == null || viconPose == null || odomPose == null || viconLastPose == viconPose) { continue; // no data, no update } else { #region discrete-time increment k = k + 1; #endregion if (k == 1) { #region state initialization initialOdomPose = odomPose; transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp); transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.yaw = odomPose.yaw; transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose; odomLastPose = transformedOdomPose; xhatvPose = transformedOdomPose; #endregion #region map initialization and management #region extract features -> fz List <Vector2> currentScanXY = new List <Vector2>(); fz.Clear(); ibook = fExtract(currentScan, bAngle, maxRange); if (ibook != null) { for (int i = 0; i < currentScan.Points.Count; i++) { currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2()); } for (int i = 0; i < ibook.Count; i++) { fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose)); } } #endregion #region populate the map -> landmarkList landmarkList.Clear(); for (int i = 0; i < fz.Count; i++) { landmarkList.Add(fz[i]); } #endregion #region populate the map -> mhat mhat = new Matrix(2 * landmarkList.Count, 1); for (int i = 0; i < landmarkList.Count; i++) { mhat[2 * i, 0] = landmarkList[i].X; mhat[2 * i + 1, 0] = landmarkList[i].Y; } #endregion #region covariance management -> Paug Qv = new Matrix(3, 3, Math.Pow(sigw, 2)); Pv = Qv; Paug = Pv; for (int i = 0; i < fz.Count; i++) { Paug = blkdiag(Paug, new Matrix(2, 2, Math.Pow(sigw, 2))); } #endregion lastScan = currentScan; #endregion } else { #region odometry processing transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp); transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.yaw = odomPose.yaw; transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose; xStep = transformedOdomPose - odomLastPose; double omega = odomLastPose.yaw + xStep.yaw; if (xStep.x == 0) { uvk.x = 0; } else { double vx = xStep.x / Math.Cos(omega); uvk.x = vx * Math.Cos(omega); } if (xStep.y == 0) { uvk.y = 0; } else { double vy = xStep.y / Math.Sin(omega); uvk.y = vy * Math.Sin(omega); } uvk.yaw = xStep.yaw; uvk.z = 0; uvk.roll = 0; uvk.pitch = 0; uvk.timestamp = odomPose.timestamp; odomLastPose = transformedOdomPose; #endregion #region time-update xhatvPose = xhatvPose + uvk; Matrix Uvk = new Matrix(3, 1); Uvk[0, 0] = uvk.x; Uvk[1, 0] = uvk.y; Uvk[2, 0] = uvk.yaw; Pv = Pv + ScalarMultiply(Math.Pow(0.5, 2), Uvk * Uvk.Transpose()) + Qv; Paug.SetSubMatrix(0, 2, 0, 2, Pv); Matrix Qmv = new Matrix(0, 0, 1.0); for (int i = 0; i < landmarkList.Count; i++) { Qmv = blkdiag(Qmv, ScalarMultiply(Math.Pow(sigw / 5, 2), Pv.Submatrix(0, 1, 0, 1))); } Matrix Qm = new Matrix(Paug.Rows - 3, Paug.Columns - 3, Math.Pow(sigw / 5, 2)); Paug.SetSubMatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1, Paug.Submatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1) + Qmv + Qm); #endregion #region feature exraction numLandmarks = landmarkList.Count; List <Vector2> currentScanXY = new List <Vector2>(); fz.Clear(); ibook = fExtract(currentScan, bAngle, maxRange); if (ibook != null) { for (int i = 0; i < currentScan.Points.Count; i++) { currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2()); } for (int i = 0; i < ibook.Count; i++) { fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose)); } } #endregion #region data association #region variables fzHat = new List <Vector2>(landmarkList); List <Vector3> jObsv = new List <Vector3>(); List <Vector2> iObsv = new List <Vector2>(); List <Vector2> obsvFeatureList = new List <Vector2>(); Matrix cLP = new Matrix(fz.Count, fzHat.Count); Matrix dapGate = new Matrix(fz.Count, fzHat.Count); Matrix fzM = new Matrix(2, 1); Matrix fzHatM = new Matrix(2, 1); #endregion #region initialization // "lpsolver55.dll" directory: // (note: Alt + F7 -> Build -> enable "Allow UnSafe Code") lpsolve.Init("C:\\users\\labuser\\desktop\\MAGIC\\Framework\\SLAM"); double[] xOpt; double ignoreThisEntry = 0; int nDecision = fz.Count * fzHat.Count; int nConstraints = fz.Count + fzHat.Count; int lp = lpsolve.make_lp((int)ignoreThisEntry, nDecision); //lpsolve.set_sense(lp, true); #endregion #region c-vector (objective function) double[] cObj = new double[nDecision + 1]; cObj[0] = ignoreThisEntry; int ic = 0; for (int i = 0; i < fz.Count; i++) { for (int j = 0; j < fzHat.Count; j++) { ic = ic + 1; cObj[ic] = -1000000.0 + ciGate(fz[i], Paug.Submatrix(0, 1, 0, 1), fzHat[j], Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4), 1.0); cLP[i, j] = cObj[ic]; fzM[0, 0] = fz[i].X; fzM[1, 0] = fz[i].Y; fzHatM[0, 0] = fzHat[j].X; fzHatM[1, 0] = fzHat[j].Y; dapGate[i, j] = ((fzM - fzHatM).Transpose() * Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4).Inverse *(fzM - fzHatM))[0, 0]; } } lpsolve.set_obj_fn(lp, cObj); //lpsolve.set_timeout(lp, 0); #endregion #region b-vector (RHS of LE) double[] bVec = new double[nConstraints]; for (int i = 0; i < bVec.Length; i++) { bVec[i] = 1.0; } #endregion #region A-matrix (constraints setup) double[,] A = new double[nConstraints, nDecision]; int jc = 0; for (int i = 0; i < fz.Count; i++) { int jr = 1; for (int j = 0; j < fzHat.Count; j++) { A[i, j + jc] = 1; A[fz.Count - 1 + jr, j + jc] = 1; jr = jr + 1; } jc = jc + fzHat.Count; } List <double[]> lpConstraints = new List <double[]>(); lpConstraints.Clear(); for (int i = 0; i < nConstraints; i++) { double[] Aline = new double[nDecision + 1]; Aline[0] = ignoreThisEntry; for (int j = 1; j < nDecision + 1; j++) { Aline[j] = A[i, j - 1]; } lpConstraints.Add(Aline); } for (int i = 0; i < nConstraints; i++) { lpsolve.add_constraint(lp, lpConstraints[i], lpsolve.lpsolve_constr_types.LE, bVec[i]); } #endregion #region optimization and results lpsolve.solve(lp); xOpt = new double[lpsolve.get_Ncolumns(lp)]; lpsolve.get_variables(lp, xOpt); lpsolve.delete_lp(lp); ic = 0; double tau = 6.63; for (int i = 0; i < fz.Count; i++) { for (int j = 0; j < fzHat.Count; j++) { if ((xOpt[ic] > 0.98) && (xOpt[ic] < 1.02) && (dapGate[i, j] < tau)) { jObsv.Add(new Vector3(i, j, cLP[i, j])); } ic = ic + 1; } } if (jObsv.Count > 0) { for (int j = 0; j < jObsv.Count; j++) { iObsv.Add(new Vector2(jObsv[j].X, jObsv[j].Y)); obsvFeatureList.Add(rotateTranslate(currentScanXY[ibook[(int)(jObsv[j].X)]], xhatvPose)); // dev-only } } numObsv = iObsv.Count; #endregion #endregion #region measurement-update if (numObsv > 0) { #region spf parameters double L = 3 + 2 * landmarkList.Count; double alpha = 1.0; double kappa = 0.0; double beta = 2.0; double lambda = Math.Pow(alpha, 2) * (L + kappa) - L; double gam = Math.Sqrt(L + lambda); double wm0 = lambda / (L + lambda); double wc0 = lambda / (L + lambda) + (1 - Math.Pow(alpha, 2) + beta); wm = new Matrix((int)(2 * L) + 1, 1); for (int j = 0; j < (int)(2 * L) + 1; j++) { if (j == 0) { wm[j, 0] = wm0; } else { wm[j, 0] = 1 / (2.0 * (L + lambda)); } } wc = new Matrix((int)(2 * L) + 1, 1); for (int j = 0; j < (int)(2.0 * L) + 1; j++) { if (j == 0) { wc[j, 0] = wc0; } else { wc[j, 0] = 1 / (2.0 * (L + lambda)); } } #endregion #region spf sampler CholeskyDecomposition PCholContainer = new CholeskyDecomposition(ScalarMultiply(L + lambda, Paug)); Matrix PChol = PCholContainer.LeftTriangularFactor; chi0 = new Matrix((int)L, 1); chi0[0, 0] = xhatvPose.x - uvk.x; chi0[1, 0] = xhatvPose.y - uvk.y; chi0[2, 0] = xhatvPose.yaw - uvk.yaw; for (int i = 0; i < mhat.Rows; i++) { chi0[3 + i, 0] = mhat[i, 0]; } chi1 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) { chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, chi0 + PChol.Submatrix(0, chi0.Rows - 1, i, i)); } chi2 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) { chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, chi0 - PChol.Submatrix(0, chi0.Rows - 1, i, i)); } chi = chi0; chi = chi.Concat(2, chi1); chi = chi.Concat(2, chi2); #endregion #region spf time-update Matrix chiBar = new Matrix((int)L, (int)(2 * L) + 1); Matrix uvkSPF = new Matrix((int)L, 1); uvkSPF.Zero(); uvkSPF[0, 0] = uvk.x; uvkSPF[1, 0] = uvk.y; uvkSPF[2, 0] = uvk.yaw; for (int i = 0; i < (int)(2 * L) + 1; i++) { chiBar.SetSubMatrix(0, chiBar.Rows - 1, i, i, chi.Submatrix(0, chi.Rows - 1, i, i) + uvkSPF); } Matrix xBar = chiBar * wm; Matrix PBar = new Matrix((int)L, (int)L); PBar.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) { PBar = PBar + ScalarMultiply(wc[i, 0], (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar) * (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar).Transpose()); } #endregion #region sigma-point update CholeskyDecomposition PBarCholContainer = new CholeskyDecomposition(PBar); Matrix PBarChol = PBarCholContainer.LeftTriangularFactor; chi0 = xBar; chi1 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) { chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, xBar + ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i))); } chi2 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) { chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, xBar - ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i))); } chiUpdate = chi0; chiUpdate = chiUpdate.Concat(2, chi1); chiUpdate = chiUpdate.Concat(2, chi2); #endregion #region spf measurement-update setup #region predicted measurement Matrix YBar = new Matrix(2 * numObsv, (int)(2 * L) + 1); YBar.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) { chiUpdateObsv = new Matrix((int)(2 * numObsv), 1); for (int j = 0; j < numObsv; j++) { chiUpdateObsv[2 * j, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 3), i]; chiUpdateObsv[2 * j + 1, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 1 + 3), i]; } YBar.SetSubMatrix(0, YBar.Rows - 1, i, i, hFunction(chiUpdate.Submatrix(0, 2, i, i), chiUpdateObsv)); } Matrix yBar = YBar * wm; #endregion #region actual measurement Matrix z = new Matrix(2 * numObsv, 1); for (int i = 0; i < numObsv; i++) { z[2 * i, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.R; z[2 * i + 1, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.theta; } #endregion #region innovation covariance S = new Matrix(2 * numObsv, 2 * numObsv); S.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) { S = S + ScalarMultiply(wc[i, 0], (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar) * (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose()); } S = S + new Matrix(2 * numObsv, 2 * numObsv, Math.Pow(sigv, 2)); #endregion #region Kalman gain matrix W = new Matrix((int)L, 2 * numObsv); W.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) { W = W + ScalarMultiply(wc[i, 0], (chiUpdate.Submatrix(0, chiUpdate.Rows - 1, i, i) - xBar) * (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose()); } W = W * S.Inverse; #endregion #region measurement wrapper zCart = new Matrix(2 * numObsv, 1); for (int i = 0; i < numObsv; i++) { zCart[2 * i, 0] = z[2 * i, 0] * Math.Cos(z[2 * i + 1, 0]); zCart[2 * i + 1, 0] = z[2 * i, 0] * Math.Sin(z[2 * i + 1, 0]); } for (int i = 0; i < numObsv; i++) { z[2 * i, 0] = Math.Sqrt(Math.Pow(zCart[2 * i, 0], 2) + Math.Pow(zCart[2 * i + 1, 0], 2)); z[2 * i + 1, 0] = Math.Atan2(zCart[2 * i + 1, 0], zCart[2 * i, 0]); } yBarCart = new Matrix(2 * numObsv, 1); for (int i = 0; i < numObsv; i++) { yBarCart[2 * i, 0] = yBar[2 * i, 0] * Math.Cos(yBar[2 * i + 1, 0]); yBarCart[2 * i + 1, 0] = yBar[2 * i, 0] * Math.Sin(yBar[2 * i + 1, 0]); } for (int i = 0; i < numObsv; i++) { yBar[2 * i, 0] = Math.Sqrt(Math.Pow(yBarCart[2 * i, 0], 2) + Math.Pow(yBarCart[2 * i + 1, 0], 2)); yBar[2 * i + 1, 0] = Math.Atan2(yBarCart[2 * i + 1, 0], yBarCart[2 * i, 0]); } #endregion #endregion #region spf measurement-update //(((z - yBar).Transpose() * S * (z - yBar))[0, 0] < 6.63) xBar = xBar + W * (z - yBar); Paug = PBar - W * S * W.Transpose(); #region SLAM-context of measurement update #region localization xhatvPose.x = xBar[0, 0]; xhatvPose.y = xBar[1, 0]; xhatvPose.yaw = xBar[2, 0]; Pv = Paug.Submatrix(0, 2, 0, 2); #endregion #region mapping mhat = xBar.Submatrix(3, (int)L - 1, 0, 0); int landmarkListCount = landmarkList.Count; landmarkList.Clear(); for (int i = 0; i < landmarkListCount; i++) { landmarkList.Add(new Vector2(mhat[2 * i, 0], mhat[2 * i + 1, 0])); } #endregion #endregion #endregion } #endregion #region show features slamFeatures.Clear(); if (ibook != null) { for (int i = 0; i < ibook.Count; i++) { slamFeatures.Add(fz[i]); } } #endregion #region map management double minDist = 0.5; double sigMapFreeze = 0.5; int landmarkMaxCount = 20; for (int i = 0; i < iObsv.Count; i++) { fz.RemoveAt((int)(iObsv[i].X - i)); } for (int i = 0; i < fz.Count; i++) { int nFlag = 0; for (int j = 0; j < landmarkList.Count; j++) { if ((fz[i] - landmarkList[j]).Length > minDist) { nFlag = nFlag + 1; } } if ((nFlag == landmarkList.Count) && (Pv.Trace < sigMapFreeze) && (landmarkList.Count - 1 <= landmarkMaxCount)) { landmarkList.Add(fz[i]); Paug = blkdiag(Paug, new Matrix(2, 2, (Pv.Submatrix(0, 1, 0, 1)).Trace)); Matrix mHatNew = new Matrix(2, 1); mHatNew[0, 0] = fz[i].X; mHatNew[1, 0] = fz[i].Y; mhat = mhat.Concat(1, mHatNew); break; // add one feature at a time } } #endregion #region show landmarks slamLandmarks.Clear(); slamLandmarksCovariance.Clear(); for (int i = 0; i < landmarkList.Count; i++) { slamLandmarks.Add(landmarkList[i]); slamLandmarksCovariance.Add(Paug.Submatrix(2 * i + 3, 2 * i + 4, 2 * i + 3, 2 * i + 4)); } #endregion #region show correspondences slamCorrespondences.Clear(); for (int i = 0; i < obsvFeatureList.Count; i++) { slamCorrespondences.Add(obsvFeatureList[i]); } #endregion #region broadcast the results viconLastPose = viconPose; //filteredPose = transformedOdomPose; filteredPose = xhatvPose; lastScan = currentScan; //covMatrix = Pv; // only vehicle xy-submatrix is used in the form covMatrix = Pv; // only vehicle xy-submatrix is used in the form //transformedOdomPose = viconPose; #endregion } } } Thread.Sleep(100); } }
public void ClearBuffer() { scan = null; }
/// <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 }
void sick_ScanReceived(object sender, ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > e) { scan = e.Scan; BuildHistogram(); }
/// <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 RobotTwoWheelCommand GetCommand(ILidarScan<ILidar2DPoint> sickScan, ILidarScan<ILidar2DPoint> hokuyoScan, int angleTolerance, double threshold, double dMax, double vMax, double wMax, out List<Polygon> polys) { double hysteresis = threshold * .9; List<Double[]> histograms = new List<double[]>(); // Transform scan to global coordinate and add to queue Vector2[] globalSickScan = TransformToGlobal(sickScan, sickDCM); sickHistory.Add(globalSickScan); histograms.Add(BuildHistogram(globalSickScan, dMax)); histograms.Add(BuildHistogram(sickHistory[0], dMax)); if (sickHistory.Count >= 30) sickHistory.RemoveAt(0); if (useHokuyo) { Vector2[] globalHokuyoScan = TransformToGlobal(hokuyoScan, hokuyoDCM); hokuyoHistory.Add(globalHokuyoScan); histograms.Add(BuildHistogram(globalHokuyoScan, dMax)); histograms.Add(BuildHistogram(hokuyoHistory[0], dMax)); if (hokuyoHistory.Count >= 30) hokuyoHistory.RemoveAt(0); } Double[] histogram = CombinedHistogram(histograms); for (int i = 0; i < numBuckets; i++) { if (rHistogram[i] >= threshold && histogram[i] > hysteresis && histogram[i] < threshold) histogram[i] = rHistogram[i]; } rHistogram = histogram; histogram = Smooth(histogram, 10); double goalAngle = 180 * (goalPoint.ArcTan) / Math.PI; int targetBucket = (int)(Math.Round(goalAngle / angularResolution)); List<double[]> valleys = FindValleys(histogram, threshold); polys = new List<Polygon>(); if (!trapped) { // pick out best candidate valley double[] valley = PickBestValley(valleys, targetBucket); // calculate steering angle double steerAngle = FindSteerAngle(valley, targetBucket); //degrees // calculate speed commands double vmin = 0; // (m/s) //double vmax = .2; // (m/s) //double wmax = 25; // (deg/s) double w = 0; double v = 0; if (Math.Abs(steerAngle) > angleTolerance) { w = wMax * Math.Sign(steerAngle); } else { w = wMax * (steerAngle / angleTolerance); } List<Vector2> points = new List<Vector2>(); for (int i = 0; i < numBuckets; i++) { points.Add(new Vector2(histogram[i] * Math.Cos(i * angularResolution * Math.PI / 180), histogram[i] * Math.Sin(i * angularResolution * Math.PI / 180))); } polys.Add(new Polygon(points)); points = new List<Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerAngle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerAngle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List<Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1 * Math.Cos(goalAngle * Math.PI / 180), threshold * 1 * Math.Sin(goalAngle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List<Vector2>(); for (int i = 0; i < 180; i++) { points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180))); } polys.Add(new Polygon(points)); double hc = Math.Min(histogram[0], threshold); double vp = vMax * (1 - hc / threshold); v = vp * (1 - Math.Abs(w) / wMax) + vmin; double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units" double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec) return new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand); } else // trapped (no valleys were found), so stop, or lift threshold return new RobotTwoWheelCommand(0, 0); }
public RobotTwoWheelCommand GetCommand(ILidarScan <ILidar2DPoint> sickScan, ILidarScan <ILidar2DPoint> hokuyoScan, int angleTolerance, double threshold, double dMax, double vMax, double wMax, out List <Polygon> polys) { double hysteresis = threshold * .9; List <Double[]> histograms = new List <double[]>(); // Transform scan to global coordinate and add to queue Vector2[] globalSickScan = TransformToGlobal(sickScan, sickDCM); sickHistory.Add(globalSickScan); histograms.Add(BuildHistogram(globalSickScan, dMax)); histograms.Add(BuildHistogram(sickHistory[0], dMax)); if (sickHistory.Count >= 30) { sickHistory.RemoveAt(0); } if (useHokuyo) { Vector2[] globalHokuyoScan = TransformToGlobal(hokuyoScan, hokuyoDCM); hokuyoHistory.Add(globalHokuyoScan); histograms.Add(BuildHistogram(globalHokuyoScan, dMax)); histograms.Add(BuildHistogram(hokuyoHistory[0], dMax)); if (hokuyoHistory.Count >= 30) { hokuyoHistory.RemoveAt(0); } } Double[] histogram = CombinedHistogram(histograms); for (int i = 0; i < numBuckets; i++) { if (rHistogram[i] >= threshold && histogram[i] > hysteresis && histogram[i] < threshold) { histogram[i] = rHistogram[i]; } } rHistogram = histogram; histogram = Smooth(histogram, 10); double goalAngle = 180 * (goalPoint.ArcTan) / Math.PI; int targetBucket = (int)(Math.Round(goalAngle / angularResolution)); List <double[]> valleys = FindValleys(histogram, threshold); polys = new List <Polygon>(); if (!trapped) { // pick out best candidate valley double[] valley = PickBestValley(valleys, targetBucket); // calculate steering angle double steerAngle = FindSteerAngle(valley, targetBucket); //degrees // calculate speed commands double vmin = 0; // (m/s) //double vmax = .2; // (m/s) //double wmax = 25; // (deg/s) double w = 0; double v = 0; if (Math.Abs(steerAngle) > angleTolerance) { w = wMax * Math.Sign(steerAngle); } else { w = wMax * (steerAngle / angleTolerance); } List <Vector2> points = new List <Vector2>(); for (int i = 0; i < numBuckets; i++) { points.Add(new Vector2(histogram[i] * Math.Cos(i * angularResolution * Math.PI / 180), histogram[i] * Math.Sin(i * angularResolution * Math.PI / 180))); } polys.Add(new Polygon(points)); points = new List <Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1.5 * Math.Cos(steerAngle * Math.PI / 180), threshold * 1.5 * Math.Sin(steerAngle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List <Vector2>(); points.Add(new Vector2(0, 0)); points.Add(new Vector2(threshold * 1 * Math.Cos(goalAngle * Math.PI / 180), threshold * 1 * Math.Sin(goalAngle * Math.PI / 180))); polys.Add(new Polygon(points)); points = new List <Vector2>(); for (int i = 0; i < 180; i++) { points.Add(new Vector2(threshold * Math.Cos(i * 2 * Math.PI / 180), threshold * Math.Sin(i * 2 * Math.PI / 180))); } polys.Add(new Polygon(points)); double hc = Math.Min(histogram[0], threshold); double vp = vMax * (1 - hc / threshold); v = vp * (1 - Math.Abs(w) / wMax) + vmin; double vcommand = v * 3.6509; // multiply by 3.6509 to get "segway units" double wcommand = w * 3.9; // convert to deg/sec and multiply by 3.9 for "segway units" (counts/deg/sec) return(new RobotTwoWheelCommand(vcommand * 2.23693629, wcommand)); } else // trapped (no valleys were found), so stop, or lift threshold { return(new RobotTwoWheelCommand(0, 0)); } }
private void ScanToTextFile(ILidarScan<ILidar2DPoint> LidarScan) { TextWriter Raw = new StreamWriter("C:\\Users\\labuser\\Desktop\\RawData.txt"); TextWriter Pts = new StreamWriter("C:\\Users\\labuser\\Desktop\\PointData.txt"); Raw.WriteLine(); Pts.WriteLine(); for (int i = 0; i < LidarScan.Points.Count; i++) { Raw.Write(LidarScan.Points[i].RThetaPoint.R); Raw.Write(" "); Raw.WriteLine(LidarScan.Points[i].RThetaPoint.theta); Pts.WriteLine(LidarScan.Points[i].RThetaPoint.ToVector2().ToString()); } Raw.Close(); Pts.Close(); }
public void UpdateLidarScan(ILidarScan <ILidar2DPoint> s) { lock (dataLocker) { Stopwatch sw = new Stopwatch(); sw.Start(); curScan2D = s; if (curRobotPose != null && curScan2D != null /*&& curRobotPose.timestamp != priorTimeStamp && curRobotPose.timestamp != 0*/) { //Get the laser to body coordinate system (this is I for now) Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody); //Get the body to global transformation Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose); //Get a vector from the current lidar pose Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z); //Transform the sensor position in body coordinates to the sensor position in global coordinates Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody); //Get the current grid indicies int xLidarPoseIndex, yLidarPoseIndex; currentOccupancyGrid.GetIndicies(lidarInGlobal.X, lidarInGlobal.Y, out xLidarPoseIndex, out yLidarPoseIndex); //Find the cells corresponding to each LIDAR return and make a list of the cells that are clear from the sensor to that point Dictionary <Vector2, Boolean> occupiedCellsThisScan = new Dictionary <Vector2, Boolean>(curScan2D.Points.Count()); Dictionary <Vector2, Boolean> clearCellsThisScan = new Dictionary <Vector2, Boolean>(); //Process each lidar return foreach (ILidar2DPoint pt in curScan2D.Points) { if (pt.RThetaPoint.R < lidarMaxPracticalRange) { //Extract the lidar point in XYZ (laser coordinates) Vector3 v3 = pt.RThetaPoint.ToVector3(); //Convert laser to body coordinate system Vector3 vBody = Tlaser2body.TransformPoint(v3); //Convert body to global cooridnate system Vector3 vGlobal = Tbody2global.TransformPoint(vBody); //Find the index of the laser return int xLaserIndex, yLaserIndex; currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex); //Add to the list of occupied cells if (currentOccupancyGrid.CheckValidIdx(xLaserIndex, yLaserIndex)) { occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex)] = true; //occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex)] = true; //occupiedCellsThisScan[new Vector2(xLaserIndex, yLaserIndex - 1)] = true; //occupiedCellsThisScan[new Vector2(xLaserIndex + 1, yLaserIndex - 1)] = true; } } } //Process each lidar return foreach (ILidar2DPoint pt in curScan2D.Points) { if (pt.RThetaPoint.R < lidarMaxPracticalRange) { //Extract the lidar point in XYZ (laser coordinates) Vector3 v3 = pt.RThetaPoint.ToVector3(); //Convert laser to body coordinate system Vector3 vBody = Tlaser2body.TransformPoint(v3); //Convert body to global cooridnate system Vector3 vGlobal = Tbody2global.TransformPoint(vBody); //Find the index of the laser return int xLaserIndex, yLaserIndex; currentOccupancyGrid.GetIndicies(vGlobal.X, vGlobal.Y, out xLaserIndex, out yLaserIndex); //Ray trace between the two points performing the update Raytrace(xLidarPoseIndex, yLidarPoseIndex, xLaserIndex, yLaserIndex, occupiedCellsThisScan, clearCellsThisScan); } } //decay the whole grid for (int i = 0; i < currentOccupancyGrid.NumCellX; i++) { for (int j = 0; j < currentOccupancyGrid.NumCellY; j++) { double value = currentOccupancyGrid.GetCellByIdx(i, j); currentOccupancyGrid.SetCellByIdx(i, j, value *= gridDecay); } } foreach (Vector2 cellIdx in occupiedCellsThisScan.Keys) { UpdateCellOccupied((int)cellIdx.X, (int)cellIdx.Y); } foreach (Vector2 cellIdx in clearCellsThisScan.Keys) { UpdateCellClear((int)cellIdx.X, (int)cellIdx.Y); } //Copy for the timestamp for the next iteration priorTimeStamp = (double)curRobotPose.timestamp; } // Console.WriteLine("OG Took: " + sw.ElapsedMilliseconds); }//lock if (outputOccupancyGrid != null) { outputOccupancyGrid = (IOccupancyGrid2D)currentOccupancyGrid.DeepCopy(); if (curRobotPose != null) { if (NewGridAvailable != null) { NewGridAvailable(this, new NewOccupancyGrid2DAvailableEventArgs(GetOccupancyGrid(), curRobotPose.timestamp)); } } } }
public List<Polygon> SegmentScan(ILidarScan<ILidar2DPoint> scan) { //this is super stupid, but may be functional for now.... //it may be necessary to sort these List<Polygon> clusters = new List<Polygon> (); List<Vector2> pts = new List<Vector2>(scan.Points.Count); //Get the laser to body coordinate system (this is I for now) Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody); //Get the body to global transformation Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose); //Get a vector from the current lidar pose Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z); //Transform the sensor position in body coordinates to the sensor position in global coordinates Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody); foreach (ILidar2DPoint pt in scan.Points) { if (pt.RThetaPoint.R < lidarMaxPracticalRange) { //Extract the lidar point in XYZ (laser coordinates) Vector3 v3 = pt.RThetaPoint.ToVector3(); //Convert laser to body coordinate system Vector3 vBody = Tlaser2body.TransformPoint(v3); //Convert body to global cooridnate system Vector3 vGlobal = Tbody2global.TransformPoint(vBody); pts.Add(new Vector2 (vGlobal.X, vGlobal.Y)); } } //int slidingWindow = 5; Polygon p = new Polygon (); p.Add (pts[0]); for (int i = 1; i < pts.Count; i++) { if (Math.Abs(pts[i].DistanceTo(pts[i - 1])) < threshold) p.Add(pts[i]); else { //create a new poly clusters.Add(p); p = new Polygon(); p.Add(pts[i]); } } clusters.Add(p); //add the last polygon Console.WriteLine("Got " + clusters.Count + " polys"); Polygon vehiclePolygon = VehiclePolygonWithRadius(vehicleRadius); // vehicle model of polygon. Note that radius is parameter // Inside foreach, I cannot reassign eachpolygon. So, make a List<Polygon>, assign into this, and return this List<Polygon> ret = new List<Polygon>(clusters.Count); foreach (Polygon eachPolygon in clusters) { // Do the MinKowskiConvolution and GrahamScan to remove interior points //Polygon convolutionPolygon = Polygon.ConvexMinkowskiConvolution(eachPolygon, vehiclePolygon); Polygon convexHull; if (eachPolygon.Count > 1) { convexHull = Polygon.GrahamScan(eachPolygon, .001); } else { convexHull = eachPolygon; } Polygon bloated = Polygon.ConvexMinkowskiConvolution(convexHull, vehiclePolygon); //convexHull.Add(convexHull[0]); ret.Add(bloated); //convolutionPolygonList.Add(eachPolygon); } // convolutionPolygonList = DecomposePolygonList(convolutionPolygonList); return ret; }
public List <Polygon> SegmentScan(ILidarScan <ILidar2DPoint> scan) { //this is super stupid, but may be functional for now.... //it may be necessary to sort these List <Polygon> clusters = new List <Polygon> (); List <Vector2> pts = new List <Vector2>(scan.Points.Count); //Get the laser to body coordinate system (this is I for now) Matrix4 Tlaser2body = Matrix4.FromPose(curLidarToBody); //Get the body to global transformation Matrix4 Tbody2global = Matrix4.FromPose(curRobotPose); //Get a vector from the current lidar pose Vector3 lidarInBody = new Vector3((double)curLidarToBody.x, (double)curLidarToBody.y, (double)curLidarToBody.z); //Transform the sensor position in body coordinates to the sensor position in global coordinates Vector3 lidarInGlobal = Tbody2global.TransformPoint(lidarInBody); foreach (ILidar2DPoint pt in scan.Points) { if (pt.RThetaPoint.R < lidarMaxPracticalRange) { //Extract the lidar point in XYZ (laser coordinates) Vector3 v3 = pt.RThetaPoint.ToVector3(); //Convert laser to body coordinate system Vector3 vBody = Tlaser2body.TransformPoint(v3); //Convert body to global cooridnate system Vector3 vGlobal = Tbody2global.TransformPoint(vBody); pts.Add(new Vector2(vGlobal.X, vGlobal.Y)); } } //int slidingWindow = 5; Polygon p = new Polygon(); p.Add(pts[0]); for (int i = 1; i < pts.Count; i++) { if (Math.Abs(pts[i].DistanceTo(pts[i - 1])) < threshold) { p.Add(pts[i]); } else { //create a new poly clusters.Add(p); p = new Polygon(); p.Add(pts[i]); } } clusters.Add(p); //add the last polygon Console.WriteLine("Got " + clusters.Count + " polys"); Polygon vehiclePolygon = VehiclePolygonWithRadius(vehicleRadius); // vehicle model of polygon. Note that radius is parameter // Inside foreach, I cannot reassign eachpolygon. So, make a List<Polygon>, assign into this, and return this List <Polygon> ret = new List <Polygon>(clusters.Count); foreach (Polygon eachPolygon in clusters) { // Do the MinKowskiConvolution and GrahamScan to remove interior points //Polygon convolutionPolygon = Polygon.ConvexMinkowskiConvolution(eachPolygon, vehiclePolygon); Polygon convexHull; if (eachPolygon.Count > 1) { convexHull = Polygon.GrahamScan(eachPolygon, .001); } else { convexHull = eachPolygon; } Polygon bloated = Polygon.ConvexMinkowskiConvolution(convexHull, vehiclePolygon); //convexHull.Add(convexHull[0]); ret.Add(bloated); //convolutionPolygonList.Add(eachPolygon); } // convolutionPolygonList = DecomposePolygonList(convolutionPolygonList); return(ret); }
public void SetScan(ILidarScan<ILidar2DPoint> scan, RobotPose robotPose, int startIdx, int endIdx) { lock (this.drawLock) { this.scan = scan; this.time = scan.Timestamp; this.numPoints = endIdx - startIdx + 1; this.robotPose = robotPose; this.startIdx = startIdx; this.endIdx = endIdx; } }
private void lidar_ScanReceived(object sender, ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > e) { currentData = e.Scan; }
public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan) { this.robotID = robotID; this.state = state; this.lidarScan = scan; this.sensorPose = sensorPose; }
public static 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; }
void SLAMEKFMainLoop(object o) { k = 0; while (running) { lock (dataLock) { if (currentScan == null || viconPose == null || odomPose == null || viconLastPose == viconPose) continue; // no data, no update else { #region discrete-time increment k = k + 1; #endregion if (k == 1) { #region state initialization initialOdomPose = odomPose; transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp); transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.yaw = odomPose.yaw; transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose; odomLastPose = transformedOdomPose; xhatvPose = transformedOdomPose; #endregion #region map initialization and management #region extract features -> fz List<Vector2> currentScanXY = new List<Vector2>(); fz.Clear(); ibook = fExtract(currentScan, bAngle, maxRange); if (ibook != null) { for (int i = 0; i < currentScan.Points.Count; i++) currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2()); for (int i = 0; i < ibook.Count; i++) fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose)); } #endregion #region populate the map -> landmarkList landmarkList.Clear(); for (int i = 0; i < fz.Count; i++) landmarkList.Add(fz[i]); #endregion #region populate the map -> mhat mhat = new Matrix(2 * landmarkList.Count, 1); for (int i = 0; i < landmarkList.Count; i++) { mhat[2 * i, 0] = landmarkList[i].X; mhat[2 * i + 1, 0] = landmarkList[i].Y; } #endregion #region covariance management -> Paug Qv = new Matrix(3, 3, Math.Pow(sigw, 2)); Pv = Qv; Paug = Pv; for (int i = 0; i < fz.Count; i++) Paug = blkdiag(Paug, new Matrix(2, 2, Math.Pow(sigw, 2))); #endregion lastScan = currentScan; #endregion } else { #region odometry processing transformedOdomPose = new RobotPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, odomPose.timestamp); transformedOdomPose.x = (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x - (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.y = (Math.Sin(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.x + (Math.Cos(initialViconPose.yaw - initialOdomPose.yaw)) * odomPose.y; transformedOdomPose.yaw = odomPose.yaw; transformedOdomPose = transformedOdomPose - initialOdomPose + initialViconPose; xStep = transformedOdomPose - odomLastPose; double omega = odomLastPose.yaw + xStep.yaw; if (xStep.x == 0) uvk.x = 0; else { double vx = xStep.x / Math.Cos(omega); uvk.x = vx * Math.Cos(omega); } if (xStep.y == 0) uvk.y = 0; else { double vy = xStep.y / Math.Sin(omega); uvk.y = vy * Math.Sin(omega); } uvk.yaw = xStep.yaw; uvk.z = 0; uvk.roll = 0; uvk.pitch = 0; uvk.timestamp = odomPose.timestamp; odomLastPose = transformedOdomPose; #endregion #region time-update xhatvPose = xhatvPose + uvk; Matrix Uvk = new Matrix(3, 1); Uvk[0, 0] = uvk.x; Uvk[1, 0] = uvk.y; Uvk[2, 0] = uvk.yaw; Pv = Pv + ScalarMultiply(Math.Pow(0.5,2),Uvk * Uvk.Transpose()) + Qv; Paug.SetSubMatrix(0, 2, 0, 2, Pv); Matrix Qmv = new Matrix(0, 0, 1.0); for (int i = 0; i < landmarkList.Count; i++) Qmv = blkdiag(Qmv, ScalarMultiply(Math.Pow(sigw / 5, 2), Pv.Submatrix(0, 1, 0, 1))); Matrix Qm = new Matrix(Paug.Rows - 3, Paug.Columns - 3, Math.Pow(sigw / 5, 2)); Paug.SetSubMatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1, Paug.Submatrix(3, Paug.Rows - 1, 3, Paug.Columns - 1) + Qmv + Qm); #endregion #region feature exraction numLandmarks = landmarkList.Count; List<Vector2> currentScanXY = new List<Vector2>(); fz.Clear(); ibook = fExtract(currentScan, bAngle, maxRange); if (ibook != null) { for (int i = 0; i < currentScan.Points.Count; i++) currentScanXY.Add(currentScan.Points[i].RThetaPoint.ToVector2()); for (int i = 0; i < ibook.Count; i++) fz.Add(rotateTranslate(currentScanXY[ibook[i]], xhatvPose)); } #endregion #region data association #region variables fzHat = new List<Vector2>(landmarkList); List<Vector3> jObsv = new List<Vector3>(); List<Vector2> iObsv = new List<Vector2>(); List<Vector2> obsvFeatureList = new List<Vector2>(); Matrix cLP = new Matrix(fz.Count, fzHat.Count); Matrix dapGate = new Matrix(fz.Count, fzHat.Count); Matrix fzM = new Matrix(2,1); Matrix fzHatM = new Matrix(2,1); #endregion #region initialization // "lpsolver55.dll" directory: // (note: Alt + F7 -> Build -> enable "Allow UnSafe Code") lpsolve.Init("C:\\users\\labuser\\desktop\\MAGIC\\Framework\\SLAM"); double[] xOpt; double ignoreThisEntry = 0; int nDecision = fz.Count * fzHat.Count; int nConstraints = fz.Count + fzHat.Count; int lp = lpsolve.make_lp((int)ignoreThisEntry, nDecision); //lpsolve.set_sense(lp, true); #endregion #region c-vector (objective function) double[] cObj = new double[nDecision + 1]; cObj[0] = ignoreThisEntry; int ic = 0; for (int i = 0; i < fz.Count; i++) { for (int j = 0; j < fzHat.Count; j++) { ic = ic + 1; cObj[ic] = -1000000.0 + ciGate(fz[i], Paug.Submatrix(0, 1, 0, 1), fzHat[j], Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4), 1.0); cLP[i, j] = cObj[ic]; fzM[0,0] = fz[i].X; fzM[1,0] = fz[i].Y; fzHatM[0,0] = fzHat[j].X; fzHatM[1,0] = fzHat[j].Y; dapGate[i, j] = ((fzM - fzHatM).Transpose() * Paug.Submatrix(2 * j + 3, 2 * j + 4, 2 * j + 3, 2 * j + 4).Inverse * (fzM - fzHatM))[0, 0]; } } lpsolve.set_obj_fn(lp, cObj); //lpsolve.set_timeout(lp, 0); #endregion #region b-vector (RHS of LE) double[] bVec = new double[nConstraints]; for (int i = 0; i < bVec.Length; i++) bVec[i] = 1.0; #endregion #region A-matrix (constraints setup) double[,] A = new double[nConstraints, nDecision]; int jc = 0; for (int i = 0; i < fz.Count; i++) { int jr = 1; for (int j = 0; j < fzHat.Count; j++) { A[i, j + jc] = 1; A[fz.Count - 1 + jr, j + jc] = 1; jr = jr + 1; } jc = jc + fzHat.Count; } List<double[]> lpConstraints = new List<double[]>(); lpConstraints.Clear(); for (int i = 0; i < nConstraints; i++) { double[] Aline = new double[nDecision + 1]; Aline[0] = ignoreThisEntry; for (int j = 1; j < nDecision + 1; j++) { Aline[j] = A[i, j - 1]; } lpConstraints.Add(Aline); } for (int i = 0; i < nConstraints; i++) lpsolve.add_constraint(lp, lpConstraints[i], lpsolve.lpsolve_constr_types.LE, bVec[i]); #endregion #region optimization and results lpsolve.solve(lp); xOpt = new double[lpsolve.get_Ncolumns(lp)]; lpsolve.get_variables(lp, xOpt); lpsolve.delete_lp(lp); ic = 0; double tau = 6.63; for (int i = 0; i < fz.Count; i++) { for (int j = 0; j < fzHat.Count; j++) { if ((xOpt[ic] > 0.98) && (xOpt[ic] < 1.02) && (dapGate[i, j] < tau)) jObsv.Add(new Vector3(i, j, cLP[i, j])); ic = ic + 1; } } if (jObsv.Count > 0) { for (int j = 0; j < jObsv.Count; j++) { iObsv.Add(new Vector2(jObsv[j].X, jObsv[j].Y)); obsvFeatureList.Add(rotateTranslate(currentScanXY[ibook[(int)(jObsv[j].X)]], xhatvPose)); // dev-only } } numObsv = iObsv.Count; #endregion #endregion #region measurement-update if (numObsv > 0) { #region spf parameters double L = 3 + 2 * landmarkList.Count; double alpha = 1.0; double kappa = 0.0; double beta = 2.0; double lambda = Math.Pow(alpha, 2) * (L + kappa) - L; double gam = Math.Sqrt(L + lambda); double wm0 = lambda / (L + lambda); double wc0 = lambda / (L + lambda) + (1 - Math.Pow(alpha, 2) + beta); wm = new Matrix((int)(2 * L) + 1, 1); for (int j = 0; j < (int)(2 * L) + 1; j++) { if (j == 0) wm[j, 0] = wm0; else wm[j, 0] = 1 / (2.0 * (L + lambda)); } wc = new Matrix((int)(2 * L) + 1, 1); for (int j = 0; j < (int)(2.0 * L) + 1; j++) { if (j == 0) wc[j, 0] = wc0; else wc[j, 0] = 1 / (2.0 * (L + lambda)); } #endregion #region spf sampler CholeskyDecomposition PCholContainer = new CholeskyDecomposition(ScalarMultiply(L + lambda, Paug)); Matrix PChol = PCholContainer.LeftTriangularFactor; chi0 = new Matrix((int)L, 1); chi0[0, 0] = xhatvPose.x - uvk.x; chi0[1, 0] = xhatvPose.y - uvk.y; chi0[2, 0] = xhatvPose.yaw - uvk.yaw; for (int i = 0; i < mhat.Rows; i++) chi0[3 + i, 0] = mhat[i, 0]; chi1 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, chi0 + PChol.Submatrix(0, chi0.Rows - 1, i, i)); chi2 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, chi0 - PChol.Submatrix(0, chi0.Rows - 1, i, i)); chi = chi0; chi = chi.Concat(2, chi1); chi = chi.Concat(2, chi2); #endregion #region spf time-update Matrix chiBar = new Matrix((int)L, (int)(2 * L) + 1); Matrix uvkSPF = new Matrix((int)L, 1); uvkSPF.Zero(); uvkSPF[0, 0] = uvk.x; uvkSPF[1, 0] = uvk.y; uvkSPF[2, 0] = uvk.yaw; for (int i = 0; i < (int)(2 * L) + 1; i++) chiBar.SetSubMatrix(0, chiBar.Rows - 1, i, i, chi.Submatrix(0, chi.Rows - 1, i, i) + uvkSPF); Matrix xBar = chiBar * wm; Matrix PBar = new Matrix((int)L, (int)L); PBar.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) PBar = PBar + ScalarMultiply(wc[i, 0], (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar) * (chiBar.Submatrix(0, chiBar.Rows - 1, i, i) - xBar).Transpose()); #endregion #region sigma-point update CholeskyDecomposition PBarCholContainer = new CholeskyDecomposition(PBar); Matrix PBarChol = PBarCholContainer.LeftTriangularFactor; chi0 = xBar; chi1 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) chi1.SetSubMatrix(0, chi1.Rows - 1, i, i, xBar + ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i))); chi2 = new Matrix((int)L, (int)L); for (int i = 0; i < (int)L; i++) chi2.SetSubMatrix(0, chi2.Rows - 1, i, i, xBar - ScalarMultiply(gam, PBarChol.Submatrix(0, chi0.Rows - 1, i, i))); chiUpdate = chi0; chiUpdate = chiUpdate.Concat(2, chi1); chiUpdate = chiUpdate.Concat(2, chi2); #endregion #region spf measurement-update setup #region predicted measurement Matrix YBar = new Matrix(2 * numObsv, (int)(2 * L) + 1); YBar.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) { chiUpdateObsv = new Matrix((int)(2 * numObsv), 1); for (int j = 0; j < numObsv; j++) { chiUpdateObsv[2 * j, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 3), i]; chiUpdateObsv[2 * j + 1, 0] = chiUpdate[(int)(2 * iObsv[j].Y + 1 + 3), i]; } YBar.SetSubMatrix(0, YBar.Rows - 1, i, i, hFunction(chiUpdate.Submatrix(0, 2, i, i), chiUpdateObsv)); } Matrix yBar = YBar * wm; #endregion #region actual measurement Matrix z = new Matrix(2 * numObsv, 1); for (int i = 0; i < numObsv; i++) { z[2 * i, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.R; z[2 * i + 1, 0] = currentScan.Points[ibook[(int)(iObsv[i].X)]].RThetaPoint.theta; } #endregion #region innovation covariance S = new Matrix(2 * numObsv, 2 * numObsv); S.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) S = S + ScalarMultiply(wc[i, 0], (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar) * (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose()); S = S + new Matrix(2 * numObsv, 2 * numObsv, Math.Pow(sigv, 2)); #endregion #region Kalman gain matrix W = new Matrix((int)L, 2 * numObsv); W.Zero(); for (int i = 0; i < (int)(2 * L) + 1; i++) W = W + ScalarMultiply(wc[i, 0], (chiUpdate.Submatrix(0, chiUpdate.Rows - 1, i, i) - xBar) * (YBar.Submatrix(0, YBar.Rows - 1, i, i) - yBar).Transpose()); W = W * S.Inverse; #endregion #region measurement wrapper zCart = new Matrix(2 * numObsv, 1); for (int i = 0; i < numObsv; i++) { zCart[2 * i, 0] = z[2 * i, 0] * Math.Cos(z[2 * i + 1, 0]); zCart[2 * i + 1, 0] = z[2 * i, 0] * Math.Sin(z[2 * i + 1, 0]); } for (int i = 0; i < numObsv; i++) { z[2 * i, 0] = Math.Sqrt(Math.Pow(zCart[2 * i, 0], 2) + Math.Pow(zCart[2 * i + 1, 0], 2)); z[2 * i + 1, 0] = Math.Atan2(zCart[2 * i + 1, 0], zCart[2 * i, 0]); } yBarCart = new Matrix(2 * numObsv, 1); for (int i = 0; i < numObsv; i++) { yBarCart[2 * i, 0] = yBar[2 * i, 0] * Math.Cos(yBar[2 * i + 1, 0]); yBarCart[2 * i + 1, 0] = yBar[2 * i, 0] * Math.Sin(yBar[2 * i + 1, 0]); } for (int i = 0; i < numObsv; i++) { yBar[2 * i, 0] = Math.Sqrt(Math.Pow(yBarCart[2 * i, 0], 2) + Math.Pow(yBarCart[2 * i + 1, 0], 2)); yBar[2 * i + 1, 0] = Math.Atan2(yBarCart[2 * i + 1, 0], yBarCart[2 * i, 0]); } #endregion #endregion #region spf measurement-update //(((z - yBar).Transpose() * S * (z - yBar))[0, 0] < 6.63) xBar = xBar + W * (z - yBar); Paug = PBar - W * S * W.Transpose(); #region SLAM-context of measurement update #region localization xhatvPose.x = xBar[0, 0]; xhatvPose.y = xBar[1, 0]; xhatvPose.yaw = xBar[2, 0]; Pv = Paug.Submatrix(0, 2, 0, 2); #endregion #region mapping mhat = xBar.Submatrix(3, (int)L - 1, 0, 0); int landmarkListCount = landmarkList.Count; landmarkList.Clear(); for (int i = 0; i < landmarkListCount; i++) landmarkList.Add(new Vector2(mhat[2 * i, 0], mhat[2 * i + 1, 0])); #endregion #endregion #endregion } #endregion #region show features slamFeatures.Clear(); if (ibook != null) { for (int i = 0; i < ibook.Count; i++) slamFeatures.Add(fz[i]); } #endregion #region map management double minDist = 0.5; double sigMapFreeze = 0.5; int landmarkMaxCount = 20; for (int i = 0; i < iObsv.Count; i++) fz.RemoveAt((int)(iObsv[i].X - i)); for (int i = 0; i < fz.Count; i++) { int nFlag = 0; for (int j = 0; j < landmarkList.Count; j++) { if ((fz[i] - landmarkList[j]).Length > minDist) nFlag = nFlag + 1; } if ((nFlag == landmarkList.Count) && (Pv.Trace < sigMapFreeze) && (landmarkList.Count - 1 <= landmarkMaxCount)) { landmarkList.Add(fz[i]); Paug = blkdiag(Paug, new Matrix(2, 2, (Pv.Submatrix(0, 1, 0, 1)).Trace)); Matrix mHatNew = new Matrix(2,1); mHatNew[0, 0] = fz[i].X; mHatNew[1, 0] = fz[i].Y; mhat = mhat.Concat(1, mHatNew); break; // add one feature at a time } } #endregion #region show landmarks slamLandmarks.Clear(); slamLandmarksCovariance.Clear(); for (int i = 0; i < landmarkList.Count; i++) { slamLandmarks.Add(landmarkList[i]); slamLandmarksCovariance.Add(Paug.Submatrix(2 * i + 3, 2 * i + 4, 2 * i + 3, 2 * i + 4)); } #endregion #region show correspondences slamCorrespondences.Clear(); for (int i = 0; i < obsvFeatureList.Count; i++) slamCorrespondences.Add(obsvFeatureList[i]); #endregion #region broadcast the results viconLastPose = viconPose; //filteredPose = transformedOdomPose; filteredPose = xhatvPose; lastScan = currentScan; //covMatrix = Pv; // only vehicle xy-submatrix is used in the form covMatrix = Pv; // only vehicle xy-submatrix is used in the form //transformedOdomPose = viconPose; #endregion } } } Thread.Sleep(100); } }
public LidarScanMessage(int robotID, ILidarScan<ILidar2DPoint> scan) { this.scan = scan; this.robotID = robotID; }
/// <summary> /// Returns lidar scan projection into a image plane /// </summary> /// <param name="lidarScan"></param> /// <param name="cameraSize">"320x240" or "640x480"</param> /// <param name="camType">"Fire-i" or "FireFly"</param> /// <param name="camPose">Your camera pose relative to the lidar</param> /// <returns>List of pixel values for each lidar point</returns> public static List<Vector2> FindLidarProjection(ILidarScan<ILidar2DPoint> lidarScan, string cameraSize, string camType, SensorPose camPose) { 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); // 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); 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])); } return pixelList; }