public void UpdateController()
{
uint buttons = 0;
prevButtons = currentButtons;
// NOTE! There is potentially data waiting in queue.
// We need to poll *all* of it by calling psmove_poll() until the queue is empty. Otherwise, data might begin to build up.
while (PsMoveApi.psmove_poll(_motionController.Handle) > 0)
{
// We are interested in every button press between the last update and this one:
buttons = buttons | PsMoveApi.psmove_get_buttons(_motionController.Handle);
// The events are not really working from the PS Move Api. So we do our own with the prevButtons
//psmove_get_button_events(handle, ref pressed, ref released);
}
currentButtons = buttons;
// For acceleration, gyroscope, and magnetometer values, we look at only the last value in the queue.
// We could in theory average all the acceleration (and other) values in the queue for a "smoothing" effect, but we've chosen not to.
ProcessData();
// Send a report to the controller to update the LEDs and rumble.
if (PsMoveApi.psmove_update_leds(_motionController.Handle) == 0)
{
// Debug.Log ("led set");
// If it returns zero, the controller must have disconnected (i.e. out of battery or out of range),
// so we should fire off any events and disconnect it.
if (this != null)
{
OnControllerDisconnected(this, new EventArgs());
}
Disconnect();
}
}
/// <summary>
/// Returns whether the connecting succeeded or not.
///
/// NOTE! This function does NOT pair the controller by Bluetooth.
/// If the controller is not already paired, it can only be connected by USB.
/// See README for more information.
/// </summary>
public PSMoveConnectStatus Init(int index)
{
_motionController.Handle = PsMoveApi.psmove_connect_by_id(index);
// Error check the result!
if (_motionController.Handle == IntPtr.Zero)
{
return(_motionController.ConnectStatus = PSMoveConnectStatus.Error);
}
// Make sure the connection is actually sending data. If not, this is probably a controller
// you need to remove manually from the OSX Bluetooth Control Panel, then re-connect.
if (PsMoveApi.psmove_update_leds(_motionController.Handle) == 0)
{
return(_motionController.ConnectStatus = PSMoveConnectStatus.NoData);
}
_motionController.Id = index;
_motionController.Remote = PsMoveApi.psmove_is_remote(_motionController.Handle) > 0;
_motionController.ConnectionType = PsMoveApi.psmove_connection_type(_motionController.Handle);;
StringBuilder builder = new StringBuilder(64);
PsMoveApi.get_moved_host(_motionController.Handle, builder);
_motionController.HostIp = builder.ToString();
PsMoveApi.get_serial(_motionController.Handle, builder);
_motionController.Serial = builder.ToString();
UpdateController();
return(_motionController.ConnectStatus = PSMoveConnectStatus.OK);
}
/// <summary>
/// Sets the LED color
/// </summary>
/// <param name="r">Red value of the LED color (0-255)</param>
/// <param name="g">Green value of the LED color (0-255)</param>
/// <param name="b">Blue value of the LED color (0-255)</param>
public void SetLED(int r, int g, int b)
{
_motionController.Color = new Color(r / 255f, g / 255f, b / 255f);
if (!disconnected)
{
PsMoveApi.psmove_set_leds(_motionController.Handle, (byte)r, (byte)g, (byte)b);
}
}
public void StopStracker()
{
if (_camera.Handle == IntPtr.Zero)
{
return;
}
PsMoveApi.delete_PSMoveTracker(_camera.Handle);
_camera.Handle = IntPtr.Zero;
}
private void CancelUpdateTask()
{
if (_ctsUpdate != null)
{
_ctsUpdate.Cancel();
_updateTask.Wait();
PsMoveApi.delete_PSMoveTracker(_camera.Handle);
_camera.Handle = IntPtr.Zero;
}
}
public void DisableTracking(MotionControllerModel mc)
{
if (mc.Design)
{
return;
}
PsMoveApi.psmove_tracker_disable(_camera.Handle, mc.Handle);
mc.Tracking[_camera] = false;
mc.TrackerStatus[_camera] = PSMoveTrackerStatus.NotCalibrated;
ConsoleService.Write(string.Format("[Tracker, {0}] Tracking of Motion Controller ({1}) disabled.",
_camera.GUID, mc.Serial));
}
private void UpdateButtons()
{
_motionController.Circle = GetButton(PSMoveButton.Circle);
_motionController.Cross = GetButton(PSMoveButton.Cross);
_motionController.Triangle = GetButton(PSMoveButton.Triangle);
_motionController.Square = GetButton(PSMoveButton.Square);
_motionController.Move = GetButton(PSMoveButton.Move);
_motionController.PS = GetButton(PSMoveButton.PS);
_motionController.Start = GetButton(PSMoveButton.Start);
_motionController.Select = GetButton(PSMoveButton.Select);
_motionController.Trigger = (PsMoveApi.psmove_get_trigger(_motionController.Handle));
}
public void Disconnect()
{
if (_motionController.Handle != IntPtr.Zero)
{
SetLED(0, 0, 0);
_motionController.Rumble = 0;
PsMoveApi.psmove_disconnect(_motionController.Handle);
_motionController.Handle = IntPtr.Zero;
disconnected = true;
Console.WriteLine("move destroyed");
}
}
public Image <Bgr, Byte> GetImage()
{
if (_camera.Handle == IntPtr.Zero)
{
return(null);
}
IntPtr frame = PsMoveApi.psmove_tracker_get_frame(_camera.Handle);
MIplImage rgb32Image = (MIplImage)Marshal.PtrToStructure(frame, typeof(MIplImage));
return(new Image <Bgr, byte>(rgb32Image.width, rgb32Image.height, rgb32Image.widthStep, rgb32Image.imageData));
}
public bool StartTracker(PSMoveTrackerExposure exposure)
{
if (_camera.Handle == IntPtr.Zero)
{
_camera.Handle = PsMoveApi.psmove_tracker_new_with_camera(_camera.TrackerId);
ConsoleService.Write(string.Format("[Tracker, {0}] Started.", _camera.GUID));
PsMoveApi.psmove_tracker_set_exposure(_camera.Handle, exposure);
// full led intensity
PsMoveApi.psmove_tracker_set_dimming(_camera.Handle, 1f);
_camera.Fusion = PsMoveApi.new_PSMoveFusion(_camera.Handle, 1.0f, 1000f);
}
return(_camera.Handle != IntPtr.Zero);
}
public void AddAvailableMotionControllers()
{
int count = PsMoveApi.count_connected();
for(int i = 0; i < count; i++)
{
IMotionControllerService mcs = new MotionControllerService();
MotionControllerModel mc = mcs.Initialize(i);
mc.Name = "MC " + i;
new MotionControllerViewModel(mc, mcs);
}
Refresh();
}
public void DoRefresh()
{
ObservableCollection<MotionControllerModel> existingControllers = new ObservableCollection<MotionControllerModel>();
AvailableMotionControllers.Clear();
NewMotionController = new MotionControllerModel();
NewMotionController.Name = null;
NewControllersDetected = false;
int connectedCount = PsMoveApi.count_connected();
if(connectedCount > 0)
{
foreach (MotionControllerViewModel mcvw in SimpleIoc.Default.GetAllCreatedInstances<MotionControllerViewModel>())
{
existingControllers.Add(mcvw.MotionController);
}
MotionControllerService motionControllerService = new MotionControllerService();
for (int i = 0; i < connectedCount; i++)
{
MotionControllerModel tmp = motionControllerService.Initialize(i);
if(existingControllers.Count > 0)
{
bool duplicate = false;
foreach (MotionControllerModel mcw in existingControllers)
{
if (tmp.ConnectStatus == PSMoveConnectStatus.OK)
{
if (tmp.Serial.Equals(mcw.Serial))
{
duplicate = true;
break;
}
}
} // end foreach
if (!duplicate) AvailableMotionControllers.Add(tmp);
}
else
{
if (tmp.ConnectStatus == PSMoveConnectStatus.OK)
{
AvailableMotionControllers.Add(tmp);
}
}
} // end for
}
if (AvailableMotionControllers.Count > 0) NewControllersDetected = true;
} // DoRefresh
public void Destroy()
{
if (_camera.Handle != IntPtr.Zero)
{
DisableTracking();
CancelUpdateTask();
ConsoleService.Write(string.Format("[Tracker, {0}] Tracker destroyed.", _camera.GUID));
}
if (_camera.Fusion != IntPtr.Zero)
{
PsMoveApi.psmove_fusion_free(_camera.Fusion);
_camera.Fusion = IntPtr.Zero;
ConsoleService.Write(string.Format("[Tracker, {0}] Fusion destroyed.", _camera.GUID));
}
}
public void EnableTracking(MotionControllerModel mc)
{
if (mc.Design)
{
return;
}
if (_camera.Handle == IntPtr.Zero)
{
StartTracker(PSMoveTrackerExposure.Low);
}
ConsoleService.Write(string.Format("[Tracker, {0}] Calibrating Motion Controller ({1}).", _camera.GUID, mc.Serial));
byte r = (byte)((mc.Color.r * 255) + 0.5f);
byte g = (byte)((mc.Color.g * 255) + 0.5f);
byte b = (byte)((mc.Color.b * 255) + 0.5f);
mc.TrackerStatus[_camera] = PsMoveApi.psmove_tracker_enable_with_color(_camera.Handle, mc.Handle, r, g, b);
if (mc.TrackerStatus[_camera] == PSMoveTrackerStatus.Tracking ||
mc.TrackerStatus[_camera] == PSMoveTrackerStatus.Calibrated)
{
PsMoveApi.psmove_tracker_update_image(_camera.Handle);
PsMoveApi.psmove_tracker_update(_camera.Handle, mc.Handle);
mc.TrackerStatus[_camera] = PsMoveApi.psmove_tracker_get_status(_camera.Handle, mc.Handle);
PsMoveApi.psmove_enable_orientation(mc.Handle, PSMoveBool.True);
PsMoveApi.psmove_reset_orientation(mc.Handle);
}
//Matrix4x4 proj = new Matrix4x4();
//for (int row = 0; row < 4; row++)
//{
// for (int col = 0; col < 4; col++)
// {
// proj[row, col] = PsMoveApi.PSMoveMatrix4x4_at(PsMoveApi.psmove_fusion_get_projection_matrix(_camera.Fusion), row * 4 + col);
// }
//}
//mc.ProjectionMatrix[_camera] = proj;
ConsoleService.Write(string.Format("[Tracker, {0}] Tracker Status of {1} = {2}",
_camera.GUID, mc.Name, Enum.GetName(typeof(PSMoveTrackerStatus), mc.TrackerStatus[_camera])));
}
public bool Orient(PSMoveBool enable)
{
bool oriented = false;
PsMoveApi.psmove_enable_orientation(_motionController.Handle, enable);
if (enable == PSMoveBool.True)
{
PsMoveApi.psmove_reset_orientation(_motionController.Handle);
oriented = PsMoveApi.psmove_has_orientation(_motionController.Handle) == PSMoveBool.True;
if (oriented)
{
float qw = 0.0f, qx = 0.0f, qy = 0.0f, qz = 0.0f;
PsMoveApi.psmove_get_orientation(_motionController.Handle, out qw, out qx, out qy, out qz);
m_orientationFix = new Quaternion(-qx, -qy, -qz, qw);
}
}
return(oriented);
}
public void UpdateTracker()
{
if (_camera.Handle != IntPtr.Zero)
{
try
{
swGrab.Restart();
PsMoveApi.psmove_tracker_update_image(_camera.Handle);
swGrab.Stop();
foreach (MotionControllerModel mc in _camera.Controllers)
{
if (mc.Id == 0)
{
swController1.Restart();
}
if (mc.Id == 1)
{
swController2.Restart();
}
if (mc.Tracking[_camera])
{
PsMoveApi.psmove_tracker_update(_camera.Handle, mc.Handle);
ProcessData(mc);
}
if (mc.Id == 0)
{
swController1.Stop();
}
if (mc.Id == 1)
{
swController2.Stop();
}
}
}
catch (AccessViolationException e)
{
Console.WriteLine(e.StackTrace);
Stop();
}
}
}
public void UpdateImage()
{
if (_camera.Handle != IntPtr.Zero)
{
if (!_camera.ShowImage)
{
return;
}
//display useful information
if (_camera.Annotate)
{
PsMoveApi.psmove_tracker_annotate(_camera.Handle);
}
//retrieve and convert image frame
Image <Bgr, Byte> img = GetImage();
if (OnImageReady != null)
{
OnImageReady(this, new OnImageReadyEventArgs(img));
}
if (_camera.Debug)
{
DrawCubeToImage(img);
// draw center of image for calibration
img.Draw(new Rectangle(315, 235, 10, 10), new Bgr(0, 255, 0), 1);
}
BitmapSource bitmapSource = BitmapHelper.ToBitmapSource(img);
//_camera.ImageSource = (BitmapSource) Emgu.CV.WPF.BitmapSourceConvert.ToBitmapSource(new Image<Bgr, byte>(rgb32Image.width, rgb32Image.height, rgb32Image.widthStep, rgb32Image.imageData)).GetAsFrozen();
//display image
//System.Drawing.Bitmap bitmap = MIplImagePointerToBitmap(rgb32Image);
//BitmapSource bitmapSource = loadBitmap(bitmap);
bitmapSource.Freeze();
_camera.ImageSource = bitmapSource;
}
}
public async void StartMagnetometerCalibrationTask(MetroWindow window)
{
CancelUpdateTask();
_ctsMagnetometerCalibration = new CancellationTokenSource();
var controller = await window.ShowProgressAsync("Magnetometer Calibration", null, true);
CancellationToken token = _ctsMagnetometerCalibration.Token;
try
{
await Task.Run(() =>
{
PsMoveApi.psmove_reset_magnetometer_calibration(_motionController.Handle);
int oldRange = 0;
bool calibrationFinished = false;
Color color = _motionController.Color;
while (!token.IsCancellationRequested && !calibrationFinished)
{
while (PsMoveApi.psmove_poll(_motionController.Handle) > 0)
{
float ax, ay, az;
PsMoveApi.psmove_get_magnetometer_vector(_motionController.Handle, out ax, out ay, out az);
int range = PsMoveApi.psmove_get_magnetometer_calibration_range(_motionController.Handle);
MagnetometerCalibrationProgress = 100 * range / 320;
if (MagnetometerCalibrationProgress > 100)
{
MagnetometerCalibrationProgress = 100;
}
else if (MagnetometerCalibrationProgress < 0)
{
MagnetometerCalibrationProgress = 0;
}
controller.SetProgress(MagnetometerCalibrationProgress / 100.0);
float r = (color.r / 100) * MagnetometerCalibrationProgress;
float g = (color.g / 100) * MagnetometerCalibrationProgress;
float b = (color.b / 100) * MagnetometerCalibrationProgress;
SetLED(new Color(r, g, b));
PsMoveApi.psmove_update_leds(_motionController.Handle);
if (controller.IsCanceled)
{
CancelMagnetometerCalibrationTask();
}
if (range >= 320)
{
if (oldRange > 0)
{
PsMoveApi.psmove_save_magnetometer_calibration(_motionController.Handle);
calibrationFinished = true;
break;
}
}
else if (range > oldRange)
{
controller.SetMessage(string.Format("Rotate the controller in all directions: {0}%...",
MagnetometerCalibrationProgress));
oldRange = range;
}
}
}
});
}
catch (OperationCanceledException)
{
}
catch (Exception ex)
{
Console.WriteLine(ex.StackTrace);
}
await controller.CloseAsync();
if (controller.IsCanceled)
{
await window.ShowMessageAsync("Magnetometer Calibration", "Calibration has been cancelled.");
}
else
{
await window.ShowMessageAsync("Magnetometer Calibration", "Calibration finished successfully.");
}
}
public void DoBundleAdjust()
{
// N = cameras
// M = point count
//public static void BundleAdjust(MCvPoint3D64f[M] points, // Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment
// MCvPoint2D64f[M][N] imagePoints, // Projections of 3d points for every camera
// int[M][N] visibility, // Visibility of 3d points for every camera
// Matrix<double>[N] cameraMatrix, // Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment
// Matrix<double>[N] R, // rotation matrices of all cameras (input and output), values will be modified by bundle adjustment
// Matrix<double>[N] T, // translation vector of all cameras (input and output), values will be modified by bundle adjustment
// Matrix<double>[N] distCoefficients, // distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment
// MCvTermCriteria termCrit) // Termination criteria, a reasonable value will be (30, 1.0e-12)
_stopwatchGet.Restart();
if (_cameras.Cameras.Count == 0)
{
return;
}
IEnumerable <CameraModel> orderedCameras = _cameras.Cameras.OrderBy(camera => camera.Calibration.Index);
ObservableCollection <MotionControllerModel> controllers = _cameras.Cameras[0].Controllers;
if (controllers.Count == 0)
{
return;
}
float radius = CameraCalibrationModel.SPHERE_RADIUS_CM;
int cameraCount = _cameras.Cameras.Count;
int pointCount = 8;
MCvPoint3D64f[] objectPoints = new MCvPoint3D64f[controllers.Count * pointCount];
MCvPoint2D64f[][] imagePoints = new MCvPoint2D64f[cameraCount][];
int[][] visibility = new int[cameraCount][];
Matrix <double>[] cameraMatrix = new Matrix <double> [cameraCount];
Matrix <double>[] R = new Matrix <double> [cameraCount];
Matrix <double>[] T = new Matrix <double> [cameraCount];
Matrix <double>[] distCoefficients = new Matrix <double> [cameraCount];
MCvTermCriteria termCrit = new MCvTermCriteria(30, 1.0e-12);
int visible = 0;
foreach (CameraModel camera in orderedCameras)
{
visibility[camera.Calibration.Index] = new int[controllers.Count * pointCount];
cameraMatrix[camera.Calibration.Index] = camera.Calibration.IntrinsicParameters.IntrinsicMatrix.Clone();
distCoefficients[camera.Calibration.Index] = camera.Calibration.IntrinsicParameters.DistortionCoeffs.Clone();
imagePoints[camera.Calibration.Index] = new MCvPoint2D64f[controllers.Count * pointCount];
R[camera.Calibration.Index] = camera.Calibration.RotationToWorld.Clone();
T[camera.Calibration.Index] = camera.Calibration.TranslationToWorld.Clone();
foreach (MotionControllerModel controller in controllers)
{
float x = controller.RawPosition[camera].x;
float y = controller.RawPosition[camera].y;
//if (x == 0 && y == 0) return;
// controller is not visible
if (controller.TrackerStatus[camera] != PSMoveTrackerStatus.Tracking)
{
for (int i = 0; i < pointCount; i++)
{
visibility[camera.Calibration.Index][i + controller.Id * pointCount] = 0;
}
}
// controller is visible
else
{
Vector3[] history = controller.PositionHistory[camera];
float avgMagnitude = 0f;
for (int i = 1; i < history.Length; i++)
{
avgMagnitude += history[i].magnitude / (history.Length - 1);
}
// check deviation of newest position
if ((Math.Abs(((history[0].magnitude * 100) / avgMagnitude)) - 100) > 5)
{
for (int i = 0; i < pointCount; i++)
{
visibility[camera.Calibration.Index][i + controller.Id * pointCount] = 0;
}
continue;
}
visible++;
//double distance = 0.0;
int startIndex = controller.Id * pointCount;
//MCvPoint3D64f cameraPositionInWorld = new MCvPoint3D64f
//{
// x = camera.Calibration.TranslationToWorld[0, 0],
// y = camera.Calibration.TranslationToWorld[1, 0],
// z = camera.Calibration.TranslationToWorld[2, 0]
//};
// set visibility and calculate distance of the controller relative to the camera
for (int i = startIndex; i < pointCount * controllers.Count; i++)
{
visibility[camera.Calibration.Index][i] = 1;
//double d = CvHelper.GetDistanceToPoint(cameraPositionInWorld,objectPoints[i]);
//distance += d / pointCount;
}
// initialize object's world coordinates
// calculate as the average of each camera's transformed world coordinate
float wx = controller.WorldPosition[camera].x;
float wy = controller.WorldPosition[camera].y;
float wz = controller.WorldPosition[camera].z;
objectPoints[startIndex] += new MCvPoint3D32f(wx - radius, wy - radius, wz - radius);
objectPoints[startIndex + 1] += new MCvPoint3D32f(wx + radius, wy - radius, wz - radius);
objectPoints[startIndex + 2] += new MCvPoint3D32f(wx + radius, wy + radius, wz - radius);
objectPoints[startIndex + 3] += new MCvPoint3D32f(wx - radius, wy + radius, wz - radius);
objectPoints[startIndex + 4] += new MCvPoint3D32f(wx - radius, wy + radius, wz + radius);
objectPoints[startIndex + 5] += new MCvPoint3D32f(wx + radius, wy + radius, wz + radius);
objectPoints[startIndex + 6] += new MCvPoint3D32f(wx + radius, wy - radius, wz + radius);
objectPoints[startIndex + 7] += new MCvPoint3D32f(wx - radius, wy - radius, wz + radius);
//imagePoints[scvm.Camera.Calibration.Index] = Utils.GetImagePoints(mcvm.MotionController.RawPosition[scvm.Camera]);
imagePoints[camera.Calibration.Index] = Array.ConvertAll(camera.Calibration.ObjectPointsProjected, CvHelper.PointFtoPoint2D);
}
} // foreach controller
} // foreach camera
if (visible == 0)
{
return;
}
// average object points
for (int i = 0; i < objectPoints.Length; i++)
{
objectPoints[i].x /= visible;
objectPoints[i].y /= visible;
objectPoints[i].z /= visible;
}
// calculate object's middle
float prex = 0, prey = 0, prez = 0;
for (int i = 0; i < objectPoints.Length; i++)
{
prex += (float)objectPoints[i].x / objectPoints.Length;
prey += (float)objectPoints[i].y / objectPoints.Length;
prez += (float)objectPoints[i].z / objectPoints.Length;
}
_stopwatchBA.Restart();
//LevMarqSparse.BundleAdjust(objectPoints, imagePoints, visibility, cameraMatrix, R, T, distCoefficients, termCrit);
_stopwatchBA.Stop();
_stopwatchSet.Restart();
// check for calucation error
for (int i = 0; i < objectPoints.Length; i++)
{
if (objectPoints[i].x.ToString().Equals("NaN"))
{
return;
}
if (objectPoints[i].y.ToString().Equals("NaN"))
{
return;
}
if (objectPoints[i].z.ToString().Equals("NaN"))
{
return;
}
}
// save changed matrices
foreach (CameraModel camera in orderedCameras)
{
if (visibility[camera.Calibration.Index][0] == 1)
{
//RotationVector3D rot1 = new RotationVector3D();
//rot1.RotationMatrix = camera.Calibration.RotationToWorld;
//RotationVector3D rot2 = new RotationVector3D();
//rot2.RotationMatrix = R[camera.Calibration.Index];
//Console.WriteLine((int)(rot1[0, 0] * (180 / Math.PI)) + " " + (int)(rot2[0, 0] * (180 / Math.PI)));
//Console.WriteLine((int)(rot1[1, 0] * (180 / Math.PI)) + " " + (int)(rot2[1, 0] * (180 / Math.PI)));
//Console.WriteLine((int)(rot1[2, 0] * (180 / Math.PI)) + " " + (int)(rot2[2, 0] * (180 / Math.PI)) + Environment.NewLine);
//camera.Calibration.IntrinsicParameters.IntrinsicMatrix = cameraMatrix[camera.Calibration.Index];
//camera.Calibration.RotationToWorld = R[camera.Calibration.Index];
//camera.Calibration.TranslationToWorld = T[camera.Calibration.Index];
//camera.Calibration.IntrinsicParameters.DistortionCoeffs = distCoefficients[camera.Calibration.Index];
//camera.Calibration.XAngle = (int)(rot2[0, 0] * (180 / Math.PI));
//camera.Calibration.YAngle = (int)(rot2[1, 0] * (180 / Math.PI));
//camera.Calibration.ZAngle = (int)(rot2[2, 0] * (180 / Math.PI));
}
}
// calculate object's middle
float preCenterX = 0, preCenterY = 0, preCenterZ = 0;
for (int i = 0; i < objectPoints.Length; i++)
{
preCenterX += (float)objectPoints[i].x / objectPoints.Length;
preCenterY += (float)objectPoints[i].y / objectPoints.Length;
preCenterZ += (float)objectPoints[i].z / objectPoints.Length;
}
Vector3 prePosition = new Vector3(preCenterX, -preCenterY, preCenterZ);
if (prePosition != _positionHistory[0])
{
for (int i = _positionHistory.Length - 1; i > 0; --i)
{
_positionHistory[i] = _positionHistory[i - 1];
}
_positionHistory[0] = prePosition;
}
//Vector3 avgPosition = Vector3.zero;
//for (int i = 0; i < _positionHistory.Length; i++)
//{
// avgPosition += _positionHistory[i] / _positionHistory.Length;
//}
// 0 predition, 1 correction / estimated
Matrix <float> kalmanResults = FilterPoints(_kalmanXYZ, prePosition.x, prePosition.y, prePosition.z);
Vector3 kalmanPosition = new Vector3(kalmanResults[1, 0], kalmanResults[1, 1], kalmanResults[1, 2]);
_cameras.Position = kalmanPosition;
_stopwatchGet.Stop();
_stopwatchSet.Stop();
for (int i = 0; i < 4; i++)
{
CameraModel camera = _cameras.Cameras[i];
float xr = controllers[0].RawPosition[camera].x;
float yr = controllers[0].RawPosition[camera].y;
float zr = controllers[0].RawPosition[camera].z;
float xc = controllers[0].CameraPosition[camera].x;
float yc = controllers[0].CameraPosition[camera].y;
float zc = controllers[0].CameraPosition[camera].z;
string str = String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3},{4},{5},{6},{7},{8}",
iteration,
xr,
yr,
zr,
PsMoveApi.psmove_tracker_distance_from_radius(camera.Handle, controllers[0].RawPosition[camera].z),
xc,
yc,
zc,
Math.Sqrt(xc * xc + yc * yc + zc * zc)
);
if (camera.Calibration.Index == 0)
{
if (csv0.Count > 0 && csv0[csv0.Count - 1].Contains(zr.ToString(new CultureInfo("en-US"))))
{
}
else
{
csv0.Add(str);
}
}
else if (camera.Calibration.Index == 1)
{
if (csv1.Count > 0 && csv1[csv1.Count - 1].Contains(zr.ToString(new CultureInfo("en-US"))))
{
}
else
{
csv1.Add(str);
}
}
else if (camera.Calibration.Index == 2)
{
if (csv2.Count > 0 && csv2[csv2.Count - 1].Contains(zr.ToString(new CultureInfo("en-US"))))
{
}
else
{
csv2.Add(str);
}
}
else if (camera.Calibration.Index == 3)
{
if (csv3.Count > 0 && csv3[csv3.Count - 1].Contains(zr.ToString(new CultureInfo("en-US"))))
{
}
else
{
csv3.Add(str);
}
}
}
csvTime.Add(String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3}",
iteration,
_stopwatchGet.ElapsedMilliseconds,
_stopwatchBA.ElapsedMilliseconds,
_stopwatchSet.ElapsedMilliseconds));
string strBA = String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3},{4},{5},{6},{7},{8}",
iteration,
prePosition.x,
prePosition.y,
prePosition.z,
Math.Sqrt(prePosition.x * prePosition.x + prePosition.y * prePosition.y + prePosition.z * prePosition.z),
kalmanPosition.x,
kalmanPosition.y,
kalmanPosition.z,
Math.Sqrt(kalmanPosition.x * kalmanPosition.x + kalmanPosition.y * kalmanPosition.y + kalmanPosition.z * kalmanPosition.z));
if (csvBA.Count > 0 && csvBA[csvBA.Count - 1].Contains(prePosition.x.ToString(new CultureInfo("en-US"))))
{
}
else
{
csvBA.Add(strBA);
}
iteration++;
if (csvBA.Count == 100)
{
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\avg_time.csv", csvTime);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance.csv", csvBA);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance0.csv", csv0);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance1.csv", csv1);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance2.csv", csv2);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance3.csv", csv3);
}
}
public void ProcessData(MotionControllerModel mc)
{
if (_camera.Handle != IntPtr.Zero)
{
Vector3 rawPosition = Vector3.zero;
Vector3 fusionPosition = Vector3.zero;
PSMoveTrackerStatus trackerStatus = mc.TrackerStatus[_camera];
if (!mc.Design)
{
trackerStatus = PsMoveApi.psmove_tracker_get_status(_camera.Handle, mc.Handle);
}
if (trackerStatus == PSMoveTrackerStatus.Tracking)
{
float rx, ry, rrad;
float fx, fy, fz;
PsMoveApi.psmove_tracker_get_position(_camera.Handle, mc.Handle, out rx, out ry, out rrad);
PsMoveApi.psmove_fusion_get_position(_camera.Fusion, mc.Handle, out fx, out fy, out fz);
rx = (int)(rx + 0.5);
ry = (int)(ry + 0.5);
rawPosition = new Vector3(rx, ry, rrad);
fusionPosition = new Vector3(fx, fy, fz);
}
else if (mc.Design)
{
switch (_camera.Calibration.Index)
{
case 0:
rawPosition = new Vector3(129, 280, 8.970074f);
break;
case 1:
rawPosition = new Vector3(180, 293, 11.9714022f);
break;
case 2:
rawPosition = new Vector3(528, 286, 9.038924f);
break;
case 3:
rawPosition = new Vector3(389, 275, 6.530668f);
break;
}
}
mc.TrackerStatus[_camera] = trackerStatus;
mc.RawPosition[_camera] = rawPosition;
mc.FusionPosition[_camera] = fusionPosition;
if (trackerStatus == PSMoveTrackerStatus.Tracking || mc.Design)
{
// controller position -> rectangle in surrounding the sphere in image coordinates
PointF[] imgPts = CvHelper.GetImagePointsF(mc.RawPosition[_camera]);
ExtrinsicCameraParameters ex = CameraCalibration.FindExtrinsicCameraParams2(
_camera.Calibration.ObjectPoints2D,
imgPts,
_camera.Calibration.IntrinsicParameters);
Matrix <double> coordinatesInCameraSpace_homo = new Matrix <double>(new double[]
{
ex.TranslationVector[0, 0],
ex.TranslationVector[1, 0],
ex.TranslationVector[2, 0],
1
});
mc.CameraPosition[_camera] = new Vector3(
(float)coordinatesInCameraSpace_homo[0, 0],
(float)coordinatesInCameraSpace_homo[1, 0],
(float)coordinatesInCameraSpace_homo[2, 0]);
ex.RotationVector[0, 0] += (Math.PI / 180) * (_camera.Calibration.RotX + _camera.Calibration.XAngle);
ex.RotationVector[1, 0] += (Math.PI / 180) * (_camera.Calibration.RotY + _camera.Calibration.YAngle);
ex.RotationVector[2, 0] += (Math.PI / 180) * (_camera.Calibration.RotZ + _camera.Calibration.ZAngle);
_camera.Calibration.ExtrinsicParameters[mc.Id] = ex;
Matrix <double> minusRotation = new Matrix <double>(3, 3);
minusRotation = CvHelper.Rotate(
-_camera.Calibration.RotX - _camera.Calibration.XAngle,
-_camera.Calibration.RotY - _camera.Calibration.YAngle,
-_camera.Calibration.RotZ - _camera.Calibration.ZAngle);
Matrix <double> R3x3_cameraToWorld = new Matrix <double>(3, 3);
R3x3_cameraToWorld = CvHelper.Rotate(
_camera.Calibration.RotX,
_camera.Calibration.RotY + _camera.Calibration.YAngle,
_camera.Calibration.RotZ);
Matrix <double> rotInv = new Matrix <double>(3, 3);
CvInvoke.cvInvert(ex.RotationVector.RotationMatrix.Ptr, rotInv, SOLVE_METHOD.CV_LU);
Matrix <double> test = CvHelper.ConvertToHomogenous(-1 * R3x3_cameraToWorld);
_camera.Calibration.ObjectPointsProjected = CameraCalibration.ProjectPoints(
_camera.Calibration.ObjectPoints3D,
_camera.Calibration.ExtrinsicParameters[mc.Id],
_camera.Calibration.IntrinsicParameters);
Matrix <double> cameraPositionInWorldSpace4x4 = new Matrix <double>(new double[, ]
{
{ 1, 0, 0, _camera.Calibration.TranslationToWorld[0, 0] },
{ 0, 1, 0, _camera.Calibration.TranslationToWorld[1, 0] },
{ 0, 0, 1, _camera.Calibration.TranslationToWorld[2, 0] },
{ 0, 0, 0, 1 },
});
Matrix <double> Rt_homo = CvHelper.ConvertToHomogenous(R3x3_cameraToWorld);
Matrix <double> x_world_homo = CvHelper.ConvertToHomogenous(minusRotation) * coordinatesInCameraSpace_homo;
Rt_homo[0, 3] = x_world_homo[0, 0];
Rt_homo[1, 3] = x_world_homo[1, 0];
Rt_homo[2, 3] = x_world_homo[2, 0];
x_world_homo = cameraPositionInWorldSpace4x4 * x_world_homo;
Vector3 v3world = new Vector3((float)x_world_homo[0, 0], (float)x_world_homo[1, 0],
(float)x_world_homo[2, 0]);
mc.WorldPosition[_camera] = v3world;
for (int i = mc.PositionHistory[_camera].Length - 1; i > 0; --i)
{
mc.PositionHistory[_camera][i] = mc.PositionHistory[_camera][i - 1];
}
mc.PositionHistory[_camera][0] = v3world;
}
}
} // ProcessData
/// <summary>
/// Process all the raw data on the Playstation Move controller
/// </summary>
protected virtual void ProcessData()
{
UpdateButtons();
{
int x = 0, y = 0, z = 0;
PsMoveApi.psmove_get_accelerometer(_motionController.Handle, out x, out y, out z);
_motionController.RawAccelerometer = new Vector3(x, y, z);
}
{
float x = 0, y = 0, z = 0;
PsMoveApi.psmove_get_accelerometer_frame(_motionController.Handle, PSMoveFrame.SecondHalf, out x, out y, out z);
_motionController.Accelerometer = new Vector3(x, y, z);
}
{
int x = 0, y = 0, z = 0;
PsMoveApi.psmove_get_gyroscope(_motionController.Handle, out x, out y, out z);
_motionController.RawGyroscope = new Vector3(x, y, z);
}
{
float x = 0, y = 0, z = 0;
PsMoveApi.psmove_get_gyroscope_frame(_motionController.Handle, PSMoveFrame.SecondHalf, out x, out y, out z);
_motionController.Gyroscope = new Vector3(x, y, z);
}
if (PsMoveApi.psmove_has_orientation(_motionController.Handle) == PSMoveBool.True)
{
float w = 0.0f, x = 0.0f, y = 0.0f, z = 0.0f;
PsMoveApi.psmove_get_orientation(_motionController.Handle, out w, out x, out y, out z);
Quaternion rot = new Quaternion(x, y, z, w);
rot = rot * m_orientationFix;
#if YISUP
Vector3 euler = rot.eulerAngles;
rot = Quaternion.Euler(-euler.x, -euler.y, euler.z);
#endif
m_orientation = rot;
_motionController.Orientation = rot;
}
else
{
m_orientation = Quaternion.identity;
}
{
int x = 0, y = 0, z = 0;
PsMoveApi.psmove_get_magnetometer(_motionController.Handle, out x, out y, out z);
// TODO: Should these values be converted into a more human-understandable range?
_motionController.Magnetometer = new Vector3(x, y, z);
}
_motionController.BatteryLevel = PsMoveApi.psmove_get_battery(_motionController.Handle);
_motionController.Temperature = PsMoveApi.psmove_get_temperature_in_celsius(_motionController.Handle);
PsMoveApi.psmove_set_rumble(_motionController.Handle, (byte)(_motionController.Rumble / 255f));
}
public MotionControllerService()
{
PsMoveApi.psmove_set_remote_config(_remoteConfig);
}