public void TestOverallFunctionalityOfCompositeDoorDetectorWithOverlap()
{
DetectedDoor door1 = new DetectedDoor(new Rectangle(0, 0, 100, 100), DetectedDoor.DetectionConfidence.LOW, 0, 0);
DetectedDoor door2 = new DetectedDoor(new Rectangle(10, 10, 80, 80), DetectedDoor.DetectionConfidence.LOW, 0, 0);
List <DetectedDoor> mock1Doors = new List <DetectedDoor>();
mock1Doors.Add(door1);
List <DetectedDoor> mock2Doors = new List <DetectedDoor>();
mock2Doors.Add(door2);
MockDoorDetector mock1 = new MockDoorDetector(mock1Doors);
MockDoorDetector mock2 = new MockDoorDetector(mock2Doors);
List <IDoorDetector> detectors = new List <IDoorDetector>();
detectors.Add(mock1);
detectors.Add(mock2);
DetectedDoor expectedDoor = new DetectedDoor(new Rectangle(0, 0, 100, 100), DetectedDoor.DetectionConfidence.HIGH, 0, 0);
List <DetectedDoor> expectedDoors = new List <DetectedDoor>();
expectedDoors.Add(expectedDoor);
CompositeDoorDetector composite = new CompositeDoorDetector(detectors);
Image <Bgr, Byte> image = new Image <Bgr, byte>(new Size(300, 300));
List <DetectedDoor> actualDoors = composite.GetDoors(image);
CollectionAssert.AreEqual(expectedDoors, actualDoors);
}
public void TestCollectDoorsCollectsContainedDoors()
{
Point center = new Point(50, 50);
Rectangle outerRect = new Rectangle(center, new Size(30, 30));
Rectangle innerRect = new Rectangle(center, new Size(10, 10));
DetectedDoor outerDoor = new DetectedDoor(outerRect, DetectedDoor.DetectionConfidence.LOW, 0, 0);
DetectedDoor innerDoor = new DetectedDoor(innerRect, DetectedDoor.DetectionConfidence.LOW, 0, 0);
List <DetectedDoor> inputDoors = new List <DetectedDoor>();
inputDoors.Add(outerDoor);
inputDoors.Add(innerDoor);
DetectedDoor expectedDoor = new DetectedDoor(outerRect, DetectedDoor.DetectionConfidence.HIGH, 0, 0);
List <DetectedDoor> expectedDoors = new List <DetectedDoor>();
expectedDoors.Add(expectedDoor);
CompositeDoorDetector doorDetector = new CompositeDoorDetector(null);
PrivateObject privateAccessor = new PrivateObject(doorDetector);
List <DetectedDoor> collectedDoors = (List <DetectedDoor>)privateAccessor.Invoke("CollectDoors", inputDoors);
CollectionAssert.AreEqual(collectedDoors, expectedDoors);
}
public void TestDrawDoorsAddsRectangleForOneDoor()
{
System.Drawing.Rectangle doorBoundingBox = new System.Drawing.Rectangle(20, 50, 300, 500);
DetectedDoor detectedDoor = new DetectedDoor(doorBoundingBox, new System.Drawing.Size(0, 0), DetectedDoor.DetectionConfidence.LOW, 0, 0, "");
Canvas canvas = new Canvas();
List <DetectedDoor> doors = new List <DetectedDoor>();
doors.Add(detectedDoor);
Doors doorsTab = new Doors(null);
PrivateObject privateAccessor = new PrivateObject(doorsTab);
privateAccessor.Invoke("DrawDoors", doors, canvas);
// There was one door given so there should be one child in the canvas
Assert.AreEqual <int>(1, canvas.Children.Count);
// That one child rectangle should have the correct bounding box
System.Windows.Shapes.Rectangle doorShape = (System.Windows.Shapes.Rectangle)canvas.Children[0];
// Compare the two bounding boxes, they should be the same
Assert.AreEqual <double>(doorBoundingBox.Width, doorShape.Width);
Assert.AreEqual <double>(doorBoundingBox.Height, doorShape.Height);
Assert.AreEqual <double>(doorBoundingBox.Left, Canvas.GetLeft(doorShape));
Assert.AreEqual <double>(doorBoundingBox.Top, Canvas.GetTop(doorShape));
}
// Return true iff we are properly lined up with the door
private bool IsNavigationSuccessfulApproachedTheDoor(DetectedDoor targetDoor)
{
if (targetDoor == null)
{
return(false);
}
double angle = targetDoor.RelativeAngle;
double distance = targetDoor.RelativeDistance;
bool res = angle >= DOOR_MIN_TARGET_ANGLE && angle <= DOOR_MAX_TARGET_ANGLE &&
distance >= DOOR_MIN_TARGET_DISTANCE && distance <= DOOR_MAX_TARGET_DISTANCE;
return(res);
}
public void DoorDetector_OnDoorDetected(object sender, DoorDetectedEventArgs eventArgs)
{
if (step == 1)
{
List <DetectedDoor> detectedDoors = eventArgs.DetectedDoors;
_targetDoor = GetTargetDoor(detectedDoors);
if (IsNavigationSuccessfulApproachedTheDoor(_targetDoor))
{
//MessageBox.Show("Approached the door");
NotifyNavigationEnded(true);
//stop subscribe door detector
_doorDetector.OnDoorDetected -= DoorDetector_OnDoorDetected;
}
}
}
public void TestNearestTargetDoorIsSelected()
{
DetectedDoor door1 = new DetectedDoor(new System.Drawing.Rectangle(), DetectedDoor.DetectionConfidence.HIGH, 0, 50);
DetectedDoor door2 = new DetectedDoor(new System.Drawing.Rectangle(), DetectedDoor.DetectionConfidence.HIGH, 0, 25);
List <DetectedDoor> doors = new List <DetectedDoor>();
doors.Add(door1);
doors.Add(door2);
DoorOrientationNavigationStrategy strategy = new DoorOrientationNavigationStrategy(new MockDoorDetector(null));
PrivateObject privateAccessor = new PrivateObject(strategy);
DetectedDoor targetDoor = (DetectedDoor)privateAccessor.Invoke("GetTargetDoor", doors);
// door2 has the smallest estimated distance therefore it should be selected as the target door
Assert.AreEqual <DetectedDoor>(door2, targetDoor);
}
public Vector GetDirection()
{
if (step == 1)
{
DetectedDoor door = _targetDoor;
if (door == null)
{
return(new Vector(0, 0));
}
// RelativeAngle is expressed such that 0 radians means the door is directly in front
// of the vehicle whereas trigonometrically 0 radians represents the right direction.
// We need to add PI / 2 to the angle in order for trig functions to work properly
double doorAngle = door.RelativeAngle + Math.PI / 2;
double xComponent = Math.Cos(doorAngle);
double yComponent = Math.Sin(doorAngle);
return(new Vector(xComponent, yComponent));
//if(door.RelativeAngle > 0.15)
//{
// return new Vector(-1, 0);
//}
//else if(door.RelativeAngle < -0.15)
//{
// return new Vector(1, 0);
//}
//else
//{
// return new Vector(0, 1);
//}
}
else if (step == 2)
{
//get direction from sonarVector
//default value is (0,1) going forward
return(sonarVector);
}
else
{
throw new Exception();
}
}
public void TestCollectDoorsDoesntCollectSeparateDoors()
{
Rectangle rect1 = new Rectangle(new Point(0, 0), new Size(10, 10));
Rectangle rect2 = new Rectangle(new Point(100, 100), new Size(10, 10));
DetectedDoor door1 = new DetectedDoor(rect1, DetectedDoor.DetectionConfidence.LOW, 0, 0);
DetectedDoor door2 = new DetectedDoor(rect2, DetectedDoor.DetectionConfidence.LOW, 0, 0);
List <DetectedDoor> inputDoors = new List <DetectedDoor>();
inputDoors.Add(door1);
inputDoors.Add(door2);
CompositeDoorDetector doorDetector = new CompositeDoorDetector(null);
PrivateObject privateAccessor = new PrivateObject(doorDetector);
List <DetectedDoor> collectedDoors = (List <DetectedDoor>)privateAccessor.Invoke("CollectDoors", inputDoors);
CollectionAssert.AreEqual(collectedDoors, inputDoors);
}
// Returns the door that we should select to navigate to (the target door)
// The target door is chosen as the door within the given list of doors with the
// minimum RelativeDistance
private DetectedDoor GetTargetDoor(List <DetectedDoor> doors)
{
if (doors == null)
{
return(null);
}
double minDistance = double.MaxValue;
DetectedDoor closestDoor = null;
foreach (DetectedDoor door in doors)
{
if (door.RelativeDistance < minDistance)
{
closestDoor = door;
minDistance = door.RelativeDistance;
}
}
return(closestDoor);
}
