├── README.md ├── helper_func.py ├── particle_class.py └── resampling.py /README.md: -------------------------------------------------------------------------------- 1 | # Grid-based FastSLAM on Turtlebot 2 | This project aims to implement the Grid-based FastSLAM algorithm on a Turtlebot. FastSLAM is a probabilistic algorithm for solving the Simultaneous Localization And Mapping (SLAM) problem. FastSLAM uses particle filters where each particle represents its belief of the robot's pose. Each particle also has a map that it updates by taking laser scan measurements of its environment. 3 | 4 | # References 5 | [1] Dr.Cyrill Stachniss, Robot Mapping course, University of Freiburg. https://www.youtube.com/watch?v=3Yl2aq28LFQ&index=15&list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_ 6 | 7 | [2] Thrun et.al., Probabilistic Robotics 8 | -------------------------------------------------------------------------------- /helper_func.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def prob_to_logodds(prob): 4 | ## Assuming that prob is a scalar 5 | prob = np.matrix(prob,float) 6 | logOdds = prob/(1+prob) 7 | return np.asscalar(logOdds) 8 | 9 | def logOdds_to_prob(logOdds): 10 | ## Assuming that logOdds is a matrix 11 | p = 1 - 1/(1 + np.exp(logOdds)) 12 | return p 13 | 14 | def swap(a,b): 15 | x,y = b,a 16 | return x,y 17 | 18 | def bresenham2(mycoords): 19 | mycoords = np.array(mycoords) 20 | x = np.round(mycoords[:,0]) 21 | y = np.round(mycoords[:,1]) 22 | steep = (abs(y[1]-y[0]) > abs(x[1]-x[0])) 23 | if steep: 24 | x,y = swap(x,y) 25 | if x[0]>x[1]: 26 | x = np.array(swap(x[0],x[1])) 27 | y = np.array(swap(y[0],y[1])) 28 | delx = x[1] - x[0] 29 | dely = abs(y[1] - y[0]) 30 | error = 0 31 | x_n,y_n = x[0],y[0] 32 | 33 | X = np.zeros(delx+1) 34 | Y = np.zeros(delx+1) 35 | 36 | if y[0] < y[1]: 37 | ystep = 1 38 | else: 39 | ystep = -1 40 | for n in range(delx+1): 41 | if steep: 42 | X[n],Y[n] = x_n,y_n 43 | else: 44 | X[n],Y[n] = y_n,x_n 45 | x_n += 1 46 | error += dely 47 | if error<<1 >= delx: 48 | y_n += ystep 49 | error -= delx 50 | X,Y = swap(X,Y) 51 | return X,Y 52 | 53 | def pose_world_to_map(pntsWorld,gridSize): 54 | pntsMap = [x/gridSize for x in pntsWorld] 55 | return pntsMap 56 | 57 | def laser_world_to_map(laserEndPnts, gridSize): 58 | pntsMap = laserEndPnts/gridSize 59 | return pntsMap 60 | 61 | def v2t(v): 62 | x = v[0] 63 | y = v[1] 64 | th = v[2] 65 | trans = np.matrix([[cos(th), -sin(th), x],[sin(th), cos(th), y],[0, 0, 1]]) 66 | return trans 67 | 68 | def laser_to_xy(rl): 69 | numBeams = len(rl.ranges) 70 | maxRange = rl.range_max 71 | # apply the max range 72 | idx = [] 73 | for i in range(len(rl.ranges)): 74 | if isnan(rl.ranges[i]): 75 | idx.append(False) 76 | else: 77 | if rl.ranges[i]>0 and rl.ranges[i] thresh_occ: 83 | distances.append(np.linalg.norm([x_pos-x,y_pos-y])) 84 | break 85 | elif n == int(scan.range_max/self.grid_size): 86 | distances.append('OOR') 87 | return distances 88 | 89 | if __name__ == '__main__': 90 | num_particles = 30 91 | particles = [particle(num_particles) for i in range(num_particles)] 92 | while(not ut.end): 93 | ### ut, scan = Subscribe to topics to get twist and laser scan after delta_t 94 | for i in range(num_particles): 95 | particles[i].pose = particles[i].sample_motion_model(ut) 96 | particles[i].weight = observation_model(scan) 97 | [mapUpdate, robPoseMap, laserEndPntsMapFrame] = inverse_sensor_model(scan) 98 | particles[i].grid_map -= logOddsPrior 99 | particles[i].grid_map += mapUpdate 100 | plot_map(particles[i].grid_map,robPoseMap, laserEndPntsMapFrame, particles[i].gridSize) 101 | particles = resampling(particles) 102 | -------------------------------------------------------------------------------- /resampling.py: -------------------------------------------------------------------------------- 1 | # This code can be worked on for the resampling part of the algorithm 2 | # Assuming the input is the particles i.e [p1,p2,p3] 3 | 4 | import random 5 | 6 | def ssum(k,particles): 7 | sum = 0 8 | for i in range(k+1): 9 | sum = sum + particle_weights[i] 10 | return sum 11 | 12 | def sus(particles): 13 | f = 360 14 | n = len(particles) 15 | p = f/n 16 | start = random.randint(0,p) 17 | pointers = [] 18 | for i in range(n): 19 | pointers.append(start + i*p) 20 | return rws(pointers,particles) 21 | 22 | def rws(pointers,particles): 23 | keep = [] 24 | for pointer in pointers: 25 | k = 0 26 | while (ssum(k,particles)) < pointer: 27 | k += 1 28 | keep.append(particles[k][4]) 29 | return keep 30 | 31 | #INPUT 32 | p1 = [0,0,0,1,1] 33 | p2 = [0,0,0,0,2] 34 | p3 = [0,0,0,0,3] 35 | particles = [p1,p2,p3] 36 | 37 | #Extracting Weights 38 | particle_weights = [] 39 | for i in range(len(particles)): 40 | particle_weights.append(particles[i][3]*360) 41 | 42 | #Resampling 43 | print sus(particles) 44 | --------------------------------------------------------------------------------