예제 #1
0
        public void GetAnimatedPoseTest()
        {
            IpObject target = CreateIpObject(); // TODO: Initialize to an appropriate value

            target.SetKeyframe(0);
            target.Position    = new Point3D(1, 1, 1);
            target.Orientation = new AxisAngle3D()
            {
                X = 1,
                Y = 1,
                Z = 1
            };
            target.SetKeyframe(10);

            double time     = 0F; // TODO: Initialize to an appropriate value
            Pose3D expected = new Pose3D()
            {
                Orientation = target.Orientation,
                Position    = target.Position,
                Time        = 10
            };     // TODO: Initialize to an appropriate value
            Pose3D actual;
            var    intermediates = new List <Pose3D>();

            for (int i = 0; i < 10; i++)
            {
                intermediates.Add(target.GetAnimatedPose(i));
            }

            actual = target.GetAnimatedPose(10);
            Assert.AreEqual(expected, actual);
            Assert.Inconclusive("Verify the correctness of this test method.");
        }
예제 #2
0
        public void Jacobian()
        {
            const double ds = 1e-5;
            Pose3D       f  = a.AddOdometry(odometry2);

            double[][] jacobian  = a.AddOdometryJacobian(odometry2);
            double[][] numerical = new double[6][];

            for (int i = 0; i < 6; i++)
            {
                double[] da = new double[6];
                da[i] = ds;

                Pose3D a2 = a.AddGlobal(da);
                //Pose3D   f2 = a.AddOdometry(Pose3D.Identity.FromLinear(odometry2).AddGlobal(da).ToLinear());
                Pose3D   f2 = a2.AddOdometry(odometry2);
                double[] df = f2.SubtractGlobal(f);

                numerical[i] = df.Divide(ds);
            }

            numerical = numerical.Transpose();

            Console.WriteLine(jacobian.ToString("f6"));
            Console.WriteLine("---");
            Console.WriteLine(numerical.ToString("f6"));

            Assert.IsTrue(numerical.IsEqual(jacobian, 1e-4));
        }
예제 #3
0
 // Converts given |position| and |rotation| from Unity space to audio space.
 private static void ConvertAudioTransformFromUnity(ref Vector3 position,
                                                    ref Quaternion rotation)
 {
     transformMatrix = Pose3D.FlipHandedness(Matrix4x4.TRS(position, rotation, Vector3.one));
     position        = transformMatrix.GetColumn(3);
     rotation        = Quaternion.LookRotation(transformMatrix.GetColumn(2), transformMatrix.GetColumn(1));
 }
            public Matrix4x4 ToMatrix4x4()
            {
                Matrix4x4 mat4 = (Matrix4x4)Marshal.PtrToStructure(valuePtr, typeof(Matrix4x4));

                // Transpose the matrix from row-major (GVR) to column-major (Unity),
                // and change from LHS to RHS coordinates.
                return(Pose3D.FlipHandedness(mat4.transpose));
            }
예제 #5
0
    public void SendInfo(int personID, Pose3D headPose)
    {
        InfoMessage msg = new InfoMessage();
        msg.id = personID;
        msg.pose = headPose;

        NetworkServer.SendToAll(MyMsgType.Info, msg);
    }
예제 #6
0
        public void FitGaussianMultipleLandmarks()
        {
            Pose3D linearpoint = new Pose3D();

            double[] estimate = new double[6] {
                -0.002, 0.003, 0.001, 0, 0, 0
            };
            Map map          = new Map(3);
            var measurements = new List <PixelRangeMeasurement>();

            measurements.Add(new PixelRangeMeasurement(115.16312, 0, 1.019803903));
            measurements.Add(new PixelRangeMeasurement(0, 57.58156, 1.004987562));
            measurements.Add(new PixelRangeMeasurement(28.79078, 0, 2.002498439));

            map.Add(new Gaussian(new double[3] {
                0.2, 0, 1
            }, identitycov, 1.0));
            map.Add(new Gaussian(new double[3] {
                0, 0.1, 1
            }, identitycov, 1.0));
            map.Add(new Gaussian(new double[3] {
                0.1, 0, 2
            }, identitycov, 1.0));

            Gaussian expected = new Gaussian(new double[7] {
                0, 0, 0, 1, 0, 0, 0
            },
                                             new double[7][] { new double[7] {
                                                                   1, 0, 0, 0, 0, 0, 0
                                                               },
                                                               new double[7] {
                                                                   0, 1, 0, 0, 0, 0, 0
                                                               },
                                                               new double[7] {
                                                                   0, 0, 1, 0, 0, 0, 0
                                                               },
                                                               new double[7] {
                                                                   0, 0, 0, 1, 0, 0, 0
                                                               },
                                                               new double[7] {
                                                                   0, 0, 0, 0, 1, 0, 0
                                                               },
                                                               new double[7] {
                                                                   0, 0, 0, 0, 0, 1, 0
                                                               },
                                                               new double[7] {
                                                                   0, 0, 0, 0, 0, 0, 1
                                                               } },
                                             1.0);

            Gaussian result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);

            Console.WriteLine(result);

            Assert.AreEqual(expected, result);
        }
예제 #7
0
 private void doEventDataCallback(Pose3D pose3D)
 {
     for (int i = 0; i < Callbacks.Count; i++)
     {
         pose3d_listener gameObjectCallback = Callbacks [i];
         if (gameObjectCallback != null)
         {
             gameObjectCallback(pose3D);
         }
     }
 }
예제 #8
0
        public void QueueReader()
        {
            while (!StopQueue)
            {
                // If the queue is empty, run the delay for checking queue
                if (PoseQueue.Count <= 0)
                {
                    Thread.Sleep(QueueCheckDelay);
                    try
                    {
                        ProcessFiles();
                    }
                    catch (IOException)
                    {
                        Console.WriteLine("IOException. File is probably in-use by another program.");
                    }
                    catch (UnauthorizedAccessException)
                    {
                        Console.WriteLine("UnauthorizedAccessException. File is probably in-use by another program.");
                    }

                    continue;
                }
                else if (PoseQueue.Count > 20)
                {
                    PoseQueue.RemoveRange(0, PoseQueue.Count - 1);
                }

                // Parse saved text
                JObject parsedPose = JObject.Parse(PoseQueue[0]);
                PoseQueue.RemoveAt(0);

                // Do JSON stuff to grab keypoints

                //Console.WriteLine(parsedPose.ToString());

                if (parsedPose["people"].HasValues && parsedPose["people"][0].HasValues)
                {
                    //Console.WriteLine(parsedPose["people"].ToString());

                    //Console.WriteLine(parsedPose["people"][0].Value<JArray>("pose_keypoints_2d"));

                    //foreach (double f in parsedPose["people"][0].Value<JArray>("pose_keypoints_2d").Values<double>())
                    //{
                    //	Console.WriteLine(f);
                    //}

                    double[] keypoints = new List <double>(parsedPose["people"][0].Value <JArray>("pose_keypoints_3d").Values <double>()).ToArray();

                    poseEventHandler.ExecuteHandlers(Pose3D.ParseDoubleArray(keypoints));
                }
            }
        }
예제 #9
0
        public void SubtractAdd()
        {
            Pose3D recovered = b.AddOdometry(a.DiffOdometry(b));

            Assert.AreEqual(a.State[0], recovered.State[0], 1e-3);
            Assert.AreEqual(a.State[1], recovered.State[1], 1e-3);
            Assert.AreEqual(a.State[2], recovered.State[2], 1e-3);
            Assert.AreEqual(a.State[3], recovered.State[3], 1e-3);
            Assert.AreEqual(a.State[4], recovered.State[4], 1e-3);
            Assert.AreEqual(a.State[5], recovered.State[5], 1e-3);
            Assert.AreEqual(a.State[6], recovered.State[6], 1e-3);
        }
예제 #10
0
        public void FitGaussianWrongLocation()
        {
            Pose3D linearpoint = new Pose3D();

            double[] estimate     = new double[6];
            Map      map          = new Map(3);
            var      measurements = new List <PixelRangeMeasurement>();

            estimate[2] = -0.05;
            measurements.Add(new PixelRangeMeasurement(0, 0, 1));

            map.Add(new Gaussian(new double[3] {
                0, 0, 1.05
            }, identitycov, 1.0));

            // this expectation assumes a diagonal measurement error and the identity pose
            SimulatedVehicle dummy = new SimulatedVehicle();
            double           a     = 1.0 / dummy.MeasurementCovariance[0][0];
            double           b     = 1.0 / dummy.MeasurementCovariance[1][1];
            double           c     = 1.0 / dummy.MeasurementCovariance[2][2];
            double           f2    = dummy.Measurer.VisionFocal * dummy.Measurer.VisionFocal;

            double[] expstate = new double[7] {
                0, 0, 0.05, 1, 0, 0, 0
            };
            double[][] expinfo = new double[6][] { new double[6] {
                                                       a *f2, 0, 0, 0, a *f2, 0
                                                   },
                                                   new double[6] {
                                                       0, b *f2, 0, -b * f2, 0, 0
                                                   },
                                                   new double[6] {
                                                       0, 0, c, 0, 0, 0
                                                   },
                                                   new double[6] {
                                                       0, -b * f2, 0, b *f2, 0, 0
                                                   },
                                                   new double[6] {
                                                       a *f2, 0, 0, 0, a *f2, 0
                                                   },
                                                   new double[6] {
                                                       0, 0, 0, 0, 0, 0
                                                   } };

            double[][] expcov = expinfo.SingularInverse();
            Gaussian   result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);

            Console.WriteLine(result.Covariance.ToString("e5"));
            Console.WriteLine(result.Mean.ToString("e5"));

            Assert.IsTrue(expstate.IsEqual(result.Mean, 1e-4));
            Assert.IsTrue(expcov.IsEqualRelative(result.Covariance, 0.001));
        }
예제 #11
0
 public List <List <Vector3> > Process(
     ref Area?xAll,
     ref Area?yAll,
     ref Area?xFrame,
     ref Area?yFrame,
     bool frameMode,
     bool background,
     Pose3D pose
     ) => ProcessInternal(
     ref xAll,
     ref yAll,
     ref xFrame,
     ref yFrame,
     frameMode,
     background,
     p => (pose * p).Let(it => new Vector3(it.X, it.Y, float.NaN)));
예제 #12
0
        public void FitGaussianNoMap()
        {
            Pose3D linearpoint = new Pose3D();

            double[] estimate     = new double[6];
            Map      map          = new Map(3);
            var      measurements = new List <PixelRangeMeasurement>();

            double[][] covariance = MatrixExtensions.Zero(6).SingularInverse();
            Gaussian   expected   = new Gaussian(new double[6] {
                0, 0, 0, 0, 0, 0
            }, covariance, 1.0);

            Gaussian result = LoopyPHDNavigator.FitGaussian(estimate, measurements, map, linearpoint);

            Assert.IsTrue(expected.Equals(result, 1e-5));
        }
예제 #13
0
    // Use this for initialization
    void Start()
    {
        netManager = GameObject.FindObjectOfType <NetworkManager> ();
        netID      = GetComponent <NetworkIdentity> ();

        //  We need this, so a player knows which camera to turn on (their own).
        //  Without it, all players get 1 shared camera, which is, in fact, awful for gameplay.
        if (netID.isLocalPlayer)
        {
            mainCamera.gameObject.SetActive(true);
        }

        nView       = GetComponent <NetworkView> ();
        gameOver    = false;
        devicePose  = Cardboard.SDK.HeadPose;
        startingDir = 0;
        inTurn      = false;
    }
예제 #14
0
        public void FitMeasurementOnlyTranslation()
        {
            PRM3DMeasurer measurer = new PRM3DMeasurer();
            Pose3D        pose0    = Pose3D.Identity;

            var measurement = new PixelRangeMeasurement(0, 0, 1);

            double[] landmark = new double[3] {
                0, 0, 2.5
            };

            Pose3D fitted = measurer.FitToMeasurement(pose0, measurement, landmark);

            Pose3D expected = new Pose3D(new double[3] {
                0, 0, 1.5
            }, Quaternion.Identity);

            Assert.IsTrue(expected.Equals(fitted, 1e-5));
        }
예제 #15
0
        public void setup()
        {
            Quaternion arot = Quaternion.CreateFromYawPitchRoll(0.4, 1.6, 0.1).Normalize();
            Quaternion brot = Quaternion.CreateFromYawPitchRoll(0.4, 0.6, 0.5).Normalize();

            a = new Pose3D(new double[3] {
                0.1, 0.3, 0.2
            }, arot);
            b = new Pose3D(new double[3] {
                0.5, -0.4, 0.7
            }, brot);

            odometry = new double[6] {
                0.12, 2.17, 1.03, 0.21, 0.05, 1.05
            };
            odometry2 = new double[6] {
                0.13, 0.09, 0.05, 0.02, 1.20, 0.20
            };
        }
예제 #16
0
        public void FitMeasurementGeneral()
        {
            PRM3DMeasurer measurer = new PRM3DMeasurer();
            Pose3D        pose0    = new Pose3D(new double[3] {
                2.0, 0.2, 0.1
            },
                                                new Quaternion(1, 2, 3, 4).Normalize());

            var measurement = new PixelRangeMeasurement(-120, 50, 1.3);

            double[] landmark = new double[3] {
                0.1, -1.0, 1.2
            };

            Pose3D fpose = measurer.FitToMeasurement(pose0, measurement, landmark);

            var measured = measurer.MeasurePerfect(fpose, landmark);

            Assert.IsTrue(measurement.ToLinear().IsEqual(measured.ToLinear(), 1e-5));
        }
        public void Show(Matrix4x4 userMatrix, bool useRecommended, float distance, Matrix4x4 model)
        {
#if UNITY_ANDROID && !UNITY_EDITOR
            currentDistance = distance;
#endif  // UNITY_ANDROID && !UNITY_EDITOR

            if (useRecommended)
            {
                worldMatrix = getRecommendedMatrix(distance);
            }
            else
            {
                // Convert to GVR coordinates.
                worldMatrix = Pose3D.FlipHandedness(userMatrix).transpose;
            }
            Matrix4x4 matToSet = worldMatrix * model.transpose;
            IntPtr    mat_ptr  = Marshal.AllocHGlobal(Marshal.SizeOf(matToSet));
            Marshal.StructureToPtr(matToSet, mat_ptr, true);
            gvr_keyboard_set_world_from_keyboard_matrix(keyboard_context, mat_ptr);
            gvr_keyboard_show(keyboard_context);
        }
예제 #18
0
        public void FitMeasurementUnmeasurableLandmark()
        {
            PRM3DMeasurer measurer = new PRM3DMeasurer();
            Pose3D        pose0    = Pose3D.Identity;

            var measurement = new PixelRangeMeasurement(0, 0, 1);

            double[] landmark = new double[3] {
                0, 1, 0
            };

            Pose3D fitted = measurer.FitToMeasurement(pose0, measurement, landmark);

            Quaternion rotation = new Quaternion(Math.Cos(Math.PI / 2 / 2),
                                                 (-Math.Sin(Math.PI / 2 / 2)).Multiply(new double[3] {
                1, 0, 0
            }));

            Pose3D expected = new Pose3D(new double[3] {
                0, 0, 0
            }, rotation);

            Assert.IsTrue(expected.Equals(fitted, 1e-5));
        }
예제 #19
0
 // Update is called once per frame
 void Update()
 {
     headview = Cardboard.SDK.HeadPose;
     GetComponent <Text>().text = "Heading: " + headview.Orientation.eulerAngles;
 }
        public void PoseInterpolate()
        {
            //Console.WriteLine("Test");

            if (eventListener.newPose == null || eventListener.TimeBetweenPoses == 0)
            {
                //Console.WriteLine("Error: Can't run interpolator...");
                return;
            }
            else if (eventListener.curPose == null || eventListener.lastPose == null)
            {
                eventListener.curPose = eventListener.newPose;
            }
            else             // Interpolate
            {
                //Console.WriteLine("Interpolating pose...");

                if (lastPose != eventListener.lastPose)
                {
                    curPoseTimePassed = 0;
                    lastPose          = eventListener.lastPose;
                }

                InterpolatorDelay = (int)Math.Floor(eventListener.TimeBetweenPoses / InterpolatorMultiplier);

                double interpolateDivider = (InterpolatorDelay > 0 ? ((eventListener.TimeBetweenPoses - curPoseTimePassed) / InterpolatorDelay) : 0);
                interpolateDivider = (interpolateDivider < 1 ? 1 : interpolateDivider);

                if (interpolateDivider >= 1 && InterpolatorDelay > 0)
                {
                    //Console.WriteLine("Interpolate Divider: " + interpolateDivider);

                    Pose newPose = eventListener.newPose;

                    if (typeof(Pose2D) == newPose.GetType())
                    {
                        List <KeyPoint2D> newCurPose = new List <KeyPoint2D>();

                        for (int i = 0; i < newPose.KeyPoints.Count; i++)
                        {
                            KeyPoint2D keyPoint = ((Pose2D)newPose).GetKeyPoint2D(i);

                            KeyPoint2D newKeyPoint = InterpolateKeyPoint2Ds(((Pose2D)lastPose).GetKeyPoint2D(keyPoint.BodyPoint), keyPoint, interpolateDivider);

                            if (newKeyPoint != null && newKeyPoint.IsValid)
                            {
                                newCurPose.Add(newKeyPoint);
                            }
                            else
                            {
                                newCurPose.Add(keyPoint);
                            }
                        }

                        Pose2D interPose = new Pose2D(newCurPose.ToArray());

                        foreach (KeyPoint2D keyPoint in newPose.KeyPoints)
                        {
                            if (interPose.GetKeyPoint2D(keyPoint.BodyPoint) == null)
                            {
                                interPose.KeyPoints.Add(keyPoint);
                            }
                        }

                        eventListener.curPose = interPose;
                    }
                    else if (typeof(Pose3D) == newPose.GetType())
                    {
                        List <KeyPoint3D> newCurPose = new List <KeyPoint3D>();

                        for (int i = 0; i < newPose.KeyPoints.Count; i++)
                        {
                            KeyPoint3D keyPoint = ((Pose3D)newPose).GetKeyPoint3D(i);

                            KeyPoint3D newKeyPoint = InterpolateKeyPoint3Ds(((Pose3D)lastPose).GetKeyPoint3D(keyPoint.BodyPoint), keyPoint, interpolateDivider);

                            if (newKeyPoint != null && newKeyPoint.IsValid)
                            {
                                newCurPose.Add(newKeyPoint);
                            }
                            else
                            {
                                newCurPose.Add(keyPoint);
                            }
                        }

                        Pose3D interPose = new Pose3D(newCurPose.ToArray());

                        foreach (KeyPoint3D keyPoint in newPose.KeyPoints)
                        {
                            if (interPose.GetKeyPoint3D(keyPoint.BodyPoint) == null)
                            {
                                interPose.KeyPoints.Add(keyPoint);
                            }
                        }

                        eventListener.curPose = interPose;
                    }
                }
            }

            //Console.WriteLine("Sending pose...");

            SendPose(eventListener.curPose);
        }
        public void SendPose(Pose pose)
        {
            try
            {
                //foreach (KeyPoint point in pose.KeyPoints)
                //{
                //	Console.WriteLine(point.ToString());
                //}

                if (pose != null && typeof(Pose2D) == pose.GetType())
                {
                    Pose2D pose2D = (Pose2D)pose;

                    KeyPoint2D nose = pose2D.GetKeyPoint2D(BodyPoint.Nose_or_Top_Head);
                    //KeyPoint2D leftEye = pose2D.GetKeyPoint2D(BodyPoint.Left_Eye);
                    //KeyPoint2D rightEye = pose2D.GetKeyPoint2D(BodyPoint.Right_Eye);

                    if (nose != null && nose.IsValid)                    //&& leftEye != null && rightEye != null && nose.IsValid && leftEye.IsValid && rightEye.IsValid)
                    {
                        //if (!HeadsetStatus)
                        //{
                        //	program.headTrackingService.ChangeStatus(true);
                        //	HeadsetStatus = true;
                        //}

                        //double height = (leftEye.Y + rightEye.Y) / 2;
                        program.headTrackingService.SendPositionOnly(nose.X, nose.Y, 0);
                    }
                    //else
                    //{
                    //	if (HeadsetStatus)
                    //	{
                    //		program.headTrackingService.ChangeStatus(false);
                    //		HeadsetStatus = false;
                    //	}
                    //}

                    KeyPoint2D rightHand = pose2D.GetKeyPoint2D(BodyPoint.Right_Wrist);

                    if (rightHand != null && rightHand.IsValid)
                    {
                        program.controllerService2.SetControllerState(0, rightHand.X, rightHand.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }

                    KeyPoint2D leftHand = pose2D.GetKeyPoint2D(BodyPoint.Left_Wrist);

                    if (leftHand != null && leftHand.IsValid)
                    {
                        program.controllerService.SetControllerState(1, leftHand.X, leftHand.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }
                }
                else if (pose != null && typeof(Pose3D) == pose.GetType())
                {
                    Pose3D pose3D = (Pose3D)pose;

                    KeyPoint3D nose = pose3D.GetKeyPoint3D(BodyPoint.Nose_or_Top_Head);

                    if (nose != null && nose.IsValid)
                    {
                        //program.headTrackingService.SendPositionOnly(nose.X, nose.Y, 0);
                        program.headTrackingService.SendRotationAndPosition(nose.Yaw, nose.Pitch, nose.Roll, nose.X, nose.Y, nose.Z);
                        //program.headTrackingService.SendAsyncOffset(0, 0, 0);
                    }

                    KeyPoint3D rightHand = pose3D.GetKeyPoint3D(BodyPoint.Right_Wrist);

                    if (rightHand != null && rightHand.IsValid)
                    {
                        program.controllerService2.SetControllerState(0, rightHand.X, rightHand.Y, rightHand.Z, rightHand.Yaw, rightHand.Pitch, rightHand.Roll, 0, 0, 0, false, false, false);
                    }

                    KeyPoint3D leftHand = pose3D.GetKeyPoint3D(BodyPoint.Left_Wrist);

                    if (leftHand != null && leftHand.IsValid)
                    {
                        program.controllerService.SetControllerState(1, leftHand.X, leftHand.Y, leftHand.Z, leftHand.Yaw, leftHand.Pitch, leftHand.Roll, 0, 0, 0, false, false, false);
                    }

                    KeyPoint3D rightFoot = pose3D.GetKeyPoint3D(BodyPoint.Right_Heel);

                    if (rightFoot != null && rightFoot.IsValid)
                    {
                        //program.controllerService3.SetControllerState(2, rightFoot.X, rightFoot.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }

                    KeyPoint3D leftFoot = pose3D.GetKeyPoint3D(BodyPoint.Left_Heel);

                    if (leftFoot != null && leftFoot.IsValid)
                    {
                        //program.controllerService4.SetControllerState(3, leftFoot.X, leftFoot.Y, -0.6, 0, 0, 0, 0, 0, 0, false, false, false);
                    }
                }
            }
            catch (Exception)
            {
                program.ConnectOrReconnect();
            }
        }
예제 #22
0
 ///<exclude/>
 public bool Equals(Pose3D other)
 {
     if (ReferenceEquals(null, other)) return false;
     if (ReferenceEquals(this, other)) return true;
     return other._Position.Equals(_Position) && other._Orientation.Equals(_Orientation);
 }
예제 #23
0
 private void updateSensorState(Pose3D pose3d)
 {
     instance.head_orientation = pose3d.Orientation;
 }