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