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