def getDataset(dataset): if (dataset == 0): joint_data = ld.get_joint("joint/train_joint0") lidar_data = ld.get_lidar("lidar/train_lidar0") # rgd_data = ld.get_rgb("cam/RGB_0") # depth_data = ld.get_depth("cam/DEPTH_0") # exIR_RGB = ld.getExtrinsics_IR_RGB() # IRCalib = ld.getIRCalib() # RGBCalib = ld.getRGBCalib() elif (dataset == 1): joint_data = ld.get_joint("joint/train_joint1") lidar_data = ld.get_lidar("lidar/train_lidar1") elif (dataset == 2): joint_data = ld.get_joint("joint/train_joint2") lidar_data = ld.get_lidar("lidar/train_lidar2") elif (dataset == 3): joint_data = ld.get_joint("joint/train_joint3") lidar_data = ld.get_lidar("lidar/train_lidar3") # rgd_data = ld.get_rgb("cam/RGB_3") # depth_data = ld.get_depth("cam/DEPTH_3") # exIR_RGB = ld.getExtrinsics_IR_RGB() # IRCalib = ld.getIRCalib() # RGBCalib = ld.getRGBCalib() elif (dataset == 4): joint_data = ld.get_joint("joint/train_joint4") lidar_data = ld.get_lidar("lidar/train_lidar4") # rgd_data = ld.get_rgb("cam/RGB_4") # depth_data = ld.get_depth("cam/DEPTH_4") # exIR_RGB = ld.getExtrinsics_IR_RGB() # IRCalib = ld.getIRCalib() # RGBCalib = ld.getRGBCalib() return joint_data, lidar_data
def __init__(self,dataset='0',data_folder='data',name=None): if name == None: joint_file = os.path.join(data_folder,'train_joint'+dataset) else: joint_file = os.path.join(data_folder,name) joint_data = ld.get_joint(joint_file) self.num_measures_ = len(joint_data['ts'][0]) self.data_ = joint_data self.head_angles = self.data_['head_angles']
def __init__(self, lfilename, jfilename): self.lfilename = lfilename self.lidar = load_data.get_lidar(self.lfilename) self.jfilename = jfilename self.joint = load_data.get_joint(self.jfilename) self.j_t = self.joint['ts'] self.j_ang = self.joint['head_angles'] self.j_n = self.j_ang[0] self.j_h = self.j_ang[1] self.pose = np.array([0.0, 0.0, 0.0]) self.scan = None
def runSlam(): # lidarFile = 'Proj4_2018_Train/data/train_lidar1' # jointFile = 'Proj4_2018_Train/data/train_joint1' lidarFile = 'Proj4_2018_test/test_lidar' jointFile = 'Proj4_2018_test/test_joint' plt.ion() # plt.show() lidar = ld.get_lidar(lidarFile) joint = ld.get_joint(jointFile) P, W, Map, poseHistoryX, poseHistoryY = slam(lidar, joint) showMaps(P, W, Map, poseHistoryX, poseHistoryY) print 'Done' plt.show(block=True)
def runTextureMap(): lidarFile = 'Proj4_2018_Train/data/train_lidar0' jointFile = 'Proj4_2018_Train/data/train_joint0' depthFile = 'Proj4_2018_Train_rgb/DEPTH_0' rgbFile = 'Proj4_2018_Train_rgb/RGB_0' plt.ion() lidar = ld.get_lidar(lidarFile) joint = ld.get_joint(jointFile) depth = ld.get_depth(depthFile) rgb = ld.get_rgb(rgbFile) slamWithTM(lidar, joint, depth, rgb) print 'Done' plt.show(block=True)
def main(): # read data lidar_file_name = "lidar/train_lidar2" joint_file_name = "joint/train_joint2" lidar_data = get_lidar(lidar_file_name) joint_data = get_joint(joint_file_name) robot_pose = get_odom(lidar_data) lidar_data = joint_align(lidar_data, joint_data) # particles init P = particleInit(num=128) # grid map init MAP = mapInit() # init parameters step_size = 20 trajectory = np.empty(shape=(1, 2)) var_scale = np.array([0.001, 0.001, 0.01 * np.pi / 180]) lidar_angles = np.arange(-135, 135.25, 0.25) * np.pi / 180 for i in range(0, len(lidar_data), step_size): if (i % 100 == 0): print(i) ''' Predict ''' delta_pose = get_motion(robot_pose, lidar_data[i], i, step_size) P['states'] = motion_model_predict(P['states'], delta_pose, var_scale) ''' Update ''' best_particle = measurement_model_update(MAP, P, lidar_data[i], lidar_angles) trajectory = np.vstack((trajectory, [ int(best_particle[0] / MAP['res']) + MAP['sizex'] // 2, int(best_particle[1] / MAP['res']) + MAP['sizey'] // 2 ])) ''' Mapping ''' MAP['map'] = mapping(MAP['map'], lidar_data[i], best_particle, MAP['res'], lidar_angles) ''' Resample ''' N_eff = 1 / np.sum(P['weight']**2) if N_eff < 0.3 * P['number']: print("Resampling...") P = resampling_wheel(P) # plot plot(MAP['map'], MAP['res'], robot_pose, trajectory)
def SLAM(data_folder_name, datanum): if test: name0 = 'test' else: name0 = 'train' if not if_multiple_data: datanum = '' #load data lidar_new = ld.get_lidar( os.path.join(data_folder_name, name0 + '_lidar' + str(datanum))) joint_new = ld.get_joint( os.path.join(data_folder_name, name0 + '_joint' + str(datanum))) # init map map = {} map = initMap(map) # init particles pr = {} pr = initParticles(pr) # starting time and intervals for data scan mints = 0 int_skip = 10 # init video videoname = 'slam' + str(datanum) + '.mp4' video = cv2.VideoWriter(videoname, cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), \ 15, (map['sizey'], map['sizex'])) cv2.namedWindow(videoname, cv2.WINDOW_NORMAL) # SLAM for i in np.arange(mints, len(lidar_new), int_skip): print('\nscan ' + str(i)) # sync time stamps ts_diff = abs(joint_new['ts'] - lidar_new[i]['t'])[0, :] ind_j = np.where(ts_diff == min(ts_diff))[0][0] #print('lidar'+str(i)+str(lidar_new[i]['t'])) #print('joint'+str(ind_j)+ str(joint_new['ts'][:, ind_j])) # first scan to update map only if i == mints: ind_j_first = ind_j pr['best'] = lidar_new[i]['pose'] pr['best'] = pr['best'].astype('float') pr['odom'] = np.tile(pr['best'], [pr['num'], 1]) ray_g = raycast(joint_new, lidar_new, map, i, ind_j, ind_j_first, pr['best']) map = updateMap(map, pr['best'], ray_g) # standard deviation for gaussian noise in motion update binary_map = (map['occu_grids'] > 0).astype('uint8') delta_pose = lidar_new[i]['pose'] - lidar_new[i - int_skip]['pose'] stdx = max(0.001 * int_skip, abs(delta_pose[0, 0]) / 3) stdy = max(0.001 * int_skip, abs(delta_pose[0, 1]) / 3) stdtheta = max(0.005 * int_skip, abs(delta_pose[0, 2]) / 3) # for each particle for p in range(pr['num']): # particle propagation motion_noise = np.random.normal(0, [[stdx], [stdy], [stdtheta]], (3, 1)) pr['odom'][p, :] = motionUpdate(lidar_new[i]['pose'], lidar_new[i - int_skip]['pose'], pr['odom'][p, :], motion_noise) # use map correlation to update particle weights ray_g = raycast(joint_new, lidar_new, map, i, ind_j, ind_j_first, pr['odom'][p, :][np.newaxis, :]) #3*n ray_cell_x, ray_cell_y = mToCell(ray_g[0, :], ray_g[1, :], map) corrsum = compCorr(binary_map, map, ray_cell_x, ray_cell_y) pr['weights'][p, :] = pr['weights'][p, :] * corrsum # find best particle with maximum weights ind_best = np.where(pr['weights'] == max(pr['weights'])) ind_best = ind_best[0][0] pr['best'] = pr['odom'][ind_best][np.newaxis, :] # weights normalization pr['weights'] = pr['weights'] / sum(pr['weights']) # update map with best particle only ray_g = raycast(joint_new, lidar_new, map, i, ind_j, ind_j_first, pr['best']) map = updateMap(map, pr['best'], ray_g) #print('...num of particles with max weights: '+str(np.where(pr['weights'] == max(pr['weights']))[0].size)) print('...max weights: ' + str(max(pr['weights']))) print('...best pose: ' + str(pr['best'])) # resample particles when effective particles is less than threshold Neff = sum(pr['weights'])**2 / sum(pr['weights']**2) print('...effective particle number: ' + str(Neff)) if Neff < pr['Neff']: pr = resample(pr) print('\nresampling done!') # meter to cell rob_cell_x, rob_cell_y = mToCell(pr['best'][:, 0], pr['best'][:, 1], map) # visulization # occupancy grids display = (map['occu_grids']).copy() displayrgb = np.tile(display[:, :, np.newaxis], (1, 1, 3)) displayrgb = (displayrgb + 100) / 200 * 255 displayrgb = displayrgb.astype('uint8') # texts font = cv2.FONT_HERSHEY_SIMPLEX bottomLeftCornerOfText = (20, 20) fontScale = 0.4 fontColor = (255, 255, 255) cv2.putText(displayrgb, 'Time passed: ' + str( (lidar_new[i]['t'][0, :] - lidar_new[0]['t'][0, :])) + ' Robot pose: ' + str(pr['best'][0, :]), bottomLeftCornerOfText, font, fontScale, fontColor, thickness=1) # ray ends of each scan #displayrgb[ray_cell_x, ray_cell_y, 0] = 255 #displayrgb[ray_cell_x, ray_cell_y, 1]= 0 #displayrgb[ray_cell_x, ray_cell_y, 2] = 0 # all particles ''' for p in range(pr['num']): # meter to cell cell_x, cell_y = mToCell(pr['odom'][p, 0], pr['odom'][p, 1], map) cv2.circle(displayrgb, (cell_y, cell_x), 1, (255, 0, 255), -11) ''' # raw odometry arrowlen = 7 raw_cell_x, raw_cell_y = mToCell(lidar_new[i]['pose'][:, 0], lidar_new[i]['pose'][:, 1], map) cv2.circle(displayrgb, (raw_cell_y, raw_cell_x), 5, (255, 10, 0), -11) end_x_raw = raw_cell_x + arrowlen * cos(lidar_new[i]['pose'][:, 2]) end_y_raw = raw_cell_y + arrowlen * sin(lidar_new[i]['pose'][:, 2]) cv2.arrowedLine(displayrgb, (raw_cell_y, raw_cell_x), \ (int(end_y_raw), int(end_x_raw)), (255, 10, 0), 5) # optimized odometry (best particle) cv2.circle(displayrgb, (rob_cell_y, rob_cell_x), 5, (0, 10, 255), -11) end_x = rob_cell_x + arrowlen * cos(pr['best'][:, 2]) end_y = rob_cell_y + arrowlen * sin(pr['best'][:, 2]) cv2.arrowedLine(displayrgb, (rob_cell_y, rob_cell_x), \ (int(end_y), int(end_x)), (0, 10, 255), 5) #display = cv2.applyColorMap(display, cv2.COLORMAP_BONE) cv2.imshow(videoname, displayrgb) key = cv2.waitKey(100) video.write(displayrgb) video.release() cv2.destroyAllWindows()
def createMap(fileNumber): lidarFilename = 'data/train_lidar'+fileNumber jointFileName = 'data/train_joint'+fileNumber lidarData = ld.get_lidar(lidarFilename) jointData = ld.get_joint(jointFileName) #find all the joint times allJointTimes = jointData['ts'][0] #find all the head angles from joint data headAngles = jointData['head_angles'] #get the total number of timestamps numTimeStamps = len(lidarData) # init MAP MAP = {} MAP['res'] = 0.05 # meters MAP['xmin'] = -20 # meters MAP['ymin'] = -20 MAP['xmax'] = 20 MAP['ymax'] = 20 MAP['sizex'] = int(np.ceil((MAP['xmax'] - MAP['xmin']) / MAP['res'] + 1)) # cells MAP['sizey'] = int(np.ceil((MAP['ymax'] - MAP['ymin']) / MAP['res'] + 1)) MAP['map'] = np.zeros((MAP['sizex'], MAP['sizey']), dtype=np.double) # DATA TYPE: char or int8 deadReckoningPoses = deadReckoning(fileNumber) for i in range(0,numTimeStamps,100): #load the data for this timestamp dataI = lidarData[i] #find the thetas theta = np.array([np.arange(-135,135.25,0.25)*np.pi/180.]) #get the scan at this point di = np.array(dataI['scan']) # take valid indices # indValid = np.logical_and((di < 30), (di > 0.1)) # theta = theta[indValid] # di = di[indValid] #find the position of the head at this time rHead = np.vstack([di * np.cos(theta), di * np.sin(theta), np.zeros((1,1081)),np.ones((1,1081))]) #find the time stamp ts = dataI['t'][0][0] #find the closest joint based on the absolute time idx = findIndexOfCloestTimeFrame(allJointTimes,ts) #find the head angles of the closest times neckAngle = headAngles[0][idx] headAngle = headAngles[1][idx] #find the position of the body at this time dNeck = .15 rotNeck = rotzHomo(neckAngle,0,0,dNeck) rotHead = rotyHomo(headAngle) rBody = np.dot(np.dot(rotNeck,rotHead),rHead) #get the body roll and pitch angles rollBody = jointData['rpy'][0][i] pitchBody = jointData['rpy'][1][i] rotBodyRoll = rotxHomo(rollBody,0,0,0) rotBodyPitch = rotyHomo(pitchBody) #apply the body roll and pitch angles rBody = np.dot(rotBodyPitch,np.dot(rotBodyRoll,rBody)) #get the dead recokoned poses xPose = deadReckoningPoses[i][0] yPose = deadReckoningPoses[i][1] thetaPose = deadReckoningPoses[i][2] #find the yaw and pitch angle of the lidar rpy = np.array(dataI['rpy']).T yawAngle = rpy[2] #convert from the body frame to the global frame dBody = .93 + .33 rotGlobal = rotzHomo(yawAngle,xPose,yPose,dBody) rGlobal = np.dot(rotGlobal,rBody) #remove the data of the lidar hitting the ground rGlobal = rGlobal[:,(rGlobal[2,:] >= 1E-4)] #convert to cells xs0 = rGlobal[0,:] ys0 = rGlobal[1,:] xis = np.ceil((xs0 - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 yis = np.ceil((ys0 - MAP['ymin']) / MAP['res']).astype(np.int16) - 1 xPose = np.ceil((xPose - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 yPose = np.ceil((yPose - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 #run getMapCellsFromRay to get points that are unoccupied cells = getMapCellsFromRay(xPose,yPose,xis,yis) xsFree = np.int_(cells[0,:]) ysFree = np.int_(cells[1,:]) #find all the occupied cells xsOcc = xis ysOcc = yis #increase log odds of unoccupied cells to the map with log odds indGood = np.logical_and(np.logical_and(np.logical_and((xsFree > 1), (ysFree > 1)), (xsFree < MAP['sizex'])), (ysFree < MAP['sizey'])) logOddsStepDecease = .008 MAP['map'][xsFree[indGood], ysFree[indGood]] = MAP['map'][xsFree[indGood], ysFree[indGood]] - logOddsStepDecease #decrease log odds of occupied cells indGoodOcc = np.logical_and(np.logical_and(np.logical_and((xsOcc > 1), (ysOcc > 1)), (xsOcc < MAP['sizex'])), (ysOcc < MAP['sizey'])) logOddsStepIncrease = .05 MAP['map'][xsOcc[indGoodOcc], ysOcc[indGoodOcc]] = MAP['map'][xsOcc[indGoodOcc], ysOcc[indGoodOcc]] + logOddsStepIncrease #show the map plt.imshow(MAP['map'], cmap="hot"); posesX = np.ceil((deadReckoningPoses[:,0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 posesY = np.ceil((deadReckoningPoses[:,1] - MAP['ymin']) / MAP['res']).astype(np.int16) - 1 #show the dead reckoned poses plt.scatter(posesX,posesY) plt.show()
bx = (np.ceil((body_Wpose[0][0] - xmin) / 0.05)).astype(np.int16) - 1 by = (np.ceil((body_Wpose[1][0] - ymin) / 0.05)).astype(np.int16) - 1 # use bresenham algorithm to get free cells r = p2.bresenham2D(bx, by, lx, ly) # log-odds map[lx, ly] += np.log(4) map[r[0].astype(np.int16), r[1].astype(np.int16)] += np.log(0.8) return map if __name__ == "__main__": l0 = ld.get_lidar("lidar/train_lidar2") j0 = ld.get_joint("joint/train_joint2") body_Wpose = get_body_pose_inWorld(l0) map = get_Occmap(l0, j0, -20, 20, -20, 20, 0.05, 0, body_Wpose[:, 0:1]) map_index = np.where(map > 0) map_indexn = np.where(map < 0) map_vi = np.zeros(map.shape) map_vi[map_index] = 1 map_vi[map_indexn] = 0 plt.imshow(map_vi, cmap="hot") plt.title("Occupancy map") plt.xlabel('y--0.05m/cell') plt.ylabel('x--0.05m/cell') plt.show() sns.heatmap(map, center=0, vmin=-5, vmax=5) plt.xlabel('y--0.05m/cell') plt.ylabel('x--0.05m/cell')
#################################################################################################### # LOAD DATA # #################################################################################################### # load lidar and joint data folder = folder_prefix + "set" print "\nLoading lidar data..." lidar_filename = folder + "/lidar/" + folder_prefix + "_lidar" + len( dataset) * dataset lidars = load.get_lidar(lidar_filename) print "Done!\n" print "Loading joint data..." joint_filename = folder + "/joint/" + folder_prefix + "_joint" + len( dataset) * dataset joint = load.get_joint(joint_filename) print "Done!\n" # load RGB and DEPTH data print "Attempting to load RGB and DEPTH data..." rgb_exists = False RGBs = [] depths = [] try: load_depth = True RGB_folder = folder + "/cam" RGB_filenames = [] RGB_filename_prefix = "RGB" + len(dataset) * "_" + dataset for filename in os.listdir(RGB_folder): if RGB_filename_prefix.lower() in filename.lower(): RGB_filenames.append(filename)
import load_data as ld import numpy as np from SLAM_helper import * import matplotlib.pyplot as plt import MapUtils as maput import cv2 import random ####Dataset#### joint = ld.get_joint("data/train_joint2") lid = ld.get_lidar("data/train_lidar2") itv = 1 # of skip in drawing maps ############### angles = np.array([np.arange(-135, 135.25, 0.25) * np.pi / 180.0]) N, N_threshold = 100, 35 particles = np.zeros((N, 3)) weight = np.einsum('..., ...', 1.0 / N, np.ones((N, 1))) mapfig = {} mapfig['res'] = 0.05 mapfig['xmin'] = -40 mapfig['ymin'] = -40 mapfig['xmax'] = 40 mapfig['ymax'] = 40 mapfig['sizex'] = int( np.ceil((mapfig['xmax'] - mapfig['xmin']) / mapfig['res'] + 1))
def randomExperiment(): joint = ld.get_joint('Proj4_2018_Train/data/train_joint3') key_names_joint = ['acc', 'ts', 'rpy', 'gyro', 'pos', 'ft_l', 'ft_r', 'head_angles'] print joint['head_angles'].shape
body2world_pose = np.hstack((body2world_rot,np.array([lidar_pose[0],lidar_pose[1],0.93]).reshape(3,1))) body2world_pose = np.vstack((body2world_pose,np.array([0,0,0,1]).reshape(1,4))) body_pose = head2body_pose @ lid2head_pose @ lidar_scan ###### Filtering Out Ground Points ###### body_pose_T = body_pose.T body_pose_corrected = body_pose_T[np.where(body_pose[2,:] > 0)] body_pose_corrected = body_pose_corrected.T return body2world_pose @ body_pose_corrected if __name__ == '__main__': lidar_data = get_lidar("lidar/train_lidar0") #Pose is already in world frame and scan has to be shifted to world frame print("Read Lidar Data") MAP = init_map() print("Map intialized") #lidar_data = sorted(lidar_data.items(),key = lambda k: k['t'][0]) #print("Sorted Lidar Data") odometry_data = get_joint("joint/train_joint0") print("Loaded Odometry data") slam(lidar_data,odometry_data,MAP)
def load_obj(name): with open('obj/' + name + '.pkl', 'rb') as f: return pickle.load(f) def roundup(x,t_jump): return int(m.ceil(x / t_jump)) * t_jump video_gen = True # Set start time and step size t_start = 0 t_jump = 10 # Load lidar and joint data lidar_dat = ld.get_lidar('data/test_lidar') joint_dat = ld.get_joint('data/test_joint') # theta = np.arange(0,270.25,0.25)*np.pi/float(180) # ax = plt.subplot(111, projection='polar') # ax.plot(theta, lidar_dat[0]['scan'][0]) # ax.set_rmax(10) # ax.set_rticks([0.5, 1, 1.5, 2]) # less radial ticks # ax.set_rlabel_position(-22.5) # get radial labels away from plotted line # ax.grid(True) # ax.set_title("Lidar scan data", va='bottom') # plt.show() # quit() if video_gen: img_dat1 = ld.get_rgb('data/test/RGB_1') print 'ok0.1'
R = rgb_data[k]['image'] R = np.fliplr(R) plt.imshow(R) plt.draw() plt.pause(0.001) def get_joint_index(joint): jointNames = [ 'Neck', 'Head', 'ShoulderL', 'ArmUpperL', 'LeftShoulderYaw', 'ArmLowerL', 'LeftWristYaw', 'LeftWristRoll', 'LeftWristYaw2', 'PelvYL', 'PelvL', 'LegUpperL', 'LegLowerL', 'AnkleL', 'FootL', 'PelvYR', 'PelvR', 'LegUpperR', 'LegLowerR', 'AnkleR', 'FootR', 'ShoulderR', 'ArmUpperR', 'RightShoulderYaw', 'ArmLowerR', 'RightWristYaw', 'RightWristRoll', 'RightWristYaw2', 'TorsoPitch', 'TorsoYaw', 'l_wrist_grip1', 'l_wrist_grip2', 'l_wrist_grip3', 'r_wrist_grip1', 'r_wrist_grip2', 'r_wrist_grip3', 'ChestLidarPan' ] joint_idx = 1 for (i, jnames) in enumerate(joint): if jnames in jointNames: joint_idx = i break return joint_idx if __name__ == "__main__": train_joint3 = ld.get_joint("data/train/train_joint3") train_lidar0 = ld.get_lidar("data/train/train_lidar0") replay_lidar(train_lidar0)
def main(): # Load lidar and joint data lidar_dat = ld.get_lidar('data/train_lidar3') joint_dat = ld.get_joint('data/train_joint3') # Match the joint index to lidar data joint_ind = [] lidar_t = lidar_dat[0]['t'] for i in range(len(lidar_dat)): joint_ind.append(np.argmin(np.abs(lidar_dat[i]['t'] - joint_dat['ts']))) lidar_t = np.vstack((lidar_t, lidar_dat[i]['t'])) np.save('time_lidar.npy', lidar_t) # save time line of lidar # Pick neck and head angles at only that timestep neck = joint_dat['head_angles'][0, joint_ind] head = joint_dat['head_angles'][1, joint_ind] t = joint_dat['ts'][:, joint_ind] # Initialize pos0 = np.array([0, 0, 0]) q0 = np.array([1, 0, 0, 0]) rob = robot(pos0, q0) t_start = 0 pos_arr = np.zeros((1, 2)) q_arr = np.array([1, 0, 0, 0]) T_arr = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) for i in range(t_start, t.shape[1] - 1): # Find the current pose T_cur = h.qp2homo(rob.q, rob.pos) # Odometry to get postion action # Get rot_act from rpy pos_next = lidar_dat[i + 1]['pose'][0] pos_next = np.array([pos_next[0], pos_next[1], 0]) rpy_next = lidar_dat[i + 1]['rpy'][0] q_next = h.rpy2quat(rpy_next[0], rpy_next[1], rpy_next[2]) T_next = h.qp2homo(q_next, pos_next) # Get the control action T_act = np.dot(np.linalg.inv(T_cur), T_next) q_act, pos_act = h.homo2qp(T_act) # motion model rob.predict(q_act, pos_act) # update pos and quaternion array pos_arr = np.vstack((pos_arr, np.array([rob.pos[0], rob.pos[1]]))) q_arr = np.vstack((q_arr, np.array(rob.q))) # update homogeneous transformation array T_cur = np.linalg.inv(h.Tcam2w(rob, head[i], neck[i])) T_arr = np.vstack((T_arr, T_cur)) # store data np.save('pos_pred.npy', pos_arr) np.save('q_pred.npy', q_arr) np.save('T_pred.npy', T_arr) # show the predicted trajectory fig1 = plt.figure(1) plt.plot(pos_arr[:, 0], pos_arr[:, 1]) plt.show()
particle = particle[:, inds] # init weight for resampled particles weight = np.zeros((1, Np)) + 1.0 / Np return particle, weight if __name__ == "__main__": l0 = ld.get_lidar('lidar/train_lidar4') l0_len = len(l0) U = mapping.get_body_pose_inWorld(l0) # use one data every 20 data segment = 15 j0 = ld.get_joint('joint/train_joint4') # 25 particles Np = 25 #init particles, weights, map, control input, pose and motion trjectory particle = np.zeros((3, Np)) weight = np.ones((1, Np)) / Np px = U[:, 0:1] px_h = px particle_pose_timestamp = l0[0]['t'][0][0] map = np.zeros((1201, 1201)) u = np.zeros((3, 1)) for i in range(int(l0_len / segment)):
def TM(data_folder_name, datanum, rgbd_flag): if test: name0 = 'test' else: name0 = 'train' if not if_multiple_data: datanum = '' ''' # Load kinect parameters fex = open('./cameraParam/exParams.pkl', 'rb') fIR = open('./cameraParam/IRcam_Calib_result.pkl', 'rb') fRGB = open('./cameraParam/RGBcamera_Calib_result.pkl', 'rb') exParams = pickle.load(fex, encoding = 'bytes') irParams = pickle.load(fIR, encoding = 'bytes') rgbParams = pickle.load(fRGB, encoding = 'bytes') # get camera intrinsic parameters fx_ir = irParams[b'fc'][0] fy_ir = irParams[b'fc'][1] px_ir = irParams[b'cc'][0] py_ir = irParams[b'cc'][1] fx_rgb = rgbParams[b'fc'][0] fy_rgb = rgbParams[b'fc'][1] px_rgb = rgbParams[b'cc'][0] py_rgb = rgbParams[b'cc'][1] ''' fx_ir, fy_ir = 364.457362486, 364.542810627 px_ir, py_ir = 258.422487562, 202.48713994 fx_rgb, fy_rgb = 1049.3317526, 1051.31847629 px_rgb, py_rgb = 956.910516428, 533.452032441 # get transformation from depth/IR camera to RGB camera # T_alignCams = alignCams(exParams[b'R'], exParams[b'T']) T_alignCams = np.array([[0.99996855, 0.00589981, 0.00529992, 52.2682/1000], [-0.00589406, 0.99998202, -0.00109998, 1.5192/1000], [-0.00530632, 0.00106871, 0.99998535, -0.6059/1000], [0., 0., 0., 1.]]) # load data lidar_new = ld.get_lidar(os.path.join(data_folder_name, name0 + '_lidar' + str(datanum))) joint_new = ld.get_joint(os.path.join(data_folder_name, name0 + '_joint' + str(datanum))) dpt_name = os.path.join(rgbd_folder_name, 'DEPTH' + (if_multiple_data * ('_' + str(datanum)))) dpt_mat = ld.get_depth(dpt_name) image_name = os.path.join(rgbd_folder_name, 'RGB' + (if_multiple_data * ('_' + str(datanum))) + ( \ if_multiple_mat * ('_' + str(1)))) rgb = ld.get_rgb(image_name) # starting time and intervals for data scan mints = 0 int_skip = 1 #prev_img_ind = 0 h = 424 # depth image height & width w = 512 xr = np.arange(w) yr = np.arange(h) u_dpt, v_dpt = np.meshgrid(xr, yr) # v=rows in mat/y-axis # u=cols in mat/x-axis # threshold of plane fitting eps = 0.02 # init video for texture map map = np.load('map' + str(datanum) + '.npy') texture_videoname = 'goodtexture' + str(datanum) + '.mp4' video_t = cv2.VideoWriter(texture_videoname, cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), \ 15, (np.shape(map)[1], np.shape(map)[0])) cv2.namedWindow(texture_videoname, cv2.WINDOW_NORMAL) img_videoname = 'grounddetection' + str(datanum) + '.mp4' video_img=cv2.VideoWriter(img_videoname, cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), \ 15, (1920, 1080)) cv2.namedWindow('im_to_show', cv2.WINDOW_NORMAL) display_floor = np.tile((map > 0)[:, :, np.newaxis], (1, 1, 3)) * 125 display_floor = display_floor.astype('uint8') #path_rx = np.load('path_rx'+str(datanum)+'.npy') #path_ry = np.load('path_ry'+str(datanum)+'.npy') path_x = np.load('path_x'+str(datanum)+'.npy') path_y = np.load('path_y'+str(datanum)+'.npy') path_theta=np.load('path_theta'+str(datanum)+'.npy') path_ts=np.load('path_ts'+str(datanum)+'.npy') # cell to meter path_x=(path_x+1)*0.1-50 path_y = (path_y + 1) * 0.1 - 50 # SLAM for i in np.arange(mints, len(dpt_mat), int_skip): i=int(i) print('\ndepth image ' + str(i)) # check whether to get frames from different RGB mat samergb=(i+1)%300 this_rgb=int((i+1)/300) if not samergb: if this_rgb ==rgbnum: print('\n...texture mapping end!') video_t.release() video_img.release() break print('\nTime to go with another RGB mat...') rgb=ld.get_rgb(os.path.join(rgbd_folder_name, 'RGB' + (if_multiple_data * ('_' + str(datanum))) + ( \ if_multiple_mat * ('_' + str(this_rgb+1))))) # sync time stamps ts_diff = abs(joint_new['ts'] - dpt_mat[i]['t'])[0, :] ind_j = np.where(ts_diff == min(ts_diff))[0][0] ts_diff = abs(path_ts- dpt_mat[i]['t']) ind_p = np.where(ts_diff == min(ts_diff))[0] #pdb.set_trace() # load image #pdb.set_trace() image=rgb[i - 300*(this_rgb)]['image'] dpt=dpt_mat[i]['depth'] dpt = dpt / 1000 # depth in meter now head_angles = joint_new['head_angles'][:, ind_j] rpy = joint_new['rpy'][:, ind_j] - joint_new['rpy'][:, 0] # 3D in dpt frame Pts_dpt = img_to_world(fx_ir, fy_ir, px_ir, py_ir, u_dpt, v_dpt, dpt[v_dpt, u_dpt]) # 3D in cam frame valid_homo = np.hstack((Pts_dpt, np.ones([np.shape(Pts_dpt)[0], 1])))# n*4 Pts_rgb = np.dot(T_alignCams, valid_homo.T) # 4*n # 2D in cam frame u_rgb, v_rgb = world_to_img(fx_rgb, fy_rgb, px_rgb, py_rgb, Pts_rgb.T) u_rgb = u_rgb.astype(np.int) v_rgb = v_rgb.astype(np.int) # get valid indices ind_max = np.logical_and(u_rgb < 1920, v_rgb < 1080) ind_min = np.logical_and(u_rgb >= 0, v_rgb >= 0) ind_range=np.logical_and(ind_max, ind_min) # 3D in world frame T=kinectToGlobal(head_angles, rpy, \ np.array([path_x[ind_p], path_y[ind_p], path_theta[ind_p]])) #pdb.set_trace() R = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0,0,0,1]]) Pts_w=np.dot(T, np.dot(R,Pts_rgb)) #4*n #pdb.set_trace() # get points close to ground plane mask_g=(Pts_w[2,:]<eps) ind_valid=np.logical_and(ind_range, mask_g) true_pos=Pts_w[:2, ind_valid] # pos in grid cells x_true_pos= np.ceil((true_pos[0,:] - (-50)) / 0.1).astype(np.int16) - 1 y_true_pos = np.ceil((true_pos[1,:] - (-50)) / 0.1).astype(np.int16) - 1 x_true_pos = x_true_pos.astype(np.int) y_true_pos = y_true_pos.astype(np.int) # visualization img_base = np.zeros_like(image) img_base[v_rgb[ind_valid], u_rgb[ind_valid]] += 1 img_base = cv2.dilate(img_base, kernel = np.ones((5,5),np.uint8), iterations = 1) gmask = img_base[:, :, 2].copy() g = np.stack((np.zeros_like(gmask), np.zeros_like(gmask), gmask), 2) im_to_show = image.copy()+g*255*0.3 cv2.imshow('im_to_show', im_to_show.astype(np.uint8)) key = cv2.waitKey(100) video_img.write(im_to_show.astype(np.uint8)) # adding texture display_floor[x_true_pos, y_true_pos, 2] = image[v_rgb[ind_valid], u_rgb[ind_valid], 0] display_floor[x_true_pos, y_true_pos, 1] = image[v_rgb[ind_valid], u_rgb[ind_valid], 1] display_floor[x_true_pos, y_true_pos, 0] = image[v_rgb[ind_valid], u_rgb[ind_valid], 2] cv2.imshow(texture_videoname, display_floor) key = cv2.waitKey(100) video_t.write(display_floor)
import matplotlib.pyplot as plt from testing import testing import pickle ''' user input change the file name of dataset and SLAM result ''' joint_file = "joint/train_joint0" lidar_file = "lidar/train_lidar0" slam_data = "slam_data" # the name of slam data that will be saved, needed for texture mapping if __name__ == '__main__': # load data j0 = load_data.get_joint(joint_file) head_idx = load_data.get_joint_index('Head') l0 = load_data.get_lidar(lidar_file) particle_num = 20 # num of particles n = len(l0) # length of iterations particles = np.zeros((3, particle_num)) # empty array for storing particles weights = np.log(np.ones((1, particle_num))[0] / particle_num) # init weights array odom_prev = np.array([0, 0, 0]) # previous odometry i_best = 0 # index of particle with highest weight MAP = slam_lib.init_map() # init a empty map x_array = np.zeros((3, n)) # trajectory array ts_array = np.zeros((3, n))[0] # timestamp array start = timeit.default_timer() # start timer
def SLAM(lidarFilepath, jointFilepath): #get the data lidarData = ld.get_lidar(lidarFilepath) jointData = ld.get_joint(jointFilepath) #find all the joint times allJointTimes = jointData['ts'][0] #find all the head angles from joint data headAngles = jointData['head_angles'] #get the total number of timestamps numTimeStamps = len(lidarData) # init MAP MAP = {} MAP['res'] = 0.05 # meters MAP['xmin'] = -20 # meters MAP['ymin'] = -20 MAP['xmax'] = 20 MAP['ymax'] = 20 MAP['sizex'] = int(np.ceil((MAP['xmax'] - MAP['xmin']) / MAP['res'] + 1)) # cells MAP['sizey'] = int(np.ceil((MAP['ymax'] - MAP['ymin']) / MAP['res'] + 1)) MAP['map'] = np.zeros((MAP['sizex'], MAP['sizey']), dtype=np.double) # DATA TYPE: char or int8 # x-positions of each pixel of the map x_im = np.arange(MAP['xmin'], MAP['xmax'] + MAP['res'], MAP['res']) # y-positions of each pixel of the map y_im = np.arange(MAP['ymin'], MAP['ymax'] + MAP['res'], MAP['res']) # find the thetas theta = np.array([np.arange(-135, 135.25, 0.25) * np.pi / 180.]) # number of particles numParticles = 150 # instantiate the particles particlePoses = np.zeros((numParticles, 3)) weights = (1.0 / numParticles) * np.ones((numParticles, )) fig = plt.figure() im = plt.imshow(MAP['map'], cmap="hot", animated=True) #all predicted robot positions finalPoses = np.zeros((numTimeStamps, 4)) stride = 300 for i in range(0, numTimeStamps - stride, stride): #load the data for this timestamp dataI = lidarData[i] #get the scan at this point di = np.array(dataI['scan']) # take valid indices # indValid = np.logical_and((di < 30), (di > 0.1)) # theta = theta[indValid] # di = di[indValid] #find the position of the head at this time rHead = np.vstack([ di * np.cos(theta), di * np.sin(theta), np.zeros((1, 1081)), np.ones((1, 1081)) ]) #find the time stamp ts = dataI['t'][0][0] #find the closest joint based on the absolute time idx = findIndexOfCloestTimeFrame(allJointTimes, ts) #find the head angles of the closest times neckAngle = headAngles[0][idx] headAngle = headAngles[1][idx] #find the position of the body at this time dNeck = .15 rotNeck = rotzHomo(neckAngle, 0, 0, dNeck) rotHead = rotyHomo(headAngle) rBody = np.dot(np.dot(rotNeck, rotHead), rHead) #get the body roll and pitch angles rollBody = jointData['rpy'][0][i] pitchBody = jointData['rpy'][1][i] rotBodyRoll = rotxHomo(rollBody, 0, 0, 0) rotBodyPitch = rotyHomo(pitchBody) #apply the body roll and pitch angles rBody = np.dot(rotBodyPitch, np.dot(rotBodyRoll, rBody)) #get the pose of the best particle p* indexOfBest = np.argmax(weights) position = particlePoses[indexOfBest, :] #get the position of the best particles xPose = position[0] yPose = position[1] thetaPose = position[2] #add the current prediction of x y to numTimeStepsx2 matrix to plot later finalPoses[i][0] = ts finalPoses[i][1] = xPose finalPoses[i][2] = yPose finalPoses[i][3] = thetaPose #find the yaw and pitch angle of IMU rpy rpy = np.array(dataI['rpy']).T yawAngle = rpy[2] #convert from the body frame to the global frame dBody = .93 + .33 rotGlobal = rotzHomo(yawAngle, xPose, yPose, dBody) rGlobal = np.dot(rotGlobal, rBody) #remove the data of the lidar hitting the ground rGlobal = rGlobal[:, (rGlobal[2, :] >= 1E-4)] #convert to cells xs0 = rGlobal[0, :] ys0 = rGlobal[1, :] xis = np.ceil((xs0 - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 yis = np.ceil((ys0 - MAP['ymin']) / MAP['res']).astype(np.int16) - 1 xPose = np.ceil( (xPose - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 yPose = np.ceil( (yPose - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 #run getMapCellsFromRay to get points that are unoccupied cells = getMapCellsFromRay(xPose, yPose, xis, yis) xsFree = np.int_(cells[0, :]) ysFree = np.int_(cells[1, :]) #find all the occupied cells xsOcc = xis ysOcc = yis #increase log odds of unoccupied cells to the map with log odds indGood = np.logical_and( np.logical_and(np.logical_and((xsFree > 1), (ysFree > 1)), (xsFree < MAP['sizex'])), (ysFree < MAP['sizey'])) logOddsStepDecease = 1 #np.log(.8/.3) MAP['map'][xsFree[indGood], ysFree[indGood]] = MAP['map'][ xsFree[indGood], ysFree[indGood]] - logOddsStepDecease #decrease log odds of occupied cells indGoodOcc = np.logical_and( np.logical_and(np.logical_and((xsOcc > 1), (ysOcc > 1)), (xsOcc < MAP['sizex'])), (ysOcc < MAP['sizey'])) logOddsStepIncrease = 3 #np.log(.7/.2)#.05 MAP['map'][xsOcc[indGoodOcc], ysOcc[indGoodOcc]] = MAP['map'][ xsOcc[indGoodOcc], ysOcc[indGoodOcc]] + logOddsStepIncrease #do localization prediction # get the odometry for the i data odomCurr = np.array(dataI['pose']).T xOdom = odomCurr[0][0] yOdom = odomCurr[1][0] thetaOdom = odomCurr[2][0] currOdomPose = np.array([xOdom, yOdom]).T # get the odometry for the i+1 data odomNext = np.array(lidarData[i + stride]['pose']).T nextXOdom = odomNext[0][0] nextYOdom = odomNext[1][0] nextThetaOdom = odomNext[2][0] nextOdomPose = np.array([nextXOdom, nextYOdom]).T #do prediction step for particles to get particle posotions at t+1 particlePoses = deadReckoning(particlePoses, currOdomPose, thetaOdom, nextOdomPose, nextThetaOdom) #do localization update and get updated weights for particles for particleI in range(0, numParticles): #get particleI at time t+1 particle = particlePoses[particleI, :] #get x and y positions of particle at time t+1 particleX = particle[0] particleY = particle[1] #create a 9x9 window around the particle position x_range = np.arange(particleX - 0.2, particleX + 0.2, 0.05) y_range = np.arange(particleY - 0.2, particleY + 0.2, 0.05) #get the correlation between window and the map corr = mapCorrelation(MAP['map'], x_im, y_im, rGlobal[0:3, :], x_range, y_range) maxCorr = np.max(corr[:]) oldWeight = weights[particleI] #get the new weight newWeight = oldWeight * maxCorr #update new weight in weights weights[particleI] = newWeight #normalize the weights weights = weights / np.sum(weights) #resample the particles Neff = 1.0 / np.sum(weights**2) Nthresh = 20 #do uniform sampling if Neff < Nthresh: weights = (1.0 / numParticles) * np.ones((numParticles, )) #if you want to visualize progress happening #print i #show the map plt.imshow(MAP['map'], cmap="hot") #show the localized path posesX = np.ceil( (finalPoses[:, 1] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1 posesY = np.ceil( (finalPoses[:, 2] - MAP['ymin']) / MAP['res']).astype(np.int16) - 1 plt.scatter(posesX, posesY, s=2) #show the plot plt.show() #save the poses and map as a pickle file with open('allPoses.pickle', 'wb') as handle: pickle.dump(finalPoses, handle, protocol=pickle.HIGHEST_PROTOCOL) with open('finalMap.pickle', 'wb') as handle: pickle.dump(MAP, handle, protocol=pickle.HIGHEST_PROTOCOL)
# saturation limit of log odds map['occu_grids'] = saturationCheck(map['occu_grids']) return map if __name__ == '__main__': if test: name0 = 'test' else: name0 = 'train' lidar_new = ld.get_lidar( os.path.join(data_folder_name, name0 + '_lidar' + str(datanum))) joint_new = ld.get_joint( os.path.join(data_folder_name, name0 + '_joint' + str(datanum))) log_odds_free_update = 0.75 log_odds_occupied_update = 1.5 log_odds_free_sat = -100 log_odds_occupied_sat = 100 # init map map = {} map['res'] = 0.1 # meters map['xmin'] = -30 # meters map['ymin'] = -30 map['xmax'] = 30 map['ymax'] = 30 map['sizex'] = int(np.ceil((map['xmax'] - map['xmin']) / map['res'] + 1)) # cells
y_range = np.arange(-0.2, 0.2 + 0.05, 0.05) # for test set #x_range=np.arange(-0.2,0.2+0.05,0.05) #y_range=np.arange(-0.2,0.2+0.05,0.05) # for training set x_im = np.arange(MAP['xmin'], MAP['xmax'] + MAP['res'], MAP['res']) y_im = np.arange(MAP['ymin'], MAP['ymax'] + MAP['res'], MAP['res']) N = 100 #change to 200 particles for training set 2 to get better map timestep = 100 # pick small value,i.e. 20 for the timestep, will get a pretty clear map but with a long time trajectory = [] ##################################### Read data ######################################## j0 = get_joint("joint/train_joint0") l0 = get_lidar("lidar/train_lidar0") # r0 = get_rgb("cam/RGB_0") # d0 = get_depth("cam/DEPTH_0") # exIR_RGB = getExtrinsics_IR_RGB() # IRCalib = getIRCalib() # RGBCalib = getRGBCalib() # visualize data # replay_lidar(l0) # replay_rgb(r0) l0_pose = [l['pose'] for l in l0] l0_scan = [l['scan'] for l in l0] j0_angle = j0['head_angles'] j0_neck = j0_angle[0]
#Specify map properties resolution = .05 xmin = -40 ymin = -40 xmax = 40 ymax = 40 numParticle = 10 N_threshold = 35 yRange = np.arange(-resolution, resolution + 0.01, resolution) xRange = np.arange(-resolution, resolution + 0.01, resolution) depthFolder = 'C:/Users/Baoqian Wang/Google Drive/cam/DEPTH_0' rgbFolder = 'C:/Users/Baoqian Wang/Google Drive/cam/RGB_0' depthImageList = ld.get_depth(depthFolder) rgbImageList = ld.get_rgb(rgbFolder) #Initialize ParticleSlam object particleSlam = ParticleFilterSlam(numParticle, N_threshold, resolution, xmin, ymin, xmax, ymax, xRange, yRange) # Get lidar data lidar = ld.get_lidar("./lidar/train_lidar0") # Get the head angles data joint = ld.get_joint("./joint/train_joint0") IRcalibration = ld.getIRCalib() # Run the SLAM MAP, bestParticles = particleSlam.slam(lidar, joint, rgbImageList, depthImageList) #fig1 = plt.figure(2) #plt.imshow(MAP['show_map'], cmap = "hot") #plt.show()
def load_obj(name): with open('obj/' + name + '.pkl', 'rb') as f: return pickle.load(f) def roundup(x,t_jump): return int(m.ceil(x / t_jump)) * t_jump video_gen = True t_start = 0 t_jump = 10 lidar_dat = ld.get_lidar('trainset/lidar/train_lidar0') joint_dat = ld.get_joint('trainset/joint/train_joint0') if video_gen: img_dat1 = ld.get_rgb('trainset/cam/RGB_0') #change here to load different dataset print 'ok0.1' img_dat2 = ld.get_rgb('trainset/cam/RGB_3_3') print 'ok0.2' img_dat3 = ld.get_rgb('trainset/cam/RGB_3_4') print 'ok0.3' print 'ok1' # Take the time steps of joints which match lidar time steps joint_ind = [] lidar_t = lidar_dat[0]['t']