예제 #1
0
        /// <summary>
        /// create a new viewpoint which is a trial pose
        /// </summary>
        /// <param name="extra_pan"></param>
        /// <param name="translation_x"></param>
        /// <param name="translation_y"></param>
        /// <returns></returns>
        public viewpoint createTrialPose(float extra_pan,
                                         float translation_x, float translation_y)
        {
            viewpoint trialPose = new viewpoint(rays.Length);

            for (int cam = 0; cam < rays.Length; cam++)
            {
                for (int r = 0; r < rays[cam].Count; r++)
                {
                    evidenceRay ray       = (evidenceRay)rays[cam][r];
                    evidenceRay trial_ray = ray.trialPose(extra_pan, translation_x, translation_y);
                    trialPose.rays[cam].Add(trial_ray);
                }
            }
            return(trialPose);
        }
예제 #2
0
        /// <summary>
        /// add an observation taken from this pose
        /// </summary>
        /// <param name="stereo_rays">list of ray objects in this observation</param>
        /// <param name="rob">robot object</param>
        /// <param name="LocalGrid">occupancy grid into which to insert the observation</param>
        /// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
        /// <returns>localisation matching score</returns>
        public float AddObservation(List <evidenceRay>[] stereo_rays,
                                    robot rob,
                                    occupancygridMultiHypothesis LocalGrid,
                                    bool localiseOnly)
        {
            // clear the localisation score
            float localisation_score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;

            // get the positions of the head and cameras for this pose
            pos3D head_location = new pos3D(0, 0, 0);

            pos3D[] camera_centre_location = new pos3D[rob.head.no_of_stereo_cameras];
            pos3D[] left_camera_location   = new pos3D[rob.head.no_of_stereo_cameras];
            pos3D[] right_camera_location  = new pos3D[rob.head.no_of_stereo_cameras];
            calculateCameraPositions(rob, ref head_location,
                                     ref camera_centre_location,
                                     ref left_camera_location,
                                     ref right_camera_location);

            // itterate for each stereo camera
            int cam = stereo_rays.Length - 1;

            while (cam >= 0)
            {
                // itterate through each ray
                int r = stereo_rays[cam].Count - 1;
                while (r >= 0)
                {
                    // observed ray.  Note that this is in an egocentric
                    // coordinate frame relative to the head of the robot
                    evidenceRay ray = stereo_rays[cam][r];

                    // translate and rotate this ray appropriately for the pose
                    evidenceRay trial_ray = ray.trialPose(camera_centre_location[cam].pan,
                                                          camera_centre_location[cam].x,
                                                          camera_centre_location[cam].y);

                    // update the grid cells for this ray and update the
                    // localisation score accordingly
                    float score =
                        LocalGrid.Insert(trial_ray, this,
                                         rob.head.sensormodel[cam],
                                         left_camera_location[cam],
                                         right_camera_location[cam],
                                         localiseOnly);
                    if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                    {
                        if (localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                        {
                            localisation_score += score;
                        }
                        else
                        {
                            localisation_score = score;
                        }
                    }
                    r--;
                }
                cam--;
            }

            return(localisation_score);
        }