Map-Making+SLAM with real Lidar data

import matplotlib.pyplot as plt

import matplotlib.image as mpimg

import numpy as np

import matplotlib.pyplot as plt

import cv2

import requests

from io import BytesIO

from PIL import Image

# Load the sensor image from URL

image_url = 'https://github.com/xiaofeng419/SLAM-2D-LIDAR-SCAN/blob/master/Image/GT_AND_RAW.png?raw=true'

#image_url = 'https://github.com/xiaofeng419/SLAM-2D-LIDAR-SCAN/blob/master/Image/GroundTruth.png?raw=true'

response = requests.get(image_url)

image = Image.open(BytesIO(response.content)).convert('L')

image = np.array(image)

# Load the image

# Display the image

plt.imshow(image)

plt.title('Sensor Image')

plt.axis('off')

plt.show()

# Process the image to extract LIDAR data and ground truth trajectory

sensor_data = np.array([[x, y] for x in range(100) for y in range(100) if image[x, y] > 128])

ground_truth_trajectory = np.array([[x, y] for x in range(100) for y in range(100) if image[x, y] == 255])

# Initialize the occupancy grid

grid_size = (100, 100)

occupancy_grid = np.zeros(grid_size)

# Update the grid with sensor data

for point in sensor_data:

x, y = point

occupancy_grid[int(x), int(y)] = 1

# Plot the occupancy grid and ground truth trajectory

plt.imshow(occupancy_grid, cmap='gray')

plt.plot(ground_truth_trajectory[:, 0], ground_truth_trajectory[:, 1], 'r') # Plot trajectory in red

plt.show()

class FastSLAM:

def __init__(self, num_particles, map_size):

self.num_particles = num_particles

self.particles = [self.create_particle(map_size) for _ in range(num_particles)]

def create_particle(self, map_size):

return {

'pose': np.random.rand(3) * map_size, # Random initial pose

'weight': 1.0 / self.num_particles,

'map': np.zeros(map_size)

}

def update(self, control, measurement):

for particle in self.particles:

self.motion_update(particle, control)

self.measurement_update(particle, measurement)

self.resample_particles()

def motion_update(self, particle, control):

# Update particle pose based on control input

pass

def measurement_update(self, particle, measurement):

# Update particle map based on measurement

pass

def resample_particles(self):

# Resample particles based on their weights

pass

class FastSLAM:

def __init__(self, num_particles, map_size):

self.num_particles = num_particles

self.map_size = map_size

self.particles = [self.create_particle() for _ in range(num_particles)]

def create_particle(self):

return {

'pose': np.random.rand(3) * np.array([self.map_size[0], self.map_size[1], 1]), # Random initial pose

'weight': 1.0 / self.num_particles,

'map': np.zeros(self.map_size)

}

def update(self, control, measurement):

for particle in self.particles:

self.motion_update(particle, control)

self.measurement_update(particle, measurement)

self.resample_particles()

def motion_update(self, particle, control):

# Update particle pose based on control input

pass

def measurement_update(self, particle, measurement):

# Update particle map based on measurement

pass

def resample_particles(self):

# Resample particles based on their weights

pass

# Initialize FastSLAM with a number of particles and map size

fastslam = FastSLAM(num_particles=100, map_size=(300, 300))

# Assuming we have control and measurement data

control_data = np.array([1, 0, 0]) # Dummy control data

measurement_data = np.array([1, 1]) # Dummy measurement data

# Update FastSLAM with control and measurement data

fastslam.update(control_data, measurement_data)

# Display the resulting map and trajectory

resulting_map = fastslam.particles[0]['map']

plt.imshow(resulting_map, cmap='gray')

plt.title('Resulting Map from FastSLAM')

plt.axis('on')

plt.show()

def scan_matching(particle, scan):

# Placeholder for scan matching algorithm

# Update particle weight based on scan matching results

return particle

# Update particle weights using scan matching

for particle in fastslam.particles:

particle = scan_matching(particle, None) # Placeholder for actual scan data

Theory:

To create an occupancy grid map using the ground truth trajectory and solve the SLAM problem with the FastSLAM algorithm, you’ll need to follow these steps:

1. Extract the Ground Truth Trajectory: Download the image and extract the ground truth trajectory data. This might involve using image processing techniques to convert the visual data into numerical coordinates.

2. Initialize the Map and Particles: Set up an initial occupancy grid map and initialize particles for the FastSLAM algorithm. Each particle represents a possible robot pose and an associated map.

3. Implement FastSLAM Algorithm:

   • Prediction Step: Use the robot’s motion model to predict the new pose of each particle.
   • Update Step: Update the map for each particle using sensor measurements (LIDAR scans). This involves associating observed landmarks with the map and updating their positions using an Extended Kalman Filter (EKF).
   • Resampling Step: Resample the particles based on their importance weights to focus on the most likely poses.

4. Visualize the Results: Plot the resulting occupancy grid map and the estimated trajectory.

Step 1: Create an Occupancy Grid Map with the Ground Truth Trajectory

1. Load the Ground Truth Data: Extract the ground truth trajectory from the provided image or dataset.
2. Initialize the Occupancy Grid: Define the grid size and resolution.
3. Update the Grid: Mark the cells in the grid as occupied or free based on the ground truth trajectory.

Step 2: Solve the SLAM Problem with FastSLAM Algorithm

1. Initialize Particles: Start with a set of particles representing possible robot poses.
2. Prediction Step: Update the particles based on the robot’s motion model.
3. Update Step: Use sensor measurements to update the particles’ weights.
4. Resampling: Resample particles based on their weights to focus on the most likely poses.
5. Map Update: Update the map for each particle using the sensor measurements.

Step 3: Compare Maps

1. Generate Map from Ground Truth: Use the ground truth trajectory to create a reference map.
2. Generate Map from FastSLAM: Use the FastSLAM algorithm to create a map.
3. Comparison: Compare the two maps to evaluate the accuracy of the SLAM algorithm.

Step 4: Scan Matching for Importance Weighting

1. Scan Matching: Use a scan matching algorithm to align the current scan with the map.
2. Importance Weighting: Assess the quality of each particle based on the scan matching results and update their weights accordingly.