def gen_rgb_video(img_folder='../Proj3_2017_Train_rgb',
                  cam_params_folder='..\Proj3_2017_Train_rgb\cameraParam',
                  RGB_file='RGB_0',
                  dataset='0'):
    RGB_file = os.path.join(img_folder, RGB_file)
    RGB0 = ld.get_rgb(RGB_file)
    rgb_times = np.array([d['t'][0][0] for d in RGB0])
    state_times, states, map = _get_pose(dataset=dataset)
    img_width = RGB0[0]['imge'].shape[0]
    img_length = RGB0[0]['imge'].shape[1]

    map_video = cv2.VideoWriter('Train_map_' + dataset + '.avi', -1, 20,
                                (img_width, img_length))
    rgb_video = cv2.VideoWriter('Train_rgb_' + dataset + '.avi', -1, 20,
                                (801, 801))

    # for k in range(rgb_times.shape):
    for k in range(10):

        print '\--------- k = {0}--------'.format(k)
        ts = rgb_times[k]
        rgb = RGB0[k]['images']
        # find corresponding time in state
        state_indx = np.argmax(abs(ts - state_times))
        print '- State time:', state_times[
            state_indx], 'delta_t:', ts - state_times[state_indx]
        map[states[0, k], states[1, k], :] = [70, 70, 228]

        # write video
        rgb_video.write(rgb)
        map_video.write(map)
    rgb_video.release()
    map_video.release()
Example #2
0
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 _get_all_imgs(self,
                      img_folder='../Proj3_2017_Train_rgb',
                      RGB_file='RGB_0',
                      D_file='DEPTH_0'):
        """To display RGB images:
        R = rgb_data[k]['image']
		R = np.fliplr(R)
		plt.imshow(R)
		plt.draw()
		plt.pause(0.001)

		To display depth images:
		for k in xrange(len(depth_data)):
		D = depth_data[k]['depth']
		D = np.flip(D,1)
		for r in xrange(len(D)):
			for (c,v) in enumerate(D[r]):
				if (v<=DEPTH_MIN) or (v>=DEPTH_MAX):
					D[r][c] = 0.0

		plt.imshow(D)
		plt.draw()
		plt.pause(0.001)"""
        print '---------------Obtaining all images-------------------'
        RGB_file = os.path.join(img_folder, RGB_file)
        RGB0 = ld.get_rgb(RGB_file)

        D_file = os.path.join(img_folder, D_file)
        D0 = ld.get_depth(D_file)

        ir_times = np.array([d['t'][0][0] for d in D0])
        rgb_times = np.array([d['t'][0][0] for d in RGB0])
        print ir_times - rgb_times
        print ir_times.shape, rgb_times.shape

        # key_names_depth = ['t','width','imu_rpy','id','odom','head_angles','c','sz','vel','rsz','body_height','tr','bpp','name','height','depth']
        self.ir_imgs = D0
        # key_names_rgb = ['t','width','imu_rpy','id','odom','head_angles','c','sz','vel','rsz','body_height','tr','bpp','name','height','image']
        self.rgb_imgs = RGB0
Example #4
0
if __name__ == "__main__":
    #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)
Example #5
0
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'
    img_dat2 = ld.get_rgb('data/test/RGB_2')
    print 'ok0.2'
    img_dat3 = ld.get_rgb('data/test/RGB_3')
    print 'ok0.3'
    img_dat4 = ld.get_rgb('data/test/RGB_4')
    print 'ok0.4'
    img_dat5 = ld.get_rgb('data/test/RGB_5')
    print 'ok0.5'
    img_dat6 = ld.get_rgb('data/test/RGB_6')
    print 'ok0.6'
    img_dat7 = ld.get_rgb('data/test/RGB_7')
    print 'ok0.7'
    img_dat8 = ld.get_rgb('data/test/RGB_8')
    print 'ok0.8'
Example #6
0
def replayRGBData():
    rgb0 = ld.get_rgb('Proj4_2018_Train_rgb/RGB_3_1')
    pdb.set_trace()
    util.replay_rgb(rgb0)
Example #7
0
def textureMap(lidarFilepath, jointFilepath, rgbFilename, depthFilename):
    #get the calibration data
    rgbCalibFile = "train_rgb/cameraParam/IRcam_Calib_result.pkl"
    depthCalibFile = "train_rgb/cameraParam/RGBcamera_Calib_result.pkl"
    IRToRGBFile = "train_rgb/cameraParam/exParams.pkl"
    with open(rgbCalibFile, 'rb') as handle:
        rgbCalib = pickle.load(handle)

    with open(depthCalibFile, 'rb') as handle:
        depthCalib = pickle.load(handle)

    #get the rgb calibration data
    fRGB = rgbCalib['fc']
    fRGBy = fRGB[0]
    fRGBx = fRGB[1]
    rgbPrincipalPoint = rgbCalib['cc']
    rgbYPrincipalPoint = rgbPrincipalPoint[0]
    rgbXPrincipalPoint = rgbPrincipalPoint[1]

    #get the IR calibratin data
    fIR = depthCalib['fc']
    fIRy = fIR[0]
    fIRx = fIR[1]
    depthPrincipalPoint = rgbCalib['cc']
    depthYPrincipalPoint = depthPrincipalPoint[0]
    depthXPrincipalPoint = depthPrincipalPoint[1]

    #get the transformation from IR to RGB
    with open(IRToRGBFile, 'rb') as handle:
        IRToRGBData = pickle.load(handle)
    #get the rotation and translations between the IR camera and the RGB camera
    IRToRGBRotation = IRToRGBData['R']
    IRToRGBTranslation = IRToRGBData['T']

    #import the SLAM data
    #NOTE THAT THESE FILES ARE FROM THE LAST SLAM ROUTINE RUN!
    #IF YOU WANT TO RUN TEXTUREMAP ON A NEW DATASET, YOU HAVE TO RERUN SLAM by uncommenting the following command
    #SLAM(lidarFilename,jointFilename)
    #If you've already run SLAM with the dataset, then just continue
    with open('allPoses.pickle', 'rb') as handle:
        allPoses = pickle.load(handle)

    with open('finalMap.pickle', 'rb') as handle:
        finalMap = pickle.load(handle)

    #get the RGB data
    rgbData = ld.get_rgb(rgbFilename)
    depthData = ld.get_depth(depthFilename)

    #initialize the occupancy grid
    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

    #number of images
    numImages = len(rgbData)
    for i in range(0, numImages):
        image = rgbData[i]['image']  #1920 x 1080 x 3
        depth = depthData[i]['depth']  # 412 x 512
        time = rgbData[i]['t']

        #create a meshgrid of the depth to do the transformations
        row = np.arange(0, depth.shape[0], 1)
        col = np.arange(0, depth.shape[1], 1)
        cols, rows = np.meshgrid(col, row)

        #do the RGB and depth image alignment
        #convert the IR data to 3Dpoints
        xIR = np.multiply(cols - depthXPrincipalPoint, depth) / fIRx
        yIR = np.multiply(rows - depthYPrincipalPoint, depth) / fIRy

        #convert image into X vector of all xyz locations
        X = np.vstack((np.array(xIR).ravel(), np.array(yIR).ravel(),
                       np.array(depth).ravel()))

        #transform the IR data into the RGB image frame using the camera parameters
        XRGB = np.dot(IRToRGBRotation, X) + IRToRGBTranslation

        #transform the points in the image to 3D points to project
        xRGB = XRGB[0, :]
        yRGB = XRGB[1, :]
        zRGB = XRGB[2, :]

        #find the colors associated with the 3D points
        #convert back to the uv points
        uRGB = np.round(fRGBy * xRGB / zRGB +
                        rgbXPrincipalPoint).astype('uint8')
        vRGB = np.round(fRGBx * yRGB / zRGB +
                        rgbYPrincipalPoint).astype('uint8')

        #get the colors for the 3D Points XRGB
        rgbColors = image[uRGB, vRGB, :]

        #convert these colors to the global frame
        #find the rotations for head and yaw angles
        #find the head angles of the closest times
        neckAngle = rgbData[i]['head_angles'].T[0]
        headAngle = rgbData[i]['head_angles'].T[1]

        #find the position of the body at this time
        dNeck = .07
        rotNeck = rotzHomo(neckAngle, 0, 0, dNeck)
        rotHead = rotyHomo(headAngle)
        totalRotHead = np.dot(rotNeck, rotHead)

        #find the closest timestep from the SLAM data
        idx = findIndexOfCloestTimeFrame(allPoses[:, 0], time)

        #find the closest pose x,y position from the SLAM data
        pose = allPoses[idx, :]
        xPose = pose[1]
        yPose = pose[2]
        thetaPose = pose[3]

        #convert the 3D RGB points to the global frame
        dBody = .93 + .33
        rotGlobal = rotzHomo(thetaPose, xPose, yPose, dBody)
        totalRotation = np.dot(rotGlobal, totalRotHead)

        #rotate the RGB image and the depth image to the global frame
        FourDPoints = np.vstack((XRGB, np.ones(XRGB.shape[1], )))
        rotated3DPoints = np.dot(totalRotation, FourDPoints)

        #find the ground plane on the rotated data via RANSAC or via thresholding
        thresh = 1
        indicesToKeep = rotated3DPoints[2, :] < thresh
        pointsToPaint = rotated3DPoints[0:3, indicesToKeep].astype('uint8')
        rgbValuesOfPointsToKeep = rgbColors[indicesToKeep, :]
Example #8
0
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)
Example #9
0
def texture_mapping(MAP, w_T_b_best):

    IRCalib = getIRCalib()
    Matrix_IRCalib = np.array([[
        IRCalib['fc'][0], IRCalib['ac'] * IRCalib['fc'][0], IRCalib['cc'][0]
    ], [0, IRCalib['fc'][1], IRCalib['cc'][1]], [0, 0, 1]])
    RGBCalib = getRGBCalib()
    Matrix_RGBCalib = np.array([[
        RGBCalib['fc'][0], RGBCalib['ac'] * RGBCalib['fc'][0],
        RGBCalib['cc'][0]
    ], [0, RGBCalib['fc'][1], RGBCalib['cc'][1]], [0, 0, 1]])
    exIR_RGB = getExtrinsics_IR_RGB()

    l0 = get_lidar("lidar/train_lidar0")
    r0 = get_rgb("cam/RGB_0")
    d0 = get_depth("cam/DEPTH_0")

    lidar_t = []
    depth_t = []
    Tneck = np.zeros((4, 4))
    Thead = np.zeros((4, 4))

    for i in range(len(l0)):
        lidar_t1 = np.array(l0[i]['t'][0])
        lidar_t.append(lidar_t1)

    for j in range(len(d0)):
        depth_t1 = np.array(d0[j]['t'][0])
        depth_t.append(depth_t1)

    rgb_it = [r['image'] for r in r0]
    rgb_angle = [r['head_angles'] for r in r0]
    rgb_neck = rgb_angle[0]
    rgb_head = rgb_angle[1]

    depth_dt = [d['depth'] for d in d0]

    ############## do the time alignment for lidar time compare with depth time
    xxx = np.array((lidar_t))
    yyy = np.array((depth_t))

    xx = np.array(([l[0] for l in xxx]))
    yy = np.array(([l[0] for l in yyy]))
    idx1 = np.searchsorted(xx, yy, side="left").clip(max=xx.size - 1)
    mask = (idx1 > 0) & \
           ((idx1 == len(xx)) | (np.fabs(yy - xx[idx1 - 1]) < np.fabs(yy - xx[idx1])))
    ##### the index is the resulting lidar timestamp index that has the closest time step as the depth's timestamp
    index1 = np.array((idx1 - mask)).tolist()

    #print(index)

    # transform from ir to rgb frame rgb T_ir
    rgb_T_ir = np.zeros((4, 4))
    rgb_T_ir[0:3, 0:3] = exIR_RGB['rgb_R_ir']
    rgb_T_ir[0, 3] = exIR_RGB['rgb_T_ir'][0]
    rgb_T_ir[1, 3] = exIR_RGB['rgb_T_ir'][1]
    rgb_T_ir[2, 3] = exIR_RGB['rgb_T_ir'][2]
    rgb_T_ir[3, 3] = 1

    # tranform from rgb to body frame  b_T_rgb

    # use the idxs to track the
    Rn = euler2mat(0, 0, rgb_neck, axes='sxyz')
    Tneck[0:3, 0:3] = Rn
    Tneck[0, 3] = 0
    Tneck[1, 3] = 0
    Tneck[2, 3] = 0.07
    Tneck[3, 3] = 1
    Rh = euler2mat(0, rgb_head, 0, axes='sxyz')
    Thead[0:3, 0:3] = Rh  # transform from lidar to body frame
    Thead[0, 3] = 0
    Thead[1, 3] = 0
    Thead[2, 3] = 0.33
    Thead[3, 3] = 1
    b_T_rgb = np.dot(Thead, Tneck)

    # w_T_b is the current best particle
    w_T_c_rgb = np.dot(w_T_b_best, b_T_rgb)

    o_T_c = np.array([[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 0], [0, 0, 0,
                                                                   1]])

    map_x = MAP['sizex']
    map_y = MAP['sizey']

    my_x = map_x.tolist()
    my_y = map_y.tolist()

    tmt = np.zeros((MAP['sizex'], MAP['sizey'], 3))

    #tmt[my_x, my_y, 0] =
    #tmt[my_x, my_y, 1] =
    #tmt[my_x, my_y, 2] =

    return tmt
Example #10
0
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)
    if len(RGB_filenames) == 0:
        print "   No RGB or DEPTH data."
        load_depth = False
    elif len(RGB_filenames) == 1:
        print "   Loading RGB data..."
        RGBs.extend(load.get_rgb(RGB_folder + "/" + RGB_filename_prefix))
        print "   Done!"
    else:
        print "   Loading " + str(len(RGB_filenames)) + " RGB datafiles..."
        for i in xrange(len(RGB_filenames)):
            print "      Loading RBG data #" + str(i + 1)
            RGB_filename = RGB_folder + "/" + RGB_filename_prefix + "_" + str(
                i + 1)
            RGBs.extend(load.get_rgb(RGB_filename))
            print "      Done!"
        print "   Done!"
    if load_depth:
        print "   Loading DEPTH data..."
        depth_filename = folder + "/cam/DEPTH" + len(dataset) * "_" + dataset
        depths = load.get_depth(depth_filename)
        print "   Done!"
Example #11
0
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']
for i in xrange(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']))