/// <summary>
/// initialise with the given number of stereo cameras
/// </summary>
/// <param name="no_of_stereo_cameras">the number of stereo cameras on the robot (not the total number of cameras)</param>
/// <param name="rays_per_stereo_camera">the number of rays which will be thrown from each stereo camera per time step</param>
/// <param name="mapping_type">the type of mapping to be used</param>
private void init(int no_of_stereo_cameras,
int rays_per_stereo_camera,
int mapping_type)
{
this.no_of_stereo_cameras = no_of_stereo_cameras;
this.mapping_type = mapping_type;
// head and shoulders
head = new stereoHead(no_of_stereo_cameras);
// sensor model used for mapping
inverseSensorModel = new stereoModel();
// set the number of stereo features to be detected and inserted into the grid
// on each time step. This figure should be large enough to get reasonable
// detail, but not so large that the mapping consumes a huge amount of
// processing resource
inverseSensorModel.no_of_stereo_features = rays_per_stereo_camera;
correspondence = new stereoCorrespondence[no_of_stereo_cameras];
for (int i = 0; i < no_of_stereo_cameras; i++)
correspondence[i] = new stereoCorrespondence(inverseSensorModel.no_of_stereo_features);
if (mapping_type == MAPPING_DPSLAM)
{
// add local occupancy grids
LocalGrid = new occupancygridMultiHypothesis[mapping_threads];
for (int i = 0; i < mapping_threads; i++) createLocalGrid(i);
// create a motion model for each possible grid
motion = new motionModel[mapping_threads];
for (int i = 0; i < mapping_threads; i++) motion[i] = new motionModel(this, LocalGrid[i], 100 * (i+1));
}
if (mapping_type == MAPPING_SIMPLE)
{
}
// a list of places where the robot might work or make observations
worksites = new kmlZone();
// zero encoder positions
prev_left_wheel_encoder = 0;
prev_right_wheel_encoder = 0;
}
/// <summary>
/// load worksite locations from a KML file
/// </summary>
/// <param name="Kml"></param>
/// <returns></returns>
public bool LoadWorkSite(String Kml)
{
bool loaded = false;
if (File.Exists(Kml))
{
worksites = new kmlZone();
worksites.Load(Kml);
}
return (loaded);
}