def _preproc(self, det_rgb, det_d): im = C.subtractbg(det_rgb, det_d, 1.0, 0.5) im = cv2.resize(im, (80, 200)) im = np.rollaxis(im, 2, 0) return im.astype(df.floatX) / 255
def cb(self, src, rgb, d, caminfo, *more): # Ugly workaround because approximate sync sometimes jumps back in time. if rgb.header.stamp <= self.last_stamp: rospy.logwarn( "Jump back in time detected and dropped like it's hot") return self.last_stamp = rgb.header.stamp detrects = get_rects(src) # Early-exit to minimize CPU usage if possible. #if len(detrects) == 0: # return # If nobody's listening, why should we be computing? listeners = sum(p.get_num_connections() for p in (self.pub, self.pub_vis, self.pub_pa)) if self.pub_tracks is not None: listeners += self.pub_tracks.get_num_connections() if listeners == 0: return header = rgb.header bridge = CvBridge() rgb = bridge.imgmsg_to_cv2( rgb)[:, :, ::-1] # Need to do BGR-RGB conversion manually. d = bridge.imgmsg_to_cv2(d) imgs = [] for detrect in detrects: detrect = self.getrect(*detrect) det_rgb = cutout(rgb, *detrect) det_d = cutout(d, *detrect) # Preprocess and stick into the minibatch. im = subtractbg(det_rgb, det_d, 1.0, 0.5) im = self.preproc(im) imgs.append(im) sys.stderr.write("\r{}".format(self.counter)) sys.stderr.flush() self.counter += 1 # TODO: We could further optimize by putting all augmentations in a # single batch and doing only one forward pass. Should be easy. if len(detrects): bits = [ self.net.forward(batch) for batch in self.aug.augbatch_pred(np.array(imgs), fast=True) ] preds = bit2deg(ensemble_biternions( bits)) - 90 # Subtract 90 to correct for "my weird" origin. # print(preds) else: preds = [] if 0 < self.pub.get_num_connections(): self.pub.publish( HeadOrientations(header=header, angles=list(preds), confidences=[0.83] * len(imgs))) # Visualization if 0 < self.pub_vis.get_num_connections(): rgb_vis = rgb[:, :, ::-1].copy() for detrect, alpha in zip(detrects, preds): l, t, w, h = self.getrect(*detrect) px = int(round(np.cos(np.deg2rad(alpha)) * w / 2)) py = -int(round(np.sin(np.deg2rad(alpha)) * h / 2)) cv2.rectangle( rgb_vis, (detrect[0], detrect[1]), (detrect[0] + detrect[2], detrect[1] + detrect[3]), (0, 255, 255), 1) cv2.rectangle(rgb_vis, (l, t), (l + w, t + h), (0, 255, 0), 2) cv2.line(rgb_vis, (l + w // 2, t + h // 2), (l + w // 2 + px, t + h // 2 + py), (0, 255, 0), 2) # cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2) vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8') vismsg.header = header # TODO: Seems not to work! self.pub_vis.publish(vismsg) if 0 < self.pub_pa.get_num_connections(): fx, cx = caminfo.K[0], caminfo.K[2] fy, cy = caminfo.K[4], caminfo.K[5] poseArray = PoseArray(header=header) for (dx, dy, dw, dh, dd), alpha in zip(get_rects(src, with_depth=True), preds): dx, dy, dw, dh = self.getrect(dx, dy, dw, dh) # PoseArray message for boundingbox centres poseArray.poses.append( Pose( position=Point(x=dd * ((dx + dw / 2.0 - cx) / fx), y=dd * ((dy + dh / 2.0 - cy) / fy), z=dd), # TODO: Use global UP vector (0,0,1) and transform into frame used by this message. orientation=Quaternion(*quaternion_about_axis( np.deg2rad(alpha), [0, -1, 0])))) self.pub_pa.publish(poseArray) if len( more ) == 1 and self.pub_tracks is not None and 0 < self.pub_tracks.get_num_connections( ): t3d = more[0] try: self.listener.waitForTransform(header.frame_id, t3d.header.frame_id, rospy.Time(), rospy.Duration(1)) for track, alpha in zip(t3d.tracks, preds): track.pose.pose.orientation = self.listener.transformQuaternion( t3d.header.frame_id, QuaternionStamped( header=header, # TODO: Same as above! quaternion=Quaternion(*quaternion_about_axis( np.deg2rad(alpha), [0, -1, 0])))).quaternion self.pub_tracks.publish(t3d) except TFException: pass
def cb(self, src, rgb, d, caminfo, *more): # Ugly workaround because approximate sync sometimes jumps back in time. if rgb.header.stamp <= self.last_stamp: rospy.logwarn("Jump back in time detected and dropped like it's hot") return self.last_stamp = rgb.header.stamp detrects = get_rects(src) # Early-exit to minimize CPU usage if possible. #if len(detrects) == 0: # return # If nobody's listening, why should we be computing? if 0 == sum(p.get_num_connections() for p in (self.pub, self.pub_vis, self.pub_pa, self.pub_tracks)): return header = rgb.header bridge = CvBridge() rgb = bridge.imgmsg_to_cv2(rgb)[:,:,::-1] # Need to do BGR-RGB conversion manually. d = bridge.imgmsg_to_cv2(d) imgs = [] for detrect in detrects: detrect = self.getrect(*detrect) det_rgb = cutout(rgb, *detrect) det_d = cutout(d, *detrect) # Preprocess and stick into the minibatch. im = subtractbg(det_rgb, det_d, 1.0, 0.5) im = self.preproc(im) imgs.append(im) sys.stderr.write("\r{}".format(self.counter)) ; sys.stderr.flush() self.counter += 1 # TODO: We could further optimize by putting all augmentations in a # single batch and doing only one forward pass. Should be easy. if len(detrects): bits = [self.net.forward(batch) for batch in self.aug.augbatch_pred(np.array(imgs), fast=True)] preds = bit2deg(ensemble_biternions(bits)) - 90 # Subtract 90 to correct for "my weird" origin. # print(preds) else: preds = [] if 0 < self.pub.get_num_connections(): self.pub.publish(HeadOrientations( header=header, angles=list(preds), confidences=[0.83] * len(imgs) )) # Visualization if 0 < self.pub_vis.get_num_connections(): rgb_vis = rgb[:,:,::-1].copy() for detrect, alpha in zip(detrects, preds): l, t, w, h = self.getrect(*detrect) px = int(round(np.cos(np.deg2rad(alpha))*w/2)) py = -int(round(np.sin(np.deg2rad(alpha))*h/2)) cv2.rectangle(rgb_vis, (detrect[0], detrect[1]), (detrect[0]+detrect[2],detrect[1]+detrect[3]), (0,255,255), 1) cv2.rectangle(rgb_vis, (l,t), (l+w,t+h), (0,255,0), 2) cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px,t+h//2+py), (0,255,0), 2) # cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2) vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8') vismsg.header = header # TODO: Seems not to work! self.pub_vis.publish(vismsg) if 0 < self.pub_pa.get_num_connections(): fx, cx = caminfo.K[0], caminfo.K[2] fy, cy = caminfo.K[4], caminfo.K[5] poseArray = PoseArray(header=header) for (dx, dy, dw, dh, dd), alpha in zip(get_rects(src, with_depth=True), preds): dx, dy, dw, dh = self.getrect(dx, dy, dw, dh) # PoseArray message for boundingbox centres poseArray.poses.append(Pose( position=Point( x=dd*((dx+dw/2.0-cx)/fx), y=dd*((dy+dh/2.0-cy)/fy), z=dd ), # TODO: Use global UP vector (0,0,1) and transform into frame used by this message. orientation=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0])) )) self.pub_pa.publish(poseArray) if len(more) == 1 and 0 < self.pub_tracks.get_num_connections(): t3d = more[0] try: self.listener.waitForTransform(header.frame_id, t3d.header.frame_id, rospy.Time(), rospy.Duration(1)) for track, alpha in zip(t3d.tracks, preds): track.pose.pose.orientation = self.listener.transformQuaternion(t3d.header.frame_id, QuaternionStamped( header=header, # TODO: Same as above! quaternion=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0])) )).quaternion self.pub_tracks.publish(t3d) except TFException: pass
def cb(self, src, rgb, d): # Ugly workaround because approximate sync sometimes jumps back in time. if rgb.header.stamp <= self.last_stamp: rospy.logwarn("Jump back in time detected and dropped like it's hot") self.time_travels += 1 # return self.last_stamp = rgb.header.stamp # Early-exit to minimize CPU usage if possible. #if len(detrects) == 0: # return # If nobody's listening, why should we be computing? #if 0 == sum(p.get_num_connections() for p in (self.pub, self.pub_vis, self.pub_pa, self.pub_tracks)): # print("IS THERE ANYBODY OUT THERE???") # return # TIMING START self.cycle_idx += 1 cycle_start_time = time.time() detrects = get_rects(src) t_ids = [(p2d.track_id) for p2d in src.boxes] header = rgb.header bridge = CvBridge() rgb = bridge.imgmsg_to_cv2(rgb)[:,:,::-1] # Need to do BGR-RGB conversion manually. d = bridge.imgmsg_to_cv2(d) imgs = [] # FREE-FLIGHT (super-ugly, do refactor... really, do it!!) if ((self.cycle_idx%self.free_flight_step) != 0): smoothed_angles = [] smoothed_confs = [] old_angles = [] old_confs = [] for t_id in t_ids: if t_id not in self.smoother_dict: continue old_ang, old_conf = self.smoother_dict[t_id].getCurrentValueAndConfidence() old_ang = bit2deg(np.array([old_ang])) old_angles.append(old_ang) old_confs.append(old_conf) # 2) PREDICT ALL EXISTING self.smoother_dict[t_id].predict() # 3) UPDATE ALL EXISTING (/wo meas) self.smoother_dict[t_id].update() # append result here to keep id_idx smoothed_ang, smoothed_conf = self.smoother_dict[t_id].getCurrentValueAndConfidence() smoothed_ang = bit2deg(np.array([smoothed_ang])) smoothed_angles.append(smoothed_ang) smoothed_confs.append(smoothed_conf) # publish smoothed if 0 < self.pub_smooth.get_num_connections(): self.pub_smooth.publish(HeadOrientations( header=header, angles=list(smoothed_angles), confidences=list(smoothed_confs), ids = list(t_ids) )) # Visualization if 0 < self.pub_vis.get_num_connections(): rgb_vis = rgb[:,:,::-1].copy() for detrect, beta, gamma in zip(detrects, old_angles, smoothed_angles): l, t, w, h = self.getrect(*detrect) px_old = int(round(np.cos(np.deg2rad(beta))*w/2)) py_old = -int(round(np.sin(np.deg2rad(beta))*h/2)) px_smooth = int(round(np.cos(np.deg2rad(gamma))*w/2)) py_smooth = -int(round(np.sin(np.deg2rad(gamma))*h/2)) cv2.rectangle(rgb_vis, (detrect[0], detrect[1]), (detrect[0]+detrect[2],detrect[1]+detrect[3]), (0,255,255), 1) cv2.rectangle(rgb_vis, (l,t), (l+w,t+h), (0,255,0), 2) cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px_smooth,t+h//2+py_smooth), (0,255,0), 2) cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px_old,t+h//2+py_old), (0,0,255), 1) # cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2) vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8') vismsg.header = header # TODO: Seems not to work! self.pub_vis.publish(vismsg) # TIMING END cycle_end_time = time.time() cycle_duration = cycle_end_time - cycle_start_time self.hz_curr = 1./(cycle_duration) self.hz_sum = self.hz_sum + self.hz_curr self.hz_mean = self.hz_sum/self.cycle_idx return # --FREE-FLIGHT END-- (really: refactor!) for detrect in detrects: detrect = self.getrect(*detrect) det_rgb = cutout(rgb, *detrect) det_d = cutout(d, *detrect) # Preprocess and stick into the minibatch. im = subtractbg(det_rgb, det_d, 1.0, 0.5) im = self.preproc(im) imgs.append(im) #sys.stderr.write("\r{}".format(self.counter)) ; sys.stderr.flush() self.counter += 1 # TODO: We could further optimize by putting all augmentations in a # single batch and doing only one forward pass. Should be easy. if len(detrects): bits = [self.net.forward(batch) for batch in self.aug.augbatch_pred(np.array(imgs), fast=True)] preds = bit2deg(ensemble_biternions(bits)) - 90 # Subtract 90 to correct for "my weird" origin. # print(preds) else: preds = [] # SMOOTHING fake_confs=[0.5] * len(preds) #TODO: de-fake new_angles = dict(zip(t_ids,list(zip(preds,fake_confs)))) smoothed_angles = [] smoothed_confs = [] old_angles = [] old_confs = [] for t_id in t_ids: # 1) INIT ALL NEW if t_id not in self.smoother_dict: # new id, start new smoothing init_dt = 1. #TODO new_smoother = Smoother(deg2bit(new_angles[t_id][0]), new_angles[t_id][1], init_dt=init_dt, filter_method=self.filter_method) smoothed_ang, smoothed_conf = new_angles[t_id] smoothed_angles.append(smoothed_ang) smoothed_confs.append(smoothed_conf) self.smoother_dict[t_id] = new_smoother continue old_ang, old_conf = self.smoother_dict[t_id].getCurrentValueAndConfidence() old_ang = bit2deg(np.array([old_ang])) old_angles.append(old_ang) old_confs.append(old_conf) # 2) PREDICT ALL EXISTING self.smoother_dict[t_id].predict() # 3) UPDATE ALL EXISTING self.smoother_dict[t_id].update(deg2bit(new_angles[t_id][0]), new_angles[t_id][1]) # 4) DELETE ALL OLD # TODO: deletion logic, or just keep all in case they return and predict up to then # append result here to keep id_idx smoothed_ang, smoothed_conf = self.smoother_dict[t_id].getCurrentValueAndConfidence() smoothed_ang = bit2deg(np.array([smoothed_ang])) smoothed_angles.append(smoothed_ang) smoothed_confs.append(smoothed_conf) # published unsmoothed if 0 < self.pub.get_num_connections(): self.pub.publish(HeadOrientations( header=header, angles=list(preds), confidences=[0.83] * len(imgs), ids=list(t_ids) )) # publish smoothed if 0 < self.pub_smooth.get_num_connections(): self.pub_smooth.publish(HeadOrientations( header=header, angles=list(smoothed_angles), confidences=list(smoothed_confs), ids = list(t_ids) )) if self.do_save_results: f_s = open(self.results_file_smoothed.name,'a') f_o = open(self.results_file_orig.name,'a') for t_id in t_ids: result_ang, result_conf = self.smoother_dict[t_id].getCurrentValueAndConfidence() result_ang = bit2deg(np.array([result_ang])) f_s.write("{0:d} {1:d} {2:.2f} {3:.2f}\n".format(src.frame_idx, t_id, result_ang[0], result_conf)) f_o.write("{0:d} {1:d} {2:.2f} {3:.2f}\n".format(src.frame_idx, t_id, new_angles[t_id][0], new_angles[t_id][1])) f_s.close() f_o.close() # Visualization if 0 < self.pub_vis.get_num_connections(): rgb_vis = rgb[:,:,::-1].copy() for detrect, alpha, beta, gamma in zip(detrects, preds, old_angles, smoothed_angles): l, t, w, h = self.getrect(*detrect) px = int(round(np.cos(np.deg2rad(alpha))*w/2)) py = -int(round(np.sin(np.deg2rad(alpha))*h/2)) px_old = int(round(np.cos(np.deg2rad(beta))*w/2)) py_old = -int(round(np.sin(np.deg2rad(beta))*h/2)) px_smooth = int(round(np.cos(np.deg2rad(gamma))*w/2)) py_smooth = -int(round(np.sin(np.deg2rad(gamma))*h/2)) cv2.rectangle(rgb_vis, (detrect[0], detrect[1]), (detrect[0]+detrect[2],detrect[1]+detrect[3]), (0,255,255), 1) cv2.rectangle(rgb_vis, (l,t), (l+w,t+h), (0,255,0), 2) cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px_smooth,t+h//2+py_smooth), (0,255,0), 2) cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px,t+h//2+py), (255,0,0), 1) cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px_old,t+h//2+py_old), (0,0,255), 1) # cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2) vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8') vismsg.header = header # TODO: Seems not to work! self.pub_vis.publish(vismsg) #if 0 < self.pub_pa.get_num_connections(): # fx, cx = caminfo.K[0], caminfo.K[2] # fy, cy = caminfo.K[4], caminfo.K[5] # poseArray = PoseArray(header=header) # for (dx, dy, dw, dh, dd), alpha in zip(get_rects(src, with_depth=True), preds): # dx, dy, dw, dh = self.getrect(dx, dy, dw, dh) # # PoseArray message for boundingbox centres # poseArray.poses.append(Pose( # position=Point( # x=dd*((dx+dw/2.0-cx)/fx), # y=dd*((dy+dh/2.0-cy)/fy), # z=dd # ), # # TODO: Use global UP vector (0,0,1) and transform into frame used by this message. # orientation=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0])) # )) # self.pub_pa.publish(poseArray) #if len(more) == 1 and 0 < self.pub_tracks.get_num_connections(): # t3d = more[0] # try: # self.listener.waitForTransform(header.frame_id, t3d.header.frame_id, rospy.Time.now(), rospy.Duration(1.0)) # for track, alpha in zip(t3d.tracks, preds): # track.pose.pose.orientation = self.listener.transformQuaternion(t3d.header.frame_id, QuaternionStamped( # header=header, # # TODO: Same as above! # quaternion=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0])) # )).quaternion # self.pub_tracks.publish(t3d) # except TFException: # print("TFException") # pass # TIMING END cycle_end_time = time.time() cycle_duration = cycle_end_time - cycle_start_time self.hz_curr = 1./(cycle_duration) self.hz_sum = self.hz_sum + self.hz_curr self.hz_mean = self.hz_sum/self.cycle_idx