def __call__(self, rgb, d, detrects): if len(detrects) == 0: return np.array([]), np.array([]) images = np.array( [self._preproc(*self._cutout(rgb, d, *rect)) for rect in detrects]) preds = self._forward(images) # Need to split out the raw predictions into what they really are, # and apply their respecitve non-linearities. biternions, confidences = preds[:, :2], preds[:, 2] biternions = C.normalized(biternions, axis=1) print(biternions[0, :, 0, 0]) confidences = self._conf_nonlin(confidences) # The predictions still contain the spatial dimension: BCHW. # We just average these out, though more advanced things could be done, # such as weighted average weighting by certainty, etc. biternions = np.mean(biternions, axis=(-2, -1)) confidences = np.mean(confidences, axis=(-2, -1)) # Need to normalize again in order to have real biternions biternions = C.normalized(biternions) # Convert the biternions into an angle as a common API. angles = C.bit2deg(biternions) return angles, confidences
def __call__(self, rgb, d, detrects): if len(detrects) == 0: return np.array([]), np.array([]) images = [] for detrect in detrects: detrect = self.getrect(*detrect) images.append( self._preproc(C.cutout(rgb, *detrect), C.cutout(d, *detrect))) images = np.array(images) bits = [ self._net.forward(batch) for batch in self._aug.augbatch_pred(images, fast=True) ] preds = C.bit2deg(C.ensemble_biternions( bits)) - 90 # Subtract 90 to correct for "my weird" origin. return preds, np.full(len(detrects), 0.83)
def cb(self, src, rgb, d): dt = (rgb.header.stamp - self.last_stamp).to_sec() self.last_stamp = rgb.header.stamp # Ugly workaround because approximate sync sometimes jumps back in time. if dt <= 0: rospy.logwarn( "Jump back in time detected and dropped like it's hot") self.time_travels += 1 # 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] self.seen_counter += len(t_ids) header = rgb.header bridge = CvBridge() rgb = bridge.imgmsg_to_cv2(rgb, desired_encoding='rgb8') d = bridge.imgmsg_to_cv2(d) is_freeflight_cycle = (self.cycle_idx % self.stride) != 0 if not is_freeflight_cycle: # Do the extraction and prediction preds_deg, preds_confs = self.model(rgb, d, detrects) preds_bit = deg2bit(preds_deg) self.ana_counter += len(preds_bit) else: preds_bit = [None] * len( t_ids) # Note: this will take care of correct filtering below. # SMOOTHING smoothed_biternions = {} smoothed_confs = {} old_biternions = {} old_confs = {} if self.smoother_dict is not None: for t_id, biternion in zip(t_ids, preds_bit): if is_freeflight_cycle and t_id not in self.smoother_dict: continue # Don't start a new smoother when I don't have a value. if t_id in self.smoother_dict: # For visualization only, get the previous smooth value. insert_each(t_id, self.smoother_dict[t_id].get(), old_biternions, old_confs) smoother = self.smoother_dict[t_id] smoother.tick(dt, biternion, conf=None) insert_each(t_id, smoother.get(), smoothed_biternions, smoothed_confs) # publish smoothed if 0 < self.pub_smooth.get_num_connections(): self.pub_smooth.publish( HeadOrientations( header=header, angles=[bit2deg(a) for a in smoothed_angles], confidences=list(smoothed_confs), ids=list(t_ids))) # published unsmoothed if 0 < self.pub.get_num_connections(): self.pub.publish( HeadOrientations( header=header, angles=list(preds_deg) if not is_freeflight_cycle else [], confidences=list(preds_confs) if not is_freeflight_cycle else [], ids=list(t_ids))) # 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 if self.do_save_results: with open(self.results_file_smoothed.name, 'a') as f_s: for t_id in t_ids: if t_id in smoothed_biternions: f_s.write("{0:d} {1:d} {2:.2f} {3:.2f}\n".format( src.frame_idx, t_id, bit2deg(smoothed_biternions[t_id]), smoothed_confs[t_id])) if not is_freeflight_cycle: with open(self.results_file_orig.name, 'a') as f_o: for t_id, bit, conf in zip(t_ids, preds_bit, preds_confs): f_o.write("{0:d} {1:d} {2:.2f} {3:.2f}\n".format( src.frame_idx, t_id, bit2deg(bit), conf)) # Visualization # TODO: Visualize confidence, too. if 0 < self.pub_vis.get_num_connections(): rgb_vis = rgb.copy() for detrect, t_id, alpha in zip(detrects, t_ids, preds_bit): l, t, w, h = self.model.getrect(*detrect) 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) if t_id in smoothed_biternions: gamma = bit2deg(smoothed_biternions[t_id]) px_smooth = int(round(np.cos(np.deg2rad(gamma)) * w / 2)) py_smooth = -int(round(np.sin(np.deg2rad(gamma)) * h / 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) if alpha is not None: px = int(round(np.cos(np.deg2rad(bit2deg(alpha))) * w / 2)) py = -int(round( np.sin(np.deg2rad(bit2deg(alpha))) * h / 2)) cv2.line(rgb_vis, (l + w // 2, t + h // 2), (l + w // 2 + px, t + h // 2 + py), (255, 0, 0), 1) if t_id in old_biternions: beta = bit2deg(old_biternions[t_id]) px_old = int(round(np.cos(np.deg2rad(beta)) * w / 2)) py_old = -int(round(np.sin(np.deg2rad(beta)) * h / 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(bit2deg(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)
printnow('Network has {:.3f}M params in {} layers\n', df.utils.count_params(net) / 1000.0 / 1000.0, len(net.modules)) print(net[:21].forward(aug.augbatch_train(Xtr[:100])[0]).shape) costs = dotrain(net, crit, aug, Xtr, ytr, nepochs=args.epochs) print("Costs: {}".format(' ; '.join(map(str, costs)))) dostats(net, aug, Xtr, batchsize=64) # Save the network. printnow("Saving the learned network to {}\n", args.output) np.save(args.output, net.__getstate__()) # Prediction, TODO: Move to ROS node. s = np.argsort(nte) Xte, yte = Xte[s], yte[s] printnow("(TEMP) Doing predictions.\n", args.output) y_pred = dopred_bit(net, aug, Xte, batchsize=64) # Ensemble the flips! #res = maad_from_deg(bit2deg(yte), bit2deg(yte)) res = maad_from_deg(bit2deg(y_pred), bit2deg(yte)) printnow("MAE for test images = {:.2f}\n", res.mean()) #y_pred2 = ensemble_biternions([yte[::2], flipbiternions(yte[1::2])]) y_pred2 = ensemble_biternions([y_pred[::2], flipbiternions(y_pred[1::2])]) res = maad_from_deg(bit2deg(y_pred2), bit2deg(yte[::2])) printnow("MAE for flipped augmented images = {:.2f}\n", res.mean())
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, 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): # 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
net = netlib.mknet() printnow('Network has {:.3f}M params in {} layers\n', df.utils.count_params(net)/1000.0/1000.0, len(net.modules)) print(net[:21].forward(aug.augbatch_train(Xtr[:100])[0]).shape) costs = dotrain(net, crit, aug, Xtr, ytr, nepochs=args.epochs) print("Costs: {}".format(' ; '.join(map(str, costs)))) dostats(net, aug, Xtr, batchsize=64) # Save the network. printnow("Saving the learned network to {}\n", args.output) np.save(args.output, net.__getstate__()) # Prediction, TODO: Move to ROS node. s = np.argsort(nte) Xte,yte = Xte[s],yte[s] printnow("(TEMP) Doing predictions.\n", args.output) y_pred = dopred_bit(net, aug, Xte, batchsize=64) # Ensemble the flips! #res = maad_from_deg(bit2deg(yte), bit2deg(yte)) res = maad_from_deg(bit2deg(y_pred), bit2deg(yte)) printnow("MAE for test images = {:.2f}\n", res.mean()) #y_pred2 = ensemble_biternions([yte[::2], flipbiternions(yte[1::2])]) y_pred2 = ensemble_biternions([y_pred[::2], flipbiternions(y_pred[1::2])]) res = maad_from_deg(bit2deg(y_pred2), bit2deg(yte[::2])) printnow("MAE for flipped augmented images = {:.2f}\n", res.mean())