/// <summary>
/// remove the mapping particles associated with this path
/// </summary>
public bool Remove(dpslam grid)
{
Enabled = false;
particlePose pose = current_pose;
if (current_pose != null)
{
// collapse this path down to the next branching point
pose = CollapseBranch(pose, grid);
if (pose != null)
{
if (pose != current_pose)
{
if (pose.no_of_children > 0)
{
// reduce the number of children at the branch point
pose.no_of_children--;
}
}
// keep on truckin' down the line...
incrementPathChildren(pose.path, -1);
if (pose.path.total_children == 0)
{
pose.path.Remove(grid);
}
}
}
return(!Enabled);
}
/// <summary>
/// remove all poses along the path until a branch point is located
/// </summary>
/// <param name="pose"></param>
/// <returns></returns>
private particlePose CollapseBranch(
particlePose pose,
dpslam grid)
{
int children = 0;
while ((pose != branch_pose) &&
(pose.path == this) &&
(children == 0))
{
if (pose != current_pose)
{
children = pose.no_of_children;
}
if (children == 0)
{
if (pose.path == this)
{
pose.Remove(grid);
pose = pose.parent;
}
}
}
return(pose);
}
/// <summary>
/// distills all grid particles associated with this pose
/// </summary>
/// <param name="grid">grid to be updated</param>
public void Distill(dpslam grid)
{
for (int i = observed_grid_cells.Count - 1; i >= 0; i--)
{
particleGridCell hypothesis = observed_grid_cells[i];
grid.Distill(hypothesis);
}
}
/// <summary>
/// distill all grid particles within this path
/// </summary>
/// <param name="grid"></param>
public void Distill(dpslam grid)
{
particlePose pose = current_pose;
while (pose != null)
{
pose.Distill(grid);
pose = pose.parent;
}
map_cache = null;
Enabled = false;
}
public motionModel(
robot rob,
dpslam LocalGrid,
int random_seed)
{
// seed the random number generator
//rnd = new Random(random_seed);
rnd = new MersenneTwister(random_seed);
this.rob = rob;
this.LocalGrid = LocalGrid;
Poses = new List <particlePath>();
ActivePoses = new List <particlePath>();
motion_noise = new float[6];
// some default noise values
int i = 0;
while (i < 2)
{
motion_noise[i] = default_motion_noise_1;
i++;
}
while (i < 4)
{
motion_noise[i] = default_motion_noise_2;
i++;
}
while (i < motion_noise.Length)
{
motion_noise[i] = default_motion_noise_3;
i++;
}
// create some initial poses
createNewPoses(rob.x, rob.y, rob.z, rob.pan, rob.tilt, rob.roll);
}
/// <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,
dpslam 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].tilt,
camera_centre_location[cam].roll,
camera_centre_location[cam].x,
camera_centre_location[cam].y,
camera_centre_location[cam].z);
//Console.WriteLine("ray.vert[0] " + (trial_ray.vertices[0] == null).ToString());
//Console.WriteLine("ray.vert[1] " + (trial_ray.vertices[1] == null).ToString());
// 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);
}
public void ProbeView()
{
int map_dim = 14;
byte[] map =
{
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
dpslam sim = CreateSimulation(map_dim, map, 50);
int image_width = 320;
int image_height = 240;
float FOV_degrees = 90;
float max_range_mm = 2000;
pos3D camPose = new pos3D(0, 0, 0);
float x0_mm = 0;
float y0_mm = 0;
float z0_mm = 100;
float x1_mm = 0;
float y1_mm = 1500;
float z1_mm = -300;
bool use_ground_plane = true;
float range = sim.ProbeRange(
null,
x0_mm, y0_mm, z0_mm,
x1_mm, y1_mm, z1_mm,
use_ground_plane);
Assert.IsTrue(range > -1, "Ground plane was not detected");
Assert.IsTrue(range > 550);
Assert.IsTrue(range < 650);
int step_size = 10;
float[] range_buffer = new float[(image_width / step_size) * (image_height / step_size)];
sim.ProbeView(
null,
camPose.x, camPose.y, camPose.z,
camPose.pan, camPose.tilt, camPose.roll,
FOV_degrees,
image_width, image_height,
step_size,
max_range_mm, false,
range_buffer);
int ctr = 0;
for (int i = 0; i < range_buffer.Length; i++)
{
if (range_buffer[i] > -1)
{
ctr++;
}
//Console.WriteLine("Range: " + range_buffer[i].ToString());
}
Assert.IsTrue(ctr > 0, "No objects were ranged within the simulation");
}
//[Test()]
public static void CreateSim()
{
int dimension_cells = 100;
int dimension_cells_vertical = 20;
int cellSize_mm = 50;
int localisationRadius_mm = 2000;
int maxMappingRange_mm = 2000;
float vacancyWeighting = 0.8f;
int map_dim = 14;
byte[] map =
{
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
dpslam sim = CreateSimulation(map_dim, map, 50);
particlePose pose = null;
int img_width = 640;
byte[] img = new byte[img_width * img_width * 3];
sim.Show(0, img, img_width, img_width, pose, true, true);
Bitmap bmp = new Bitmap(img_width, img_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("dpslam_tests_CreateSimulation1.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
robot rob = new robot(1);
rob.WheelBase_mm = 90;
rob.WheelDiameter_mm = 30;
rob.x = 0;
rob.y = 0;
rob.pan = 90 * (float)Math.PI / 180.0f;
rob.head.cameraFOVdegrees[0] = 90;
rob.head.cameraSensorSizeMm[0] = 4.17f;
rob.head.cameraFocalLengthMm[0] = 3.6f;
rob.head.cameraImageWidth[0] = 320;
rob.head.cameraImageHeight[0] = 240;
rayModelLookup sensor_model = new rayModelLookup(10, 10);
sensor_model.InitSurveyorSVS();
rob.head.sensormodel[0] = sensor_model;
float time_elapsed_sec = 1;
float forward_velocity = 50;
float angular_velocity_pan = 0;
float angular_velocity_tilt = 0;
float angular_velocity_roll = 0;
float min_x_mm = -((dimension_cells / 2) * cellSize_mm) / 3;
float min_y_mm = -((dimension_cells / 2) * cellSize_mm) / 3;
float max_x_mm = -min_x_mm;
float max_y_mm = -min_y_mm;
for (float t = 0.0f; t < 4; t += time_elapsed_sec)
{
rob.updateFromVelocities(sim, forward_velocity, angular_velocity_pan, angular_velocity_tilt, angular_velocity_roll, time_elapsed_sec);
Console.WriteLine("xy: " + rob.x.ToString() + " " + rob.y.ToString());
rob.motion.Show(img, img_width, img_width, min_x_mm, min_y_mm, max_x_mm, max_y_mm, true, false, t == 0.0f);
}
rob.SaveGridImage("dpslam_tests_CreateSimulation2.bmp");
BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
bmp.Save("dpslam_tests_CreateSimulation3.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
}
private static dpslam CreateSimulation(
int map_dim,
byte[] map,
int cellSize_mm)
{
int dimension_cells = 100;
int dimension_cells_vertical = 20;
int localisationRadius_mm = 2000;
int maxMappingRange_mm = 2000;
float vacancyWeighting = 0.8f;
dpslam sim = new dpslam(
dimension_cells,
dimension_cells_vertical,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
Assert.IsNotNull(sim);
int world_tx = -cellSize_mm * dimension_cells / 2;
int world_ty = -cellSize_mm * dimension_cells / 2;
//int world_bx = cellSize_mm * dimension_cells / 2;
//int world_by = cellSize_mm * dimension_cells / 2;
for (int y = 0; y < map_dim; y++)
{
int start_x = 0;
bool wall = false;
for (int x = 0; x < map_dim; x++)
{
if ((!wall) && (map[y * map_dim + x] == 1) && (map[y * map_dim + x + 1] == 1))
{
start_x = x;
wall = true;
}
if ((wall) && (map[y * map_dim + x] == 0))
{
int tx = (int)(world_tx + ((start_x + 0.5f) * dimension_cells / map_dim * cellSize_mm));
int ty = (int)(world_ty + ((y + 0.5f) * dimension_cells / map_dim * cellSize_mm));
int bx = (int)(world_tx + ((x - 0.5f) * dimension_cells / map_dim * cellSize_mm));
int by = ty;
sim.InsertWall(tx, ty, bx, by, (int)cellSize_mm * 10, (int)cellSize_mm, 0.2f, 0, 0, 0);
wall = false;
}
}
}
for (int x = 0; x < map_dim; x++)
{
int start_y = 0;
bool wall = false;
for (int y = 0; y < map_dim; y++)
{
if ((!wall) && (map[y * map_dim + x] == 1) && (map[(y + 1) * map_dim + x] == 1))
{
start_y = y;
wall = true;
}
if ((wall) && (map[y * map_dim + x] == 0))
{
int tx = (int)(world_tx + ((x + 0.5f) * dimension_cells / map_dim * cellSize_mm));
int ty = (int)(world_ty + ((start_y + 0.5f) * dimension_cells / map_dim * cellSize_mm));
int bx = tx;
int by = (int)(world_ty + ((y - 0.5f) * dimension_cells / map_dim * cellSize_mm));
sim.InsertWall(tx, ty, bx, by, cellSize_mm * 10, cellSize_mm, 0.2f, 0, 0, 0);
wall = false;
}
}
}
return(sim);
}
