public List<PositionAndOrientation> GetTrackFromPreparedPlanner(PositionAndOrientation target)
{
if (preparedMap_ == null)
throw new ApplicationException("Prepare tracks first!");
if (seen_.Count == 0)
throw new ApplicationException();
double smallestError = Double.MaxValue;
BFSNode nodeClosestToTarget = null;
foreach (BFSNode x in seen_)
{
double currError = Math.Pow(x.position.x - target.x, 2.0d) +
Math.Pow(x.position.y - target.y, 2.0d) +
Math.Pow(x.position.angle - target.angle, 2.0d);
if (currError < smallestError)
{
smallestError = currError;
nodeClosestToTarget = x;
}
}
return GetPathBetweenPoints(startPoint_, nodeClosestToTarget);
}
public List <PositionAndOrientation> GetTrackFromPreparedPlanner(PositionAndOrientation target)
{
if (preparedMap_ == null)
{
throw new ApplicationException("Prepare tracks first!");
}
if (seen_.Count == 0)
{
throw new ApplicationException();
}
double smallestError = Double.MaxValue;
BFSNode nodeClosestToTarget = null;
foreach (BFSNode x in seen_)
{
double currError = Math.Pow(x.position.x - target.x, 2.0d) +
Math.Pow(x.position.y - target.y, 2.0d) +
Math.Pow(x.position.angle - target.angle, 2.0d);
if (currError < smallestError)
{
smallestError = currError;
nodeClosestToTarget = x;
}
}
return(GetPathBetweenPoints(startPoint_, nodeClosestToTarget));
}
private List <BFSNode> generateSuccessors(BFSNode predecessor)
{
List <BFSNode> successors = new List <BFSNode>();
double oldAngleInRadians = predecessor.position.angle / 180.0f * Math.PI;
double angleStepInRadians = angleStep_ / 180.0f * Math.PI;
foreach (int i in new List <int>()
{
0, -1, 1
})
{
double newAngleInRadians = oldAngleInRadians + angleStepInRadians * i;
double newX = predecessor.position.x + Math.Cos(newAngleInRadians) * positionStep_;
double newY = predecessor.position.y + Math.Sin(newAngleInRadians) * positionStep_;
if (newX < 0 || newX > mapSizeX_ || newY < 0 || newY > mapSizeY_)
{
continue;
}
double newAngle = predecessor.position.angle + angleStep_ * i;
PositionAndOrientation newPosition = new PositionAndOrientation(newX, newY, newAngle);
if (!IsPositionSeen(newPosition) && !(IsPositionObstacled(newPosition)))
{
BFSNode newNode = new BFSNode(newPosition, predecessor);
successors.Add(new BFSNode(newPosition, predecessor));
}
}
return(successors);
}
public void PrepareTracks(Map map)
{
seen_.Clear();
preparedMap_ = map; // hack a bit - we r holding state here
obstacles_ = map.obstacles;
Queue <BFSNode> frontier = new Queue <BFSNode>();
startPoint_ = new BFSNode(map.car, null);
frontier.Enqueue(startPoint_);
PositionAndOrientation target = map.parking;
while (frontier.Count != 0)
{
BFSNode curr = frontier.Dequeue();
List <BFSNode> succesors = generateSuccessors(curr);
succesors.ForEach(x => frontier.Enqueue(x));
succesors.ForEach(x => seen_.Add(x));
}
}
public void UpdateCarPosition(Map baseMap, List <ExtractedGlyphData> glyphs)
{
IDictionary <string, PositionAndOrientation> markersPositions = markerFinder_.FindMarkers(glyphs);
if (markersPositions.ContainsKey("car") && markersPositions.Count > 1 &&
markersPositions.All(x => x.Key == "car" || baseMap.markers.ContainsKey(x.Key)) &&
baseMap.markers.All(x => markersPositions.ContainsKey(x.Key)))
{
PositionAndOrientation carPosition = markersPositions["car"];
IDictionary <string, PositionAndOrientation> stableMarkersPosition =
markersPositions.Where(x => x.Key.StartsWith("static")).ToDictionary(x => x.Key, x => x.Value);
double averageAngleChange =
stableMarkersPosition.Average(x => x.Value.angle - baseMap.markers[x.Key].angle);
IDictionary <string, PositionAndOrientation> stableMarkersPositionWithAngleCorrection =
stableMarkersPosition.ToDictionary(x => x.Key,
x => TransformPositionOnAngleChange(x.Value, -averageAngleChange, baseMap.mapWidth, baseMap.mapHeight));
double averageXChange =
stableMarkersPositionWithAngleCorrection.Average(x => x.Value.x - baseMap.markers[x.Key].x);
double averageYChange =
stableMarkersPositionWithAngleCorrection.Average(x => x.Value.y - baseMap.markers[x.Key].y);
PositionAndOrientation correctedCarPosition =
TransformPositionOnAngleChange(carPosition, -averageAngleChange, baseMap.mapWidth, baseMap.mapHeight);
correctedCarPosition.x -= averageXChange;
correctedCarPosition.y -= averageYChange;
baseMap.car = correctedCarPosition;
Console.WriteLine("car position has been updated");
}
else
{
Console.WriteLine("car position cannot be updated basing on current frame");
}
}
private double CalculateDistanceAndSideBetweenLineAndPoint(PositionAndOrientation line, PositionAndOrientation point)
{
double lineA = Math.Cos(line.angle / 180.0 * Math.PI);
double lineB = Math.Sin(line.angle / 180.0 * Math.PI);
double lineC = -1 * (line.x * lineA + line.y * lineB);
return(lineA * point.x + lineB * point.y + lineC); // dividing by sqrt(A^2 + B^2) is not needed as these values are taken from sin/cos
}
private bool IsPositionSeen(PositionAndOrientation point)
{
List <BFSNode> matchingPositionNodes = seen_.GetObjects(new Rectangle(
(int)point.x - locationTolerance_ / 2, (int)point.y - locationTolerance_ / 2,
locationTolerance_, locationTolerance_));
return(matchingPositionNodes.Any(
x => MathTools.AnglesEqual(x.position.angle, point.angle, angleTolerance_)));
}
/// <summary>
/// transformation by change to polar coordinate system
/// </summary>
/// <param name="currentPosition"></param>
/// <param name="angleChange"></param>
/// <param name="imageMaxX"></param>
/// <param name="imageMaxY"></param>
/// <returns></returns>
private PositionAndOrientation TransformPositionOnAngleChange(
PositionAndOrientation currentPosition, double angleChange, int imageMaxX, int imageMaxY)
{
PositionAndOrientation output = new PositionAndOrientation();
output.angle = currentPosition.angle + angleChange;
double oldX = currentPosition.x - (imageMaxX / 2);
double oldY = currentPosition.y - (imageMaxY / 2);
double r = Math.Sqrt(Math.Pow(oldX, 2) + Math.Pow(oldY, 2));
double oldAngle = Math.Acos(oldX / r) * Math.Sign(oldY);
double newAngle = oldAngle + angleChange / 180.0f * Math.PI;
output.x = r * Math.Cos(newAngle);
output.y = r * Math.Sin(newAngle);
return(output);
}
public void BuildMapTest()
{
Rhino.Mocks.MockRepository mocksReposiory = new MockRepository();
MarkerFinder markerFinderMock = mocksReposiory.StrictMock<MarkerFinder>();
ObstaclesFinder obstaclesFinderMock = mocksReposiory.StrictMock<ObstaclesFinder>();
System.Drawing.Size image = new System.Drawing.Size(1000, 1000);
string carMarker = "car";
string parkingMarker = "parking";
List<string> stableMarkers = new List<string>(){"s1"};
PositionAndOrientation carPosition = new PositionAndOrientation(5.0f, 4.0f, 90.0f);
PositionAndOrientation parkingPosition = new PositionAndOrientation(1.0f, 1.0f, 1.0f);
List<PositionAndOrientation> stableMarkersPosition =
new List<PositionAndOrientation>() { new PositionAndOrientation(3.0f, 1.0f, 0.0f) };
ObjectsToTrace objectsToTrace = new ObjectsToTrace(stableMarkers, carMarker, parkingMarker);
MapBuilder mapBuilder = new MapBuilder(markerFinderMock, obstaclesFinderMock, objectsToTrace);
using (mocksReposiory.Record())
{
markerFinderMock.Expect(x => x.FindMarker(image, carMarker))
.Return(carPosition);
markerFinderMock.Expect(x => x.FindMarker(image, parkingMarker))
.Return(parkingPosition);
markerFinderMock.Expect(x => x.FindMarker(image, stableMarkers[0]))
.Return(stableMarkersPosition[0]);
}
using (mocksReposiory.Playback())
{
Map map = mapBuilder.BuildMap(image);
Assert.AreEqual(carPosition, map.car);
Assert.AreEqual(parkingPosition, map.parking);
Assert.AreEqual(stableMarkersPosition[0], map.markers[stableMarkers[0]]);
}
}
//TODO: add version with recalc from 80% with better accuaracy
/// <summary>
/// it finishes when gets to target
/// </summary>
/// <param name="map"></param>
/// <returns></returns>
public List <PositionAndOrientation> PlanTrack(Map map)
{
if (seen_.GetAllObjects().Count != 0)
{
throw new ApplicationException();
}
obstacles_ = map.obstacles;
Queue <BFSNode> frontier = new Queue <BFSNode>();
BFSNode startPoint = new BFSNode(map.car, null);
frontier.Enqueue(startPoint);
PositionAndOrientation target = map.parking;
while (frontier.Count != 0)
{
BFSNode curr = frontier.Dequeue();
List <BFSNode> succesors = generateSuccessors(curr);
foreach (BFSNode s in succesors)
{
if (ArePointsSame(s.position, target))
{
return(GetPathBetweenPoints(startPoint, s));
}
}
succesors.ForEach(x => frontier.Enqueue(x));
succesors.ForEach(x => seen_.Add(x));
}
return(new List <PositionAndOrientation>()); // couldn't find result - returning empty list
}
public void RebuildMapTest()
{
Rhino.Mocks.MockRepository mocksReposiory = new MockRepository();
MarkerFinder markerFinderMock = mocksReposiory.StrictMock<MarkerFinder>();
ObstaclesFinder obstaclesFinderMock = mocksReposiory.StrictMock<ObstaclesFinder>();
const int IMAGE_SIZE_X = 1000;
const int HALF_IMAGE_SIZE_X = IMAGE_SIZE_X / 2;
const int IMAGE_SIZE_Y = 800;
const int HALF_IMAGE_SIZE_Y = IMAGE_SIZE_Y / 2;
System.Drawing.Size image = new System.Drawing.Size(0, 0);
string carMarker = "car";
string parkingMarker = "parking";
List<string> stableMarkers = new List<string>() { "s1" };
PositionAndOrientation carPosition = new PositionAndOrientation(HALF_IMAGE_SIZE_X + 5.0f, HALF_IMAGE_SIZE_Y + 4.0f, 0.0f);
PositionAndOrientation parkingPosition = new PositionAndOrientation(HALF_IMAGE_SIZE_X + 1.0f, HALF_IMAGE_SIZE_Y + 1.0f, 1.0f);
List<PositionAndOrientation> stableMarkersPosition =
new List<PositionAndOrientation>() { new PositionAndOrientation(HALF_IMAGE_SIZE_X + 3.0f, HALF_IMAGE_SIZE_Y + 5.0f, 90.0f) };
ObjectsToTrace objectsToTrace = new ObjectsToTrace(stableMarkers, carMarker, parkingMarker);
MapBuilder mapBuilder = new MapBuilder(markerFinderMock, obstaclesFinderMock, objectsToTrace);
using (mocksReposiory.Record())
{
markerFinderMock.Expect(x => x.FindMarker(image, carMarker))
.Return(carPosition);
markerFinderMock.Expect(x => x.FindMarker(image, parkingMarker))
.Return(parkingPosition);
markerFinderMock.Expect(x => x.FindMarker(image, stableMarkers[0]))
.Return(stableMarkersPosition[0]);
}
Map map;
using (mocksReposiory.Playback())
{
map = mapBuilder.BuildMap(image);
}
PositionAndOrientation newCarPosition = new PositionAndOrientation(HALF_IMAGE_SIZE_X - 4.0f, HALF_IMAGE_SIZE_Y + 5.0f, 90.0f);
List<PositionAndOrientation> newStableMarkersPosition =
new List<PositionAndOrientation>() { new PositionAndOrientation(HALF_IMAGE_SIZE_X - 3.0f, HALF_IMAGE_SIZE_Y + 1.0f, 180.0f) };
System.Drawing.Size newImage = new System.Drawing.Size(0, 0);
using (mocksReposiory.Record())
{
markerFinderMock.Expect(x => x.FindMarker(newImage, carMarker))
.Return(newCarPosition);
markerFinderMock.Expect(x => x.FindMarker(newImage, stableMarkers[0]))
.Return(newStableMarkersPosition[0]);
}
using (mocksReposiory.Playback())
{
mapBuilder.UpdateCarPosition(map, newImage);
double delta = 0.0001f;
Assert.AreEqual(HALF_IMAGE_SIZE_X + 7.0f, map.car.x, delta);
Assert.AreEqual(HALF_IMAGE_SIZE_Y + 6.0f, map.car.y, delta);
Assert.AreEqual(0.0f, map.car.angle, delta);
}
}
private double CalculateDistance(PositionAndOrientation a, PositionAndOrientation b)
{
return(Math.Sqrt(Math.Pow(a.x - b.x, 2.0d) + Math.Pow(a.y - b.y, 2.0d)));
}
public BFSNode(PositionAndOrientation _position, BFSNode _predecessor)
{
position = _position;
predecessor = _predecessor;
Rect = new Rectangle((int)position.x, (int)position.y, 1, 1);
}
public BFSNode(PositionAndOrientation _position, BFSNode _predecessor)
{
position = _position;
predecessor = _predecessor;
Rect = new Rectangle((int)position.x, (int)position.y, 1, 1);
}
private bool IsPositionObstacled(PositionAndOrientation point)
{
Rectangle pointRectangle = new Rectangle((int)point.x, (int)point.y, 1, 1);
return obstacles_.Any(x => x.Contains(pointRectangle));
}
private bool IsPositionSeen(PositionAndOrientation point)
{
List<BFSNode> matchingPositionNodes = seen_.GetObjects(new Rectangle(
(int)point.x - locationTolerance_ / 2, (int)point.y - locationTolerance_ / 2,
locationTolerance_, locationTolerance_));
return matchingPositionNodes.Any(
x => MathTools.AnglesEqual(x.position.angle, point.angle, angleTolerance_));
}
private bool ArePointsSame(PositionAndOrientation a, PositionAndOrientation b)
{
return Math.Pow(a.x - b.x, 2.0d) + Math.Pow(a.y - b.y, 2.0d) <= locationToleranceSquared_
&&
MathTools.AnglesEqual(a.angle, b.angle, angleTolerance_);
}
private List<BFSNode> generateSuccessors(BFSNode predecessor)
{
List<BFSNode> successors = new List<BFSNode>();
double oldAngleInRadians = predecessor.position.angle / 180.0f * Math.PI;
double angleStepInRadians = angleStep_ / 180.0f * Math.PI;
foreach (int i in new List<int>(){0, -1, 1})
{
double newAngleInRadians = oldAngleInRadians + angleStepInRadians * i;
double newX = predecessor.position.x + Math.Cos(newAngleInRadians) * positionStep_;
double newY = predecessor.position.y + Math.Sin(newAngleInRadians) * positionStep_;
if (newX < 0 || newX > mapSizeX_ || newY < 0 || newY > mapSizeY_)
continue;
double newAngle = predecessor.position.angle + angleStep_ * i;
PositionAndOrientation newPosition = new PositionAndOrientation(newX, newY, newAngle);
if (!IsPositionSeen(newPosition) && !(IsPositionObstacled(newPosition)))
{
BFSNode newNode = new BFSNode(newPosition, predecessor);
successors.Add(new BFSNode(newPosition, predecessor));
}
}
return successors;
}
/// <summary>
/// transformation by change to polar coordinate system
/// </summary>
/// <param name="currentPosition"></param>
/// <param name="angleChange"></param>
/// <param name="imageMaxX"></param>
/// <param name="imageMaxY"></param>
/// <returns></returns>
private PositionAndOrientation TransformPositionOnAngleChange(
PositionAndOrientation currentPosition, double angleChange, int imageMaxX, int imageMaxY)
{
PositionAndOrientation output = new PositionAndOrientation();
output.angle = currentPosition.angle + angleChange;
double oldX = currentPosition.x - (imageMaxX / 2);
double oldY = currentPosition.y - (imageMaxY / 2);
double r = Math.Sqrt(Math.Pow(oldX, 2) + Math.Pow(oldY, 2));
double oldAngle = Math.Acos(oldX / r) * Math.Sign(oldY);
double newAngle = oldAngle + angleChange / 180.0f * Math.PI;
output.x = r * Math.Cos(newAngle);
output.y = r * Math.Sin(newAngle);
return output;
}
private double CalculateDistanceAndSideBetweenLineAndPoint(PositionAndOrientation line, PositionAndOrientation point)
{
double lineA = Math.Cos(line.angle / 180.0 * Math.PI);
double lineB = Math.Sin(line.angle / 180.0 * Math.PI);
double lineC = -1 * (line.x * lineA + line.y * lineB);
return lineA * point.x + lineB * point.y + lineC; // dividing by sqrt(A^2 + B^2) is not needed as these values are taken from sin/cos
}
private double CalculateDistance(PositionAndOrientation a, PositionAndOrientation b)
{
return Math.Sqrt(Math.Pow(a.x - b.x, 2.0d) + Math.Pow(a.y - b.y, 2.0d));
}
private bool IsPositionObstacled(PositionAndOrientation point)
{
Rectangle pointRectangle = new Rectangle((int)point.x, (int)point.y, 1, 1);
return(obstacles_.Any(x => x.Contains(pointRectangle)));
}
private bool ArePointsSame(PositionAndOrientation a, PositionAndOrientation b)
{
return(Math.Pow(a.x - b.x, 2.0d) + Math.Pow(a.y - b.y, 2.0d) <= locationToleranceSquared_
&&
MathTools.AnglesEqual(a.angle, b.angle, angleTolerance_));
}
