/// <summary> /// insert a stereo ray into the grid /// </summary> /// <param name="ray">stereo ray</param> /// <param name="origin">pose from which this observation was made</param> /// <param name="sensormodel">sensor model for each grid level</param> /// <param name="left_camera_location">left stereo camera position and orientation</param> /// <param name="right_camera_location">right stereo camera position and orientation</param> /// <param name="localiseOnly">whether we are mapping or localising</param> /// <returns>localisation matching score</returns> public float Insert( evidenceRay ray, particlePose origin, stereoModel[] sensormodel, pos3D left_camera_location, pos3D right_camera_location, bool localiseOnly) { float matchingScore = 0; switch (grid_type) { case TYPE_MULTI_HYPOTHESIS: { for (int grid_level = 0; grid_level < grid.Length; grid_level++) { rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model; occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level]; matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly); } break; } } return(matchingScore); }
/// <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); }