def createCloud(fileName):
points = []
for line in open(fileName):
splittedLine = re.split('(?:\s|[,]|\n)+', line.strip())
coord = map(float, splittedLine)
while len(coord) < 3:
coord.append(0)
points.append(coord)
return point_cloud.create_cloud_xyz32(Header(), points)
def createCloud(fileName):
points = []
for line in open(fileName):
splittedLine = re.split("(?:\s|[,]|\n)+", line.strip())
coord = map(float, splittedLine)
while len(coord) < 3:
coord.append(0)
points.append(coord)
return point_cloud.create_cloud_xyz32(roslib.msg.Header(), points)
def process_faces(self, boxes):
if not self.depth_image:
print 'whoops! no depth image!'
return
skip = 1
xyz_all = []
idNum = 1
#f = open("output.dat","w")
boxNum = 1
prevV = None
for (x1,y1,x2,y2) in boxes:
x1, y1, x2, y2 = cv.Round(x1), cv.Round(y1), cv.Round(x2), cv.Round(y2)
for xpad, ypad, ypad2 in zip([10,10],[40,40],[40,0]):
if ypad2 == 0:
colorFlag = 2
else:
colorFlag = 1
u,v = np.mgrid[y1-ypad:y2+ypad2:skip,x1-xpad:x2+xpad:skip]
u,v = u.flatten(), v.flatten()
d = np.asarray(self.depth_image[y1-ypad:y2+ypad2,x1-xpad:x2+xpad])
d = d[::skip,::skip]
d = d.flatten()
u = u[np.isfinite(d)]
v = v[np.isfinite(d)]
d = d[np.isfinite(d)]
### only if from dat, bad value should actually be 2047
u = u[d!=0]
v = v[d!=0]
d = d[d!=0]
#u = u[d < alpha*median]
#v = v[d < alpha*median]
#d = d[d < alpha*median]
xyz = self.makeCloud_correct(u,v,d)
'''median = sorted(xyz[:,2])[len(d)//2]
alpha = 1.2
xyz = xyz[xyz[:,2] < alpha*median,:]'''
xyz_all.extend(xyz)
try:
n = len(xyz)
mu = np.sum(xyz, axis=0)/n
xyz_norm = xyz - mu
cov = np.dot(xyz_norm.T, xyz_norm)/n
e, v = eig(cov)
#print v[2]
if v[2][2] > 0: v[2] = -v[2]
# publish marker here
if colorFlag == 1:
m = self.makeMarker(mu, v[2], idNum=idNum, color=(1,0,0))
print '%d %f %f %f' % (boxNum, v[2][0], v[2][1], v[2][2]),
else:
m = self.makeMarker(mu, v[2], idNum=idNum, color=(0,1,0))
print '%f %f %f ' % (v[2][0], v[2][1], v[2][2])
idNum += 1
self.pubFaceNormals.publish(m)
except:
pass
boxNum += 1
# getting rid of bad markers in rviz by sending them away
for blah in range(idNum,25):
m = self.makeMarker([-10,-10,-10], [1,1,1], idNum=idNum, color=(1,1,1))
idNum += 1
self.pubFaceNormals.publish(m)
pc = PointCloud2()
pc.header.frame_id = "/camera_rgb_optical_frame"
pc.header.stamp = rospy.Time()
pc = create_cloud_xyz32(pc.header, xyz_all)
self.pubFaceCloud.publish(pc)
def process_faces(self, boxes):
if not self.depth_image:
print 'whoops! no depth image!'
return
skip = 1
xyz_all = []
idNum = 1
#f = open("output.dat","w")
boxNum = 1
prevV = None
for (x1, y1, x2, y2) in boxes:
x1, y1, x2, y2 = cv.Round(x1), cv.Round(y1), cv.Round(
x2), cv.Round(y2)
for xpad, ypad, ypad2 in zip([10, 10], [40, 40], [40, 0]):
if ypad2 == 0:
colorFlag = 2
else:
colorFlag = 1
u, v = np.mgrid[y1 - ypad:y2 + ypad2:skip,
x1 - xpad:x2 + xpad:skip]
u, v = u.flatten(), v.flatten()
d = np.asarray(self.depth_image[y1 - ypad:y2 + ypad2,
x1 - xpad:x2 + xpad])
d = d[::skip, ::skip]
d = d.flatten()
u = u[np.isfinite(d)]
v = v[np.isfinite(d)]
d = d[np.isfinite(d)]
### only if from dat, bad value should actually be 2047
u = u[d != 0]
v = v[d != 0]
d = d[d != 0]
#u = u[d < alpha*median]
#v = v[d < alpha*median]
#d = d[d < alpha*median]
xyz = self.makeCloud_correct(u, v, d)
'''median = sorted(xyz[:,2])[len(d)//2]
alpha = 1.2
xyz = xyz[xyz[:,2] < alpha*median,:]'''
xyz_all.extend(xyz)
try:
n = len(xyz)
mu = np.sum(xyz, axis=0) / n
xyz_norm = xyz - mu
cov = np.dot(xyz_norm.T, xyz_norm) / n
e, v = eig(cov)
#print v[2]
if v[2][2] > 0: v[2] = -v[2]
# publish marker here
if colorFlag == 1:
m = self.makeMarker(mu,
v[2],
idNum=idNum,
color=(1, 0, 0))
print '%d %f %f %f' % (boxNum, v[2][0], v[2][1],
v[2][2]),
else:
m = self.makeMarker(mu,
v[2],
idNum=idNum,
color=(0, 1, 0))
print '%f %f %f ' % (v[2][0], v[2][1], v[2][2])
idNum += 1
self.pubFaceNormals.publish(m)
except:
pass
boxNum += 1
# getting rid of bad markers in rviz by sending them away
for blah in range(idNum, 25):
m = self.makeMarker([-10, -10, -10], [1, 1, 1],
idNum=idNum,
color=(1, 1, 1))
idNum += 1
self.pubFaceNormals.publish(m)
pc = PointCloud2()
pc.header.frame_id = "/camera_rgb_optical_frame"
pc.header.stamp = rospy.Time()
pc = create_cloud_xyz32(pc.header, xyz_all)
self.pubFaceCloud.publish(pc)
def main():
if len(sys.argv) < 3:
print 'grab_cbs_auto cb_config.yaml output_bag.bag'
return
rospy.init_node("grab_cbs")
f = file(sys.argv[1], 'r')
cb_config = yaml.safe_load(f.read())
print cb_config
f.close()
is_kinect = rospy.get_param("/grab_cbs/is_kinect", True)
# load cb stuff
chessboard = ChessboardInfo()
chessboard.n_cols = cb_config['cols'] # 6
chessboard.n_rows = cb_config['rows'] # 7
chessboard.dim = cb_config['dim'] # 0.0258
calib = Calibrator([chessboard])
bridge = CvBridge()
l = DataListener(is_kinect, bridge, calib)
tf_list = tf.TransformListener()
cb_knowns = []
for j in range(chessboard.n_cols):
for i in range(chessboard.n_rows):
cb_knowns.append((chessboard.dim*i, chessboard.dim*j, 0.0))
bag = rosbag.Bag(sys.argv[2], 'w')
i = 0
while not rospy.is_shutdown():
if raw_input("Press enter to take CB, type 'q' to quit: ") == "q":
break
tries = 0
while not rospy.is_shutdown() and tries < 3:
corners = l.wait_for_new(5.)
if corners is None:
print "No corners detected"
tries += 1
continue
corners_2d = np.uint32(np.round(corners)).tolist()
corners_3d = []
if is_kinect:
for x,y,z in read_points(l.cur_pc, field_names=['x', 'y', 'z'], uvs=corners_2d):
corners_3d.append((x,y,z))
frame_id = l.cur_pc.header
else:
obj_pts = cv.fromarray(np.array(cb_knowns))
img_pts = cv.fromarray(np.array(corners))
K = cv.fromarray(np.reshape(l.cam_info.K,(3,3)))
D = cv.fromarray(np.array([l.cam_info.D]))
R_vec = cv.fromarray(np.zeros((3,1)))
t = cv.fromarray(np.zeros((3,1)))
cv.FindExtrinsicCameraParams2(obj_pts, img_pts, K, D, R_vec, t)
R_mat = cv.fromarray(np.zeros((3,3)))
cv.Rodrigues2(R_vec, R_mat)
T = PoseConv.to_homo_mat(np.mat(np.asarray(t)).T.A.tolist(),
np.mat(np.asarray(R_mat)).A.tolist())
cb_knowns_mat = np.vstack((np.mat(cb_knowns).T, np.mat(np.ones((1, len(cb_knowns))))))
corners_3d = np.array((T * cb_knowns_mat)[:3,:].T)
frame_id = l.cur_img.header
print corners_3d
if np.any(np.isnan(corners_3d)):
print "Pointcloud malformed"
tries += 1
continue
now = rospy.Time.now()
corners_pc = create_cloud_xyz32(frame_id, corners_3d)
try:
pose = tf_list.lookupTransform('/base_link', '/ee_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF screwed up..."
continue
bag.write("/pose", PoseConv.to_pose_stamped_msg('/base_link', pose), now)
bag.write("/pc", corners_pc, now)
print "Wrote pose/CB to bag file"
break
i += 1
bag.close()
