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