/// <summary>
/// update using known velocities within a simulated map
/// </summary>
/// <param name="sim_map">simulated environment</param>
/// <param name="forward_velocity">forward velocity in mm/sec</param>
/// <param name="angular_velocity_pan">angular velocity in the pan axis in radians/sec</param>
/// <param name="angular_velocity_tilt">angular velocity in the tilt axis in radians/sec</param>
/// <param name="angular_velocity_roll">angular velocity in the roll axis in radians/sec</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in sec</param>
public void updateFromVelocities(
dpslam sim_map,
float forward_velocity,
float angular_velocity_pan,
float angular_velocity_tilt,
float angular_velocity_roll,
float time_elapsed_sec)
{
// update the grid
updateSimulation(sim_map);
// update the motion model
motion.InputType = motionModel.INPUTTYPE_BODY_FORWARD_AND_ANGULAR_VELOCITY;
motion.forward_acceleration = forward_velocity - motion.forward_velocity;
motion.forward_velocity = forward_velocity;
motion.angular_acceleration_pan = angular_velocity_pan - motion.angular_velocity_pan;
motion.angular_velocity_pan = angular_velocity_pan;
//motion.angular_acceleration_tilt = angular_velocity_tilt - motion.angular_velocity_tilt;
//motion.angular_velocity_tilt = angular_velocity_tilt;
//motion.angular_acceleration_roll = angular_velocity_roll - motion.angular_velocity_roll;
//motion.angular_velocity_roll = angular_velocity_roll;
clock.Start();
motion.Predict(time_elapsed_sec);
float deviation = 0;
float deviation_forward = 0;
float deviation_perp = 0;
float deviation_vertical = 0;
motion.AveragePose(
ref x,
ref y,
ref z,
ref pan,
ref tilt,
ref roll,
ref deviation,
ref deviation_forward,
ref deviation_perp,
ref deviation_vertical);
clock.Stop();
benchmark_prediction = clock.time_elapsed_mS;
storePreviousPosition();
}
/// <summary>
/// update from a known position and orientation within a simulated environment
/// </summary>
/// <param name="sim_map">simulated environment</param>
/// <param name="x">x position in mm</param>
/// <param name="y">y position in mm</param>
/// <param name="z">z position in mm</param>
/// <param name="pan">pan angle in radians</param>
/// <param name="tilt">tilt angle in radians</param>
/// <param name="roll">roll angle in radians</param>
/// <param name="mapping">mapping or localisation</param>
public void updateFromKnownPosition(
dpslam sim_map,
float x,
float y,
float z,
float pan,
float tilt,
float roll)
{
// update the grid
updateSimulation(sim_map);
// set the robot at the known position
this.x = x;
this.y = y;
this.z = z;
this.pan = pan;
this.tilt = tilt;
this.roll = roll;
// update the motion model
motionModel motion_model = motion;
motion_model.InputType = motionModel.INPUTTYPE_BODY_FORWARD_AND_ANGULAR_VELOCITY;
if (!((previousPosition.x == -1) && (previousPosition.y == -1)))
{
float time_per_index_sec = 1;
float dx = x - previousPosition.x;
float dy = y - previousPosition.y;
float distance = (float)Math.Sqrt((dx * dx) + (dy * dy));
float acceleration = (2 * (distance - (motion_model.forward_velocity * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
acceleration /= time_per_index_sec;
float forward_velocity = motion_model.forward_velocity + (acceleration * time_per_index_sec);
motion.forward_acceleration = acceleration;
motion.forward_velocity = forward_velocity;
distance = pan - previousPosition.pan;
acceleration = (2 * (distance - (motion_model.angular_velocity_pan * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
acceleration /= time_per_index_sec;
float angular_velocity_pan = motion_model.angular_velocity_pan + (acceleration * time_per_index_sec);
motion.angular_velocity_pan = angular_velocity_pan;
distance = tilt - previousPosition.tilt;
acceleration = (2 * (distance - (motion_model.angular_velocity_tilt * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
acceleration /= time_per_index_sec;
float angular_velocity_tilt = motion_model.angular_velocity_tilt + (acceleration * time_per_index_sec);
motion.angular_velocity_tilt = angular_velocity_tilt;
distance = roll - previousPosition.roll;
acceleration = (2 * (distance - (motion_model.angular_velocity_roll * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
acceleration /= time_per_index_sec;
float angular_velocity_roll = motion_model.angular_velocity_roll + (acceleration * time_per_index_sec);
motion.angular_velocity_roll = angular_velocity_roll;
clock.Start();
motion.Predict(time_per_index_sec);
clock.Stop();
benchmark_prediction = clock.time_elapsed_mS;
}
storePreviousPosition();
}
/// <summary>
/// update using wheel encoder positions within a simulated environment
/// </summary>
/// <param name="sim_map">simulated environment</param>
/// <param name="left_wheel_encoder">left wheel encoder position</param>
/// <param name="right_wheel_encoder">right wheel encoder position</param>
/// <param name="time_elapsed_sec">time elapsed since the last update in sec</param>
/// <param name="mapping">mapping or localisation</param>
public void updateFromEncoderPositions(
dpslam sim_map,
long left_wheel_encoder,
long right_wheel_encoder,
float time_elapsed_sec)
{
// update the grid
updateSimulation(sim_map);
//float wheel_circumference_mm = (float)Math.PI * WheelDiameter_mm;
long countsPerWheelRev = CountsPerRev * GearRatio;
if ((time_elapsed_sec > 0.00001f) &&
(countsPerWheelRev > 0) &&
(prev_left_wheel_encoder != 0))
{
// calculate angular velocity of the left wheel in radians/sec
float angle_traversed_radians = (float)(left_wheel_encoder - prev_left_wheel_encoder) * 2 * (float)Math.PI / countsPerWheelRev;
motion.LeftWheelAngularVelocity = angle_traversed_radians / time_elapsed_sec;
// calculate angular velocity of the right wheel in radians/sec
angle_traversed_radians = (float)(right_wheel_encoder - prev_right_wheel_encoder) * 2 * (float)Math.PI / countsPerWheelRev;
motion.RightWheelAngularVelocity = angle_traversed_radians / time_elapsed_sec;
}
clock.Start();
// update the motion model
motion.InputType = motionModel.INPUTTYPE_WHEEL_ANGULAR_VELOCITY;
motion.Predict(time_elapsed_sec);
clock.Stop();
benchmark_prediction = clock.time_elapsed_mS;
prev_left_wheel_encoder = left_wheel_encoder;
prev_right_wheel_encoder = right_wheel_encoder;
storePreviousPosition();
}
/// <summary>
/// recreate the local grid
/// </summary>
private void createLocalGrid()
{
// create the local grid
LocalGrid = new dpslam(LocalGridDimension, LocalGridDimensionVertical, (int)LocalGridCellSize_mm, (int)LocalGridLocalisationRadius_mm, (int)LocalGridMappingRange_mm, LocalGridVacancyWeighting);
}
private void updateSimulation(
dpslam sim_map)
{
// create simulated observation based upon the given map
List<evidenceRay>[] stereo_rays = new List<evidenceRay>[head.no_of_stereo_cameras];
for (int cam = 0; cam < head.no_of_stereo_cameras; cam++)
stereo_rays[cam] = sim_map.createSimulatedObservation(this, cam);
// update the motion model and occupancy grid using the observations
MappingUpdate(stereo_rays);
}
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);
}
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);
}
/// <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>
/// 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);
}
/// <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>
/// 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;
}