Example #1
0
  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)