def cast_ray(self, point_in, tf, name):
base_link_point = tf.transformPoint('/base_link', point_in)
t = tf.getLatestCommonTime('/base_link', point_in.header.frame_id)
pos, quat = tf.lookupTransform('/base_link', point_in.header.frame_id, t)
height = pos[2]
x_slope = (base_link_point.point.x - pos[0])/(pos[2]-base_link_point.point.z)
y_slope = (base_link_point.point.y - pos[1])/(pos[2]-base_link_point.point.z)
ground_point = np.array([0.,0.,0.])
ground_point[0] = x_slope*height + pos[0]
ground_point[1] = y_slope*height + pos[1]
ground_named_point = NamedPoint()
ground_named_point.point.x = ground_point[0]
ground_named_point.point.y = ground_point[1]
ground_named_point.point.z = ground_point[2]
ground_named_point.header = point_in.header
ground_named_point.header.frame_id = 'base_link'
ground_named_point.header.stamp = point_in.header.stamp
ground_named_point.name = name
odom_named_point = NamedPoint()
odom_point = self.tf.transformPoint('/odom',ground_named_point)
odom_named_point.point = odom_point.point
odom_named_point.header = point_in.header
odom_named_point.header.frame_id = "/odom"
odom_named_point.header.stamp = point_in.header.stamp
odom_named_point.name = name
return ground_named_point, odom_named_point
def split_mser_and_color(self, img, mask, i, q_img, q_proj, header):
regions = self.mser.detect(img, mask)
for r in regions:
r += np.array([0,self.height*i])
hulls = [cv2.convexHull(r.reshape(-1,1,2)) for r in regions]
img = cv2.cvtColor(self.img,cv2.COLOR_BGR2RGB)
mask = np.zeros((img.shape[0],img.shape[1]))
for h in (hulls):
top_index, similarity = self.compute_color_name(h,img,mask)
if self.sample_names[top_index] is not None and similarity >= self.sample_thresh[top_index]:
moments = cv2.moments(h)
# converts to x,y
location = np.array([moments['m10']/moments['m00'],moments['m01']/moments['m00']])
named_img_point = NamedPoint()
named_img_point.header = copy.deepcopy(header)
named_img_point.point.x = location[0]
named_img_point.point.y = location[1]
named_img_point.name = self.sample_names[top_index]
named_point = self.project_xyz_point(h, top_index, header)
#rospy.logdebug("Named_point: %s",named_point)
size = self.real_size_check(h,header)
if self.min_size < size < self.max_size:
q_img.put(named_img_point)
q_proj.put(named_point)
def cast_ray(self, point_in, tf, name):
#rospy.logdebug("Point In: %s", point_in)
base_link_point = tf.transformPoint('/base_link', point_in)
t = tf.getLatestCommonTime('/base_link', point_in.header.frame_id)
pos, quat = tf.lookupTransform('/base_link', point_in.header.frame_id, t)
height = pos[2]
#rospy.logdebug("Pos: %s", pos)
x_slope = np.abs((pos[0]-base_link_point.point.x)/(pos[2]-base_link_point.point.z))
y_slope = np.abs((pos[1]-base_link_point.point.y)/(pos[2]-base_link_point.point.z))
#rospy.logdebug("X Slope: %s", x_slope)
#rospy.logdebug("Y Slope: %s", y_slope)
ground_point = np.array([0.,0.,0.])
ground_point[0] = x_slope*height
ground_point[1] = y_slope*height
ground_named_point = NamedPoint()
ground_named_point.point.x = ground_point[0]
ground_named_point.point.y = ground_point[1]
ground_named_point.point.z = ground_point[2]
ground_named_point.header = copy.deepcopy(point_in.header)
ground_named_point.header.frame_id = 'base_link'
ground_named_point.name = name
odom_named_point = self.tf.transformPoint('/odom',ground_named_point)
#rospy.logdebug("Ground Point: %s", ground_named_point)
#rospy.logdebug("Odom Point: %s", odom_named_point)
return ground_named_point, odom_named_point
def project_xyz_point(self, hull, top_index, header):
if self.disp_img is not None:
rect = cv2.boundingRect(hull)
local_disp = self.disp_img[rect[1]:rect[1]+rect[3],rect[0]:rect[0]+rect[2]]
# Trim off extreme disparities
local_disp = cv2.threshold(local_disp.astype(np.float32), self.min_disp, 0, cv2.THRESH_TOZERO)[1]
local_disp = cv2.threshold(local_disp.astype(np.float32), self.max_disp, 0, cv2.THRESH_TOZERO_INV)[1]
# Sort disparities, grab ends, compute mean
count = cv2.countNonZero(local_disp)
local_disp = local_disp.reshape((rect[2]*rect[3],1))
local_disp = np.sort(local_disp)
accum_disp = local_disp[:10].sum() + local_disp[count-10:count].sum()
ave_disp = accum_disp/20.
# Depth in meters
ave_depth = self.f*self.T/ave_disp
x = rect[0]+rect[2]/2
y = rect[1]+rect[3]/2
XY = np.dot(self.inv_K,np.array([x,y,1]))
XYZ = XY*ave_depth
named_point = NamedPoint()
named_point.name = self.sample_names[top_index]
named_point.header = copy.deepcopy(header)
named_point.point.x = XYZ[0]
named_point.point.y = XYZ[1]
named_point.point.z = XYZ[2]
#self.named_point_pub.publish(named_point)
return named_point
else:
rect = cv2.boundingRect(hull)
x = (rect[0]+rect[2]/2)#*2.
y = (rect[1]+rect[3]/2)#*2.
XY = np.dot(self.inv_K,np.array([x,y,1]))
named_point = NamedPoint()
named_point.name = self.sample_names[top_index]
named_point.header = copy.deepcopy(header)
named_point.point.x = XY[0]
named_point.point.y = XY[1]
named_point.point.z = 1.0
return named_point
def handle_monocular_img(self, Image):
detections = self.find_samples(Image)
self.debug_img_pub.publish(self.bridge.cv_to_imgmsg(cv2.cv.fromarray(self.debug_img),'bgr8'))
for d in detections:
if detections[d]['location'] is None:
continue
else:
location = detections[d]['location']
pt=self.create_point_stamped(location)
self.sample_imgpoints[d].publish(pt)
named=NamedPoint()
named.header=pt.header
named.point=pt.point
named.name=d
self.namedpoints.publish(named)
def find_samples(self, Image):
self.img = np.asarray(self.bridge.imgmsg_to_cv(Image,'bgr8'))
self.debug_img = self.img.copy()
#if self.static_mask is None:
# self.static_mask = np.zeros((self.img.shape[0],self.img.shape[1],1),np.uint8)
# self.static_mask[400:,:,:] = 1
lab = cv2.cvtColor(self.img, cv2.COLOR_BGR2LAB)
gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY)
#a_regions = self.mser.detect(lab[:,:,1] ,None)
#a_hulls = [cv2.convexHull(r.reshape(-1,1,2)) for r in a_regions]
#b_regions = self.mser.detect(lab[:,:,2] ,None)
#b_hulls = [cv2.convexHull(r.reshape(-1,1,2)) for r in b_regions]
#g_regions = self.mser.detect(gray, self.static_mask)
mask = np.ones((self.img.shape[0],self.img.shape[1],1),np.uint8)
mask[:,:100] = 0
#mask[:,-100:] = 0
g_regions = self.mser.detect(gray, mask)
rospy.logdebug("number of regions: %s", len(g_regions))
g_hulls = [cv2.convexHull(r.reshape(-1,1,2)) for r in g_regions]
rospy.logdebug("number of hulls: %s", len(g_hulls))
img = cv2.cvtColor(self.img,cv2.COLOR_BGR2RGB)
mask = np.zeros((img.shape[0],img.shape[1]))
#for h in (a_hulls + b_hulls):
for h in (g_hulls):
top_index, similarity = self.compute_color_name(h,img,mask)
if self.sample_names[top_index] is not None and similarity >= self.sample_thresh[top_index]:
moments = cv2.moments(h)
# converts to x,y
location = np.array([moments['m10']/moments['m00'],moments['m01']/moments['m00']])
rospy.logdebug("Publishing Named Point")
named_img_point = NamedPoint()
named_img_point.header = Image.header
named_img_point.point.x = location[0]
named_img_point.point.y = location[1]
named_img_point.name = self.sample_names[top_index]
self.named_img_point_pub.publish(named_img_point)
named_point = self.project_xyz_point(h, top_index, Image.header)
self.named_point_pub.publish(named_point)