private void SendPacketNotification(string jsonPacket) { var packet = JsonUtility.FromJson<OptitrackPacket>(jsonPacket); var poses = new List<OptitrackPose>(); foreach (var body in packet.Rigidbodies) { var pose = new OptitrackPose(); pose.DetectionTime = Time.unscaledTime; pose.Id = body.Id; pose.RigidbodyName = body.Name; pose.Position = new Vector3(body.X, body.Y, body.Z); pose.Rotation = new Quaternion(body.QX, body.QY, body.QZ, body.QW); foreach (var marker in body.Markers) { pose.Markers.Add(new OptitrackPose.Marker { Id = marker.Id, Position = new Vector3(marker.X, marker.Y, marker.Z) }); } } if (PosesReceived != null) { PosesReceived(poses); } }
private void CalibrateMarker(OptitrackPose pose) { var anchorMarker = pose.Markers.First((m) => m.Id == AnchorId); var span1Marker = pose.Markers.First((m) => m.Id == Span1Id); var span2Marker = pose.Markers.First((m) => m.Id == Span2Id); var normalVector = Vector3.Normalize(Vector3.Cross(span1Marker.Position - anchorMarker.Position, span2Marker.Position - anchorMarker.Position)); var rotationAxis = Vector3.Normalize(Vector3.Cross(normalVector, -OptitrackCalibratorObject.up)); var rotationAngle = Vector3.Dot(normalVector, -OptitrackCalibratorObject.up); Quaternion rotation = Quaternion.AngleAxis(rotationAngle, rotationAxis); //var ground = OptitrackCalibratorObject; //var groundPlane = new Plane(ground.position, ground.position + ground.forward, ground.position + ground.right); //var localUpVector = OptitrackCalibratorObject.up; //// assume that point is *above* plain, no idea if that's important in unity's algorithm //float distanceAnchorPlane = 0f; //groundPlane.Raycast(new Ray(anchorMarker.Position, -localUpVector), out distanceAnchorPlane); //float distanceSpan1Plane = 0f; //groundPlane.Raycast(new Ray(span1Marker.Position, -localUpVector), out distanceSpan1Plane); //// bring span1marker position to the same height (based on groundplane) as anchorMarker //var angleA1 = var markerSetup = GetComponent<MultiMarker_MarkerSetup>(); markerSetup.CalibratorToMarkerRotOffset = rotation.eulerAngles; var markerSize = ArucoListener.Instance.MarkerSizeInMeter; markerSetup.CalibratorToMarkerPosOffset = new Vector3(-markerSize/2f, 0, -markerSize/2f); }
private void OnOptitrackPose(List <OptitrackPose> poses) { var matchingPose = poses.FirstOrDefault((p) => p.RigidbodyName == CalibratorName); if (matchingPose != null) { _calibratorPose = matchingPose; } }
private static void SendPacketNotification(string jsonPacket) { var packet = JsonUtility.FromJson <OptitrackPacket>(jsonPacket); var poses = new List <OptitrackPose>(); foreach (var body in packet.Rigidbodies) { var pose = new OptitrackPose(); pose.DetectionTime = Time.unscaledTime; pose.Id = body.Id; pose.RigidbodyName = body.Name; // right to left handed conversion pose.Position = new Vector3(body.X, body.Y, -body.Z) * Globals.OptitrackScaling; // z -> -z pose.Rotation = new Quaternion(body.QX, body.QY, -body.QZ, -body.QW); // qz -> -qz, qw -> -qw foreach (var marker in body.Markers) { pose.Markers.Add(new OptitrackPose.Marker { Id = marker.Id, // right to left handed conversion Position = new Vector3(marker.X, marker.Y, -marker.Z) * Globals.OptitrackScaling }); } poses.Add(pose); _detectedPoses[body.Name] = pose; } if (PosesReceived != null) { PosesReceived(poses); } }
private void OnOptitrackPose(List<OptitrackPose> poses) { foreach (var pose in poses) { if (pose.RigidbodyName == OptitrackCameraName) { _optitrackCameraPose = pose; } } }
private void OnOptitrackPose(List<OptitrackPose> poses) { var matchingPose = poses.FirstOrDefault((p) => p.RigidbodyName == CalibratorName); if (matchingPose != null) { _calibratorPose = matchingPose; } }
private void OnOptitrackPose(List<OptitrackPose> poses) { foreach (var pose in poses) { if (pose.RigidbodyName == OptitrackCameraName) { _optitrackCameraPose = pose; _optitrackCameraDetectionTime = Time.unscaledTime; } } }
private void OnOptitrackPose(List<OptitrackPose> poses) { foreach (var pose in poses) { if (pose.RigidbodyName == OptitrackCameraName) { // TODO: compare markers etc to determine steadiness if (_optitrackCameraPose == null) { HasSteadyOptitrackCameraPose = false; } else { HasSteadyOptitrackCameraPose = true; } _optitrackCameraPose = pose; // TODO thresholding etc HasSteadyOptitrackCameraPose = true; break; } } }
private void SaveOptitrackPose(OptitrackPose newPose, ref OptitrackPose memberPose, ref bool steadyIndicator) { if (memberPose == null) { steadyIndicator = false; } else { //var prevPose = memberPose; // TODO: compare markers etc... steadyIndicator = true; } memberPose = newPose; }