private void Window_Loaded(object sender, RoutedEventArgs e)
{
Room room = new Room();
Tracker tracker = new Tracker("kinect", new Point(10, 10), 225, 15);
room.AddTracker(tracker);
//double angle = 180.0;
//double fov = 190;
//room.AddDevice(new Device("one", new Point(5, 0), angle, fov));
//room.AddDevice(new Device("two", new Point(10, 0), angle, fov));
//room.AddDevice(new Device("three", new Point(15, 0), angle, fov));
//room.AddDevice(new Device("four", new Point(10, 5), angle, fov));
//room.AddDevice(new Device("five", new Point(10, -5), angle, fov));
//Debug.WriteLine(room.GetDevicesInViewOf(room.GetDevice("one")).Count);
//Debug.WriteLine(room.GetDevicesInViewOf(room.GetDevice("two")).Count);
//Debug.WriteLine(room.GetDevicesInViewOf(room.GetDevice("three")).Count);
//Debug.WriteLine(room.GetDevicesInViewOf(room.GetDevice("four")).Count);
//Debug.WriteLine(room.GetDevicesInViewOf(room.GetDevice("five")).Count);
Device dev = new Device("dev");
room.AddDevice(dev);
tracker.UpdatePositionForDevice(dev, new Vector(-20,0));
Debug.Write(dev.Location.X + " " + dev.Location.Y);
}
public List<Device> GetDevicesInViewOf(Device observer)
{
// We imagine the field of view as two vectors, pointing away from the observing device. Targets between the vectors are in view.
// We will use angles to represent these vectors.
double leftFieldOfView = Util.NormalizeAngle(observer.Orientation + observer.FieldOfView / 2);
double rightFieldOfView = Util.NormalizeAngle(observer.Orientation - observer.FieldOfView / 2);
List<Device> returnDevices = new List<Device>();
if (observer.FieldOfView == 0.0)
return returnDevices;
foreach (Device target in devices.Values)
{
if (target.Name == observer.Name)
continue;
// Atan2 is the inverse tangent function, given lengths for the opposite and adjacent legs of a right triangle, it returns the angle
double angle = Util.NormalizeAngle(Math.Atan2(target.Location.Y - observer.Location.Y, target.Location.X - observer.Location.X) * 180/Math.PI);
// Ordinarily, the angle defining the left boundary of the field of view will be larger than the angle for the right.
// For example, if our devices has orientation of 90.0 and a field of view of 15 degrees, then the left and right FoVs are 97.5 and 82.5 degrees
// In this case, the target must be at an angle between left and right to be in view.
if (leftFieldOfView > rightFieldOfView && angle < leftFieldOfView && angle > rightFieldOfView)
{
returnDevices.Add(target);
}
// If the field of view includes the X axis, then the left field of view will be smaller than the right field of view.
// For example, if our device has an orientation of 0.0 and a field of view of 15 degrees, then the left FoV vector will be at 7.5 degrees
// and the right FoV will be at 352.5 degrees.
else if (leftFieldOfView < rightFieldOfView)
{
if(angle < leftFieldOfView || angle > rightFieldOfView)
returnDevices.Add(target);
}
}
return returnDevices;
}
/// <summary>
/// Initializes a new instance of the MainWindow class.
/// </summary>
public MainWindow()
{
InitializeComponent();
nearX = 1000;
nearY = 1000;
nearZ = 1000;
farX = 0;
farY = 0;
farZ = 0;
myRoom = new Room();
myTracker = new Tracker("myKinect", new Point(0, 2.5), 0.0, 15);
myRoom.AddTracker(myTracker);
myDevice = new Device("me");
myRoom.AddDevice(myDevice);
}
public void RemoveDevice(Device device)
{
devices.Remove(device.Name);
}
public void AddDevice(Device device)
{
devices.Add(device.Name, device);
}
public void UpdatePositionForDevice(Device device, SkeletonPoint skeletonPoint)
{
Vector updatedPosition = Util.TranslateFromCoordinateSpace(new Vector(skeletonPoint.Z, skeletonPoint.X), Orientation, new Vector(Location.X, Location.Y));
device.Location = new Point(updatedPosition.X, updatedPosition.Y);
}
// Accepts a point in the Tracker's coordinate space, translates this into the room's coordinate space using the Tracker's position and orientation, and updates the location of the device with it.
// In the Tracker's coordinate space, it is at the origin and with orientation 0 (facing down the X axis).
// In the Kinect's coordinate space, it is at 0.0, and faces down the Z axis.
// Left-right movement in front of the camera is mapped to the X axis, and vertical movement is mapped to the Y axis.
// So, the Kinect's Z axis corresponds to the Tracker's X axis, and the Kinect's X axis corresponds to the Tracker's Y axis.
// To translate from the Kinect's coordinate space, you want to pass a new Vector(SkeletonPoint.Z, SkeletonPoint.X).
public void UpdatePositionForDevice(Device device, Vector vector)
{
Vector updatedPosition = Util.TranslateFromCoordinateSpace(vector, Orientation, new Vector(Location.X, Location.Y));
device.Location = new Point(updatedPosition.X, updatedPosition.Y);
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
// Create the drawing group we'll use for drawing
this.drawingGroup = new DrawingGroup();
// Create an image source that we can use in our image control
this.imageSource = new DrawingImage(this.drawingGroup);
// Display the drawing using our image control
Image.Source = this.imageSource;
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
// To make your app robust against plug/unplug,
// it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
this.sensor = potentialSensor;
break;
}
}
if (null != this.sensor)
{
// Turn on the skeleton stream to receive skeleton frames
this.sensor.SkeletonStream.Enable();
// Add an event handler to be called whenever there is new color frame data
this.sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;
// Start the sensor!
try
{
this.sensor.Start();
}
catch (IOException)
{
this.sensor = null;
}
}
if (null == this.sensor)
{
MessageBox.Show("No Kinect");
//this.statusBarText.Text = Properties.Resources.NoKinectReady;
}
room = new Room();
tracker = new Tracker("kinect", new Point(2.25, yRange), 270.0);
room.AddTracker(tracker);
devices = new Device[6];
for (int i = 0; i < 6; i++)
{
devices[i] = new Device(i.ToString());
if (i % 2 == 1)
devices[i].Orientation = 0;
else
devices[i].Orientation = 180.0;
room.AddDevice(devices[i]);
}
}
public void CoordinateTranslationTest()
{
Room r = new Room();
Tracker t = new Tracker("myKinect", new Point(5, 5), 270);
r.AddTracker(t);
Device one = new Device("one");
Device two = new Device("two");
Device three = new Device("three");
r.AddDevice(one);
r.AddDevice(two);
r.AddDevice(three);
t.UpdatePositionForDevice(one, new Vector(1, 1));
t.UpdatePositionForDevice(two, new Vector(5, 0));
t.UpdatePositionForDevice(three, new Vector(3, -3));
Assert.AreEqual(one.Location.X, 6, 0.001);
Assert.AreEqual(one.Location.Y, 4, 0.001);
Assert.AreEqual(two.Location.X, 5, 0.001);
Assert.AreEqual(two.Location.Y, 0, 0.001);
Assert.AreEqual(three.Location.X, 2, 0.001);
Assert.AreEqual(three.Location.Y, 2, 0.001);
// test of translation alone
Tracker t2 = new Tracker("kinect2", new Point(0, -10), 0.0);
r.AddTracker(t2);
t2.UpdatePositionForDevice(one, new Vector(6, 14));
t2.UpdatePositionForDevice(two, new Vector(5, 10));
t2.UpdatePositionForDevice(three, new Vector(2, 12));
Assert.AreEqual(one.Location.X, 6, 0.001);
Assert.AreEqual(one.Location.Y, 4, 0.001);
Assert.AreEqual(two.Location.X, 5, 0.001);
Assert.AreEqual(two.Location.Y, 0, 0.001);
Assert.AreEqual(three.Location.X, 2, 0.001);
Assert.AreEqual(three.Location.Y, 2, 0.001);
//test of rotation alone
Tracker t3 = new Tracker("kinect3", new Point(0, 0), 135);
t3.UpdatePositionForDevice(one, new Vector(-1.425, -7.0678));
t3.UpdatePositionForDevice(two, new Vector(-3.535, -3.535));
t3.UpdatePositionForDevice(three, new Vector(0, -Math.Sqrt(8)));
Assert.AreEqual(one.Location.X, 6, 0.01); // to deal with horrible horrible rounding error introduced by hardcoded positions just above
Assert.AreEqual(one.Location.Y, 4, 0.01);
Assert.AreEqual(two.Location.X, 5, 0.001);
Assert.AreEqual(two.Location.Y, 0, 0.001);
Assert.AreEqual(three.Location.X, 2, 0.001);
Assert.AreEqual(three.Location.Y, 2, 0.001);
}
