Example #1
0
        /// <summary>
        /// Obtain several measurements from the hidden state.
        /// </summary>
        /// <param name="time">Provides a snapshot of timing values.</param>
        /// <returns>Pixel-range measurements.</returns>
        public override List <PixelRangeMeasurement> Measure(GameTime time)
        {
            var measurements = new List <PixelRangeMeasurement>();

            NextFrame(out DepthFrame, out ColorFrame, out interest);

            for (int i = 0; i < interest.Count; i++)
            {
                float range = GetRange(interest[i].I, interest[i].K, (float)interest[i].Value);

                measurements.Add(new PixelRangeMeasurement(interest[i].I - ResX / 2, interest[i].K - ResY / 2, range));
            }

            if (ShowSidebar && depthsensed != null)
            {
                DepthMatrixToTexture(depthsensed, DepthFrame);
                ColorMatrixToTexture(colorsensed, ColorFrame);
            }

            MappedMeasurements.Clear();
            foreach (PixelRangeMeasurement z in measurements)
            {
                MappedMeasurements.Add(Measurer.MeasureToMap(Pose, z));
            }

            return(measurements);
        }
Example #2
0
        /// <summary>
        /// Obtain several measurements from the hidden state.
        /// Ladmarks may be misdetected.
        /// </summary>
        /// <param name="time">Provides a snapshot of timing values.</param>
        /// <returns>Pixel-range measurements.</returns>
        public override List <MeasurementT> Measure(GameTime time)
        {
            List <double[]> mlinear = Measurements[FrameIndex].Item2;

            if (FrameIndex < RecLength - 1)
            {
                updateWants();
                FrameIndex++;
            }
            else
            {
                WantsToStop = true;
            }

            List <MeasurementT> measurements = new List <MeasurementT>();
            MeasurementT        dummy        = new MeasurementT();

            MappedMeasurements.Clear();
            foreach (double[] z in mlinear)
            {
                MeasurementT measurement = dummy.FromLinear(z);
                measurements.Add(measurement);
                MappedMeasurements.Add(Measurer.MeasureToMap(Pose, measurement));
            }

            return(measurements);
        }
Example #3
0
        /// <summary>
        /// Obtain several measurements from the hidden state.
        /// Ladmarks may be misdetected.
        /// </summary>
        /// <param name="time">Provides a snapshot of timing values.</param>
        /// <returns>Pixel-range measurements.</returns>
        public override List <MeasurementT> Measure(GameTime time)
        {
            List <MeasurementT> measurements = new List <MeasurementT>();
            Map visible = new Map(3);

            DataAssociation = new List <int>();

            double[][] diraccov = new double[3][] { new double[3] {
                                                        0.001, 0, 0
                                                    },
                                                    new double[3] {
                                                        0, 0.001, 0
                                                    },
                                                    new double[3] {
                                                        0, 0, 0.001
                                                    } };

            // add every measurement with probability = DetectionProbability
            for (int i = 0; i < Landmarks.Count; i++)
            {
                double pd = DetectionProbability(Landmarks[i]);
                if (pd > 0)
                {
                    if (Util.Uniform.Next() < pd)
                    {
                        measurements.Add(MeasureDetected(Landmarks[i]));
                        DataAssociation.Add(i);
                        visible.Add(new Gaussian(Landmarks[i], diraccov, 1.0));
                    }
                    else
                    {
                        visible.Add(new Gaussian(Landmarks[i], diraccov, 0.0));
                        // weight indicates visible but not detected
                    }
                }
            }

            // poisson distributed clutter measurement count
            // a cap of 10 lambda is enforced because technically
            // the number is unbounded so it could freeze the system
            int nclutter;

            try {
                // the poisson generator may underflow if lambda is too small
                nclutter = Math.Min(clutterGen.Generate(), (int)(ClutterCount * 10));
            }
            catch (ArithmeticException)
            {
                nclutter = 0;
            }

            for (int i = 0; i < nclutter; i++)
            {
                measurements.Add(Measurer.RandomMeasure());
                DataAssociation.Add(int.MinValue);
            }

            MappedMeasurements.Clear();
            foreach (MeasurementT z in measurements)
            {
                MappedMeasurements.Add(Measurer.MeasureToMap(Pose, z));
            }

            WayVisibleMaps.Add(Tuple.Create(time.TotalGameTime.TotalSeconds, visible));

            return(measurements);
        }