/// <summary> /// /// </summary> /// <param name="goalPoint"></param> /// <param name="obstacles"></param> /// <param name="goal"></param> /// <param name="foundPath"></param> /// <param name="samplePoint"></param> /// <param name="closestNode"></param> /// <returns>true if the node was added to the tree (i.e. didnt hit crap)</returns> private RRTNode ExtendNode(ref Vector2 goalPoint, List <Polygon> obstacles, ref RRTNode goal, ref bool foundPath, ref Vector2 samplePoint, RRTNode closestNode, Random rand) { //3) generate a control input that drives towards the sample point also biased with our initial control inputs //3a) -Biasing Details: // Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity // Select Turn Rate: // Apply the following heuristic: mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT) // sigma = SigmaTurnRate // velocity distribution MathNet.Numerics.Distributions.NormalDistribution vDist = new MathNet.Numerics.Distributions.NormalDistribution(closestNode.State.Command.velocity, vSigma); // turn-rate biased double mixingSample = rand.NextDouble(); double wMean = 0; if (mixingSample > mixingProportion) { double angleToClosestNode = Math.Atan2((samplePoint.Y - closestNode.Point.Y), (samplePoint.X - closestNode.Point.X)); //wMean = -kPwSample * angleToClosestNode; wMean = ((angleToClosestNode - closestNode.State.Pose.yaw)) * 180.0 / Math.PI / timeStep; if (wMean > MAX_TURN - 20) { wMean = MAX_TURN - 20; } if (wMean < MIN_TURN + 20) { wMean = MIN_TURN + 20; } } else { wMean = 0; } MathNet.Numerics.Distributions.NormalDistribution wDist = new MathNet.Numerics.Distributions.NormalDistribution(wMean, wSigma); double velSampled = vDist.NextDouble(); double wSampled = wDist.NextDouble(); while (velSampled > MAX_VEL || velSampled < MIN_VEL) { velSampled = vDist.NextDouble(); } while (wSampled > MAX_TURN || wSampled < MIN_TURN) { wSampled = wDist.NextDouble(); } // 4) Predict a node RRTNode predictedNode = CalculateNextNode(closestNode, velSampled, wSampled, obstacles); if (predictedNode != null) { closestNode.AddChild(predictedNode); //5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1. //Polygon goalPolygon = Polygon.VehiclePolygonWithRadius(0.5, goalPoint); Circle c = new Circle(.5, goalPoint); LineSegment nodeToParent = new LineSegment(predictedNode.Point, predictedNode.Parent.Point); Vector2[] pts = new Vector2[2]; if (c.Intersect(nodeToParent, out pts)) { foundPath = true; goal = predictedNode; } //if (predictedNode.DistanceTo(goalPoint) < goalTolerance) //{ // foundPath = true; // goal = predictedNode; //} return(predictedNode); } return(null); }
private Matrix MeasurementFct(Matrix x_POI, Matrix x_x2, Matrix x_SCR_noise, Matrix rangeNoise, string cameraType, string size) { Matrix Z_SCR = new Matrix(x_SCR_noise.Rows, x_POI.Columns); MathNet.Numerics.Distributions.NormalDistribution normDist = new MathNet.Numerics.Distributions.NormalDistribution(0, 2.0); string newSize = "320x240"; if (size.Equals("960x240")) newSize = "320x240"; else newSize = size; for (int i = 0; i < x_POI.Columns; i++) { Matrix xNAV = x_x2.Submatrix(0, 2, i, i); Matrix xATT = x_x2.Submatrix(3, 5, i, i); Matrix xGIM = x_x2.Submatrix(6, 8, i, i); Z_SCR.SetSubMatrix(0, x_SCR_noise.Rows - 2, i, i, Point2Pixel(x_POI.Submatrix(0, x_POI.Rows - 1, i, i), xNAV, xATT, xGIM, x_SCR_noise.Submatrix(0, x_SCR_noise.Rows - 2, i, i), cameraType, newSize)); double laser = Math.Sqrt((x_POI[0, i] - xNAV[0, 0]) * (x_POI[0, i] - xNAV[0, 0]) + (x_POI[1, i] - xNAV[1, 0]) * (x_POI[1, i] - xNAV[1, 0]) + (x_POI[2, i] - xNAV[2, 0]) * (x_POI[2, i] - xNAV[2, 0]));// +normDist.NextDouble() * rangeNoise[0, 0]; Z_SCR[2, i] = laser; } return Z_SCR; }
/// <summary> /// /// </summary> /// <param name="goalPoint"></param> /// <param name="obstacles"></param> /// <param name="goal"></param> /// <param name="foundPath"></param> /// <param name="samplePoint"></param> /// <param name="closestNode"></param> /// <returns>true if the node was added to the tree (i.e. didnt hit crap)</returns> private RRTNode ExtendNode(ref Vector2 goalPoint, List<Polygon> obstacles, ref RRTNode goal, ref bool foundPath, ref Vector2 samplePoint, RRTNode closestNode, Random rand) { //3) generate a control input that drives towards the sample point also biased with our initial control inputs //3a) -Biasing Details: // Select Velocity: Normal Distribution with mean = closest node velocity and sigma = SigmaVelocity // Select Turn Rate: // Apply the following heuristic: mean = (atan2(yf-yi,xf-xi) - thetaInit)/(delT) // sigma = SigmaTurnRate // velocity distribution MathNet.Numerics.Distributions.NormalDistribution vDist = new MathNet.Numerics.Distributions.NormalDistribution(closestNode.State.Command.velocity, vSigma); // turn-rate biased double mixingSample = rand.NextDouble(); double wMean = 0; if (mixingSample > mixingProportion) { double angleToClosestNode = Math.Atan2((samplePoint.Y - closestNode.Point.Y), (samplePoint.X - closestNode.Point.X)); //wMean = -kPwSample * angleToClosestNode; wMean = ((angleToClosestNode - closestNode.State.Pose.yaw)) * 180.0 / Math.PI / timeStep; if (wMean > MAX_TURN - 20) wMean = MAX_TURN - 20; if (wMean < MIN_TURN + 20) wMean = MIN_TURN + 20; } else wMean = 0; MathNet.Numerics.Distributions.NormalDistribution wDist = new MathNet.Numerics.Distributions.NormalDistribution(wMean, wSigma); double velSampled = vDist.NextDouble(); double wSampled = wDist.NextDouble(); while (velSampled > MAX_VEL || velSampled < MIN_VEL) velSampled = vDist.NextDouble(); while (wSampled > MAX_TURN || wSampled < MIN_TURN) wSampled = wDist.NextDouble(); // 4) Predict a node RRTNode predictedNode = CalculateNextNode(closestNode, velSampled, wSampled, obstacles); if (predictedNode != null) { closestNode.AddChild(predictedNode); //5) Check if the new node added is within some tolerance of the goal node. If so, mark node as goal and you're done! Else, Goto 1. //Polygon goalPolygon = Polygon.VehiclePolygonWithRadius(0.5, goalPoint); Circle c = new Circle(.5, goalPoint); LineSegment nodeToParent = new LineSegment(predictedNode.Point, predictedNode.Parent.Point); Vector2[] pts = new Vector2[2]; if (c.Intersect(nodeToParent, out pts)) { foundPath = true; goal = predictedNode; } //if (predictedNode.DistanceTo(goalPoint) < goalTolerance) //{ // foundPath = true; // goal = predictedNode; //} return predictedNode; } return null; }