public void AddMotionController(MotionControllerModel mc) { if (!_camera.Controllers.Contains(mc)) { _camera.Controllers.Add(mc); } }
public MotionControllerModel Initialize(int id) { _motionController = new MotionControllerModel(); Init(id); return _motionController; }
public RemoveMotionControllerMessage(MotionControllerModel motionController) { MotionController = motionController; }
/// <summary> /// Initializes a new instance of the MotionControllerViewModel class. /// </summary> public MotionControllerViewModel(MotionControllerModel mc, IMotionControllerService mcs) { if (!IsInDesignMode) { _motionController = mc; _motionControllerService = mcs; Palette = Palette.Create(new RGBColorWheel(), System.Windows.Media.Colors.Red, PaletteSchemaType.Analogous, 1); MotionControllerService.Initialize(MotionController); DoSelectColor(Palette); Messenger.Default.Register<AddCameraMessage>(this, message => { // initialize entries for newly added camera if (!MotionController.Tracking.ContainsKey(message.Camera)) { MotionController.TrackerStatus.Add(message.Camera, PSMoveTrackerStatus.NotCalibrated); MotionController.Tracking.Add(message.Camera, false); MotionController.RawPosition.Add(message.Camera, Float3.Zero); MotionController.WorldPosition.Add(message.Camera, Float3.Zero); MotionController.CameraPosition.Add(message.Camera, Float3.Zero); MotionController.FusionPosition.Add(message.Camera, Float3.Zero); MotionController.PositionHistory.Add(message.Camera, new Vector3[5]); } }); Messenger.Default.Register<RemoveCameraMessage>(this, message => { // remove entries corresponding to recently removed camera MotionController.TrackerStatus.Remove(message.Camera); MotionController.Tracking.Remove(message.Camera); MotionController.RawPosition.Remove(message.Camera); MotionController.WorldPosition.Remove(message.Camera); MotionController.CameraPosition.Remove(message.Camera); MotionController.FusionPosition.Remove(message.Camera); MotionController.PositionHistory.Remove(message.Camera); }); if (mc.Serial != null) { // register with service locator SimpleIoc.Default.Register(() => this, mc.Serial, true); // notify existing cameras to add this controller Messenger.Default.Send(new AddMotionControllerMessage(MotionController)); } // initialize existing cameras foreach (CameraViewModel scvm in SimpleIoc.Default.GetAllCreatedInstances<CameraViewModel>()) { MotionController.RawPosition.Add(scvm.Camera, Float3.Zero); MotionController.FusionPosition.Add(scvm.Camera, Float3.Zero); MotionController.CameraPosition.Add(scvm.Camera, Float3.Zero); MotionController.WorldPosition.Add(scvm.Camera, Float3.Zero); MotionController.Tracking.Add(scvm.Camera, false); MotionController.TrackerStatus.Add(scvm.Camera, PSMoveTrackerStatus.NotCalibrated); MotionController.PositionHistory.Add(scvm.Camera, new Vector3[5]); } } }
/// <summary> /// for design purposes only /// </summary> public MotionControllerViewModel() : this(new MotionControllerModel { Design = true, Id = MotionControllerModel.COUNTER, Name = "Design " + MotionControllerModel.COUNTER, Serial = "00:00:00:00:00:0" + MotionControllerModel.COUNTER++, }, new DesignMotionControllerService()) { #if DEBUG Random rnd = new Random(); if (IsInDesignMode) { CameraModel cam0 = new CameraModel { Name = "cam0" }; CameraModel cam1 = new CameraModel { Name = "cam1" }; MotionController = new MotionControllerModel { Name = "Design " + rnd.Next(10), Serial = "00:00:00:00:00:0" + rnd.Next(10), RawPosition = new ObservableConcurrentDictionary<CameraModel, Vector3> { {cam0, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))}, {cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))} }, FusionPosition = new ObservableConcurrentDictionary<CameraModel, Vector3> { {cam0, new Vector3(rnd.Next(-50, 50), rnd.Next(-50, 50), rnd.Next(-50, 50))}, {cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))} }, CameraPosition = new ObservableConcurrentDictionary<CameraModel, Vector3> { {cam0, new Vector3(rnd.Next(-50, 50), rnd.Next(-50, 50), rnd.Next(-50, 50))}, {cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))} }, WorldPosition = new ObservableConcurrentDictionary<CameraModel, Vector3> { {cam0, new Vector3(rnd.Next(-50, 50), rnd.Next(-50, 50), rnd.Next(-50, 50))}, {cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))} } }; } MotionController.Circle = rnd.Next(2) > 0; MotionController.Cross = rnd.Next(2) > 0; MotionController.Triangle = rnd.Next(2) > 0; MotionController.Square = rnd.Next(2) > 0; MotionController.Start = rnd.Next(2) > 0; MotionController.Select = rnd.Next(2) > 0; MotionController.Move = rnd.Next(2) > 0; MotionController.PS = rnd.Next(2) > 0; MotionController.Trigger = rnd.Next(256); #endif }
public AddMotionControllerMessage(MotionControllerModel motionController) { MotionController = motionController; }
public MotionControllerModel Initialize(MotionControllerModel motionController) { return _motionController = motionController; }
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; }
public void RemoveMotionController(MotionControllerModel mc) { _camera.Controllers.Remove(mc); }
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
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)); }
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 void RemoveMotionController(MotionControllerModel mc) { if(_camera.Controllers.Contains(mc)) { if(mc.Tracking[_camera]) { DisableTracking(mc); } _camera.Controllers.Remove(mc); } }
public MotionControllerModel Initialize(int id) { return MotionController = new MotionControllerModel(); }