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
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)
def replayDepthData():
depth0 = ld.get_depth('Proj4_2018_Train_rgb/DEPTH_0')
# pdb.set_trace()
util.replay_depth(depth0)
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, :]
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)
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
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!"
rgb_exists = True
except:
print " Error loading RGB or DEPTH data."
print "Done!\n"
####################################################################################################
# INITIALIZE VARIABLES #
####################################################################################################
# particle related variables (particles are in SE3)
n_particles_thresh = 10
n_particles = 100
particle_weights = np.ones(n_particles) / (1.0 * n_particles)
particle_corrs = np.zeros(n_particles)