/// <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); }
/// <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); }