def calc_3d_vel_estimate(self, pcOld, pcNew):
(rows, cols) = (self.velX.rows, self.velY.cols)
self.rows = rows
self.cols = cols
# initialize velocity matrix (2d array of 3-tuples)
V = [[None]*cols for row in xrange(rows)]
# iterate through the point clouds
self.old_points = [pt for pt in point_cloud.read_points(pcOld)]
self.new_points = [pt for pt in point_cloud.read_points(pcNew)]
# get 3d velocity estimate at each pixel
for i in xrange(rows):
for j in xrange(cols):
iDiff = int(round(self.velX[i,j]))
jDiff = int(round(self.velY[i,j]))
iOld = i - iDiff
jOld = j - jDiff
# handle out of bounds: set V at (i,j) to None
# TODO think of a better way
if iOld<0 or iOld >=rows or jOld<0 or jOld>cols:
V[i][j] = None
else:
new_point = self.new_points[rows*i + j]
old_point = self.old_points[rows*iOld + jOld]
# use difference b/w new_point and old_point as vel estimate
V[i][j] = (vel_x, vel_y, vel_z) = tuple(new-old for (new,old) in zip(new_point,old_point))
self.vel3d = V
def estimate_3d_vel(self, flowX, flowY, last_points_msg, points_msg):
rows, cols = flowX.rows, flowX.cols
stime = time()
last_points = [pt for pt in point_cloud.read_points(last_points_msg)]
elapsed = time()-stime
print 'point cloud iteration: ', elapsed, 'sec'
stime = time()
points = [pt for pt in point_cloud.read_points(points_msg)]
elapsed = time()-stime
print 'point cloud iteration: ', elapsed, 'sec'
new_points = np.empty([rows,cols],dtype=('f4,f4,f4'))
pixel_vels_3d = np.empty([rows,cols],dtype=('f4,f4,f4'))
# get 3d velocity estimate at each pixel
stime = time()
for i in xrange(rows):
for j in xrange(cols):
iDiff = int(round(flowX[i,j]))
jDiff = int(round(flowY[i,j]))
iNew = i + iDiff
jNew = j + jDiff
#iOld = i - iDiff
#jOld = j - jDiff
# handle out of bounds: set V at (i,j) to NaN
# TODO think of a better way?
#if iOld<0 or iOld >=rows or jOld<0 or jOld>=cols:
# CORRECTION
if iNew<0 or iNew>=rows or jNew<0 or jNew>=cols:
try:
pixel_vels_3d[i,j] = (np.nan, np.nan, np.nan)
except:
import pdb; pdb.set_trace()
else:
# CORRECTION
#newp = points[cols*i+j]
#oldp = last_points[cols*iOld+jOld]
newp = points[cols*iNew+jNew]
oldp = last_points[cols*i+j]
pixel_vels_3d[i,j] = newp[0]-oldp[0], newp[1]-oldp[1], newp[2]-oldp[2]
elapsed = time()-stime
print 'pixel-wise velocity estimate: ', elapsed, 'sec'
return pixel_vels_3d
def smooth_3d_vel(self, pixel_vels, pointcloud_message, region_size):
rows, cols = pixel_vels.shape
stime = time()
points = [pt for pt in point_cloud.read_points(pointcloud_message)]
print 'point cloud iteration: ', (time()-stime), 'sec'
s_rows, s_cols = rows/region_size,cols/region_size
result_points = []
velocities = []
stime = time()
isnan = math.isnan
point_index = 0
for region_i in xrange(s_rows):
for region_j in xrange(s_cols):
v_count = 0.
v_accum = [0.,0.,0.]
p_accum = [0.,0.,0.]
for i in xrange(region_i*region_size,region_i*region_size+region_size):
for j in xrange(region_j*region_size,region_j*region_size+region_size):
v = vx,vy,vz = pixel_vels[i,j]
if not (isnan(vx) or isnan(vy) or isnan(vz)):
p = px,py,pz = points[i * cols + j]
if not (isnan(px) or isnan(py) or isnan(pz)):
p_accum[0]+=px; p_accum[1]+=py; p_accum[2]+=pz
v_accum[0]+=vx; v_accum[1]+=vy; p_accum[2]+=vz
v_count += 1
if not (v_count > 0): v_count = 1. # prevent division by zero
result_points.append( (p_accum[0]/v_count, p_accum[1]/v_count, p_accum[2]/v_count) )
velocities.append( (v_accum[0]/v_count, v_accum[1]/v_count, v_accum[2]/v_count))
print 'smoothing 3d velocity: ', (time() - stime), 'sec'
return result_points, velocities
def getDepthChanges(self, pclOld, pclNew, X, Y):
rospy.loginfo("calculating depth change")
Z = cv.CreateMat(X.rows, X.cols, X.type) # X.type?
self.old_depth = [pt[2] for pt in point_cloud.read_points(pclOld)]
self.new_depth = [pt[2] for pt in point_cloud.read_points(pclNew)]
(rows, cols) = (X.rows, X.cols)
for i in xrange(rows):
for j in xrange(cols):
iDiff = round(X[i,j])
jDiff = round(Y[i,j])
iOld = i - iDiff
jOld = j - jDiff
# handle out of bounds TODO think of a better way
if iOld<0 or iOld >=rows or jOld<0 or jOld>cols:
Z[i,j] = float("nan")
else:
Z[i,j] = self.new_depth[rows*i + j] - self.old_depth[rows*iOld + jOld]
self.velZ = Z
def estimate_3d_vel(self, points_2d, vels_2d, last_points_msg, points_msg):
stime = time()
last_points = [pt for pt in point_cloud.read_points(last_points_msg)]
elapsed = time()-stime
print 'point cloud iteration: ', elapsed, 'sec'
stime = time()
points = [pt for pt in point_cloud.read_points(points_msg)]
elapsed = time()-stime
print 'point cloud iteration: ', elapsed, 'sec'
points_3d = []
vels_3d = []
# get 3d velocity estimate at each pixel
stime = time()
for pt, vel in izip(points_2d, vels_2d):
#for i in xrange(rows):
#for j in xrange(cols):
px, py = pt
vx, vy = vel
p2x, p2y = int(round(px + vx)), int(round(py + vy))
# handle out of bounds: set V at (i,j) to NaN
# TODO think of a better way?
#if iOld<0 or iOld >=rows or jOld<0 or jOld>=cols:
# CORRECTION
if not (p2x<0 or p2x>=640 or p2y<0 or p2y>=480) :
newp = points[640*p2y+p2x]
oldp = last_points[640*py+px]
points_3d.append( oldp )
vels_3d.append(( newp[0]-oldp[0], newp[1]-oldp[1], newp[2]-oldp[2] ))
elapsed = time()-stime
print 'Feature-wise 3d velocity estimate: ', elapsed, 'sec'
return points_3d, vels_3d
d_msg = None
p_msg = None
for topic, msg, t in bag.read_messages(topics=[depth_topic, rgb_topic, pcl_topic]):
if topic == depth_topic: d_msg = msg
elif topic == pcl_topic: p_msg = msg
if d_msg != None and p_msg != None and d_msg.header.stamp.to_nsec() == p_msg.header.stamp.to_nsec():
break
# convert Image to cv format
bridge = CvBridge()
im = bridge.imgmsg_to_cv(d_msg)
# print out depth values and hope for the best
count = 0
ptmax = 2000
for v in point_cloud.read_points(p_msg):
print v
count += 1
if count > ptmax: break
count = 0
for i in xrange(im.rows):
for j in xrange(im.cols):
count += 1
if count > ptmax: break
print im[i,j]
if count > ptmax: break
finally:
bag.close()
