Esempio n. 1
0
        /// <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();
        }
Esempio n. 2
0
        /// <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();
        }
Esempio n. 3
0
        /// <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();
        }
Esempio n. 4
0
        /// <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);
		}
Esempio n. 5
0
        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);			
		}
Esempio n. 6
0
        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);
        }
Esempio n. 7
0
		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);
		}
Esempio n. 8
0
 /// <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);
     }
 }
Esempio n. 9
0
        /// <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);
        }
Esempio n. 10
0
        /// <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);
        }
Esempio n. 11
0
        /// <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);
        }
Esempio n. 12
0
 /// <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;
 }