def go(self): robot_name = dtu.get_current_robot_name() output = self.options.output if output is None: output = 'out-calibrate-extrinsics' # + dtu.get_md5(self.options.image)[:6] self.info('No --output given, using %s' % output) if self.options.input is None: print("{}\nCalibrating using the ROS image stream...\n".format( "*" * 20)) import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join('/', robot_name, 'camera_node/image/compressed') print('Topic to listen to is: %s' % topic_name) print('Let\'s wait for an image. Say cheese!') # Dummy for getting a ROS message rospy.init_node('calibrate_extrinsics') img_msg = None try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) print('Image captured!') except rospy.ROSException as e: print( '\n\n\n' 'Didn\'t get any message!: %s\n ' 'MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!' '\n\n\n' % (e, )) bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg)) print('Picture taken: %s ' % str(bgr.shape)) else: print('Loading input image %s' % self.options.input) bgr = dtu.bgr_from_jpg_fn(self.options.input) if bgr.shape[1] != 640: interpolation = cv2.INTER_CUBIC bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation) print('Resized to: %s ' % str(bgr.shape)) # Disable the old calibration file print("Disableing old homography") disable_old_homography(robot_name) print("Obtaining camera info") try: camera_info = get_camera_info_for_robot(robot_name) except Exception as E: print("Error on obtaining camera info!") print(E) print("Get default homography") try: homography_dummy = get_homography_default() except Exception as E: print("Error on getting homography") print(E) print("Rectify image") try: rect = Rectify(camera_info) except Exception as E: print("Error rectifying image!") print(E) print("Calculate GPG") try: gpg = GroundProjectionGeometry(camera_info.width, camera_info.height, homography_dummy.reshape((3, 3))) except Exception as E: print("Error calculating GPG!") print(E) print("Ordered Dict") res = OrderedDict() try: bgr_rectified = rect.rectify(bgr, interpolation=cv2.INTER_CUBIC) res['bgr'] = bgr res['bgr_rectified'] = bgr_rectified _new_matrix, res['rectified_full_ratio_auto'] = rect.rectify_full( bgr, ratio=1.65) (result_gpg, status) = gpg.estimate_homography(bgr_rectified) if status is not None: raise Exception(status) save_homography(result_gpg.H, robot_name) msg = ''' To check that this worked well, place the robot on the road, and run: rosrun complete_image_pipeline single_image Look at the produced jpgs. ''' print(msg) except Exception as E: print(E) finally: dtu.write_bgr_images_as_jpgs(res, output)
def run_pipeline( image: dtu.NPImageBGR, gpg: GroundProjectionGeometry, rectifier: Rectify, line_detector_name: str, image_prep_name: str, lane_filter_name: str, anti_instagram_name: str, all_details: bool = False, ground_truth=None, actual_map=None, ) -> Tuple[Dict[str, dtu.NPImageBGR], Dict[str, float]]: """ Image: numpy (H,W,3) == BGR Returns a dictionary, res with the following fields: res['input_image'] ground_truth = pose """ logger.debug(f"backend: {matplotlib.get_backend()}") logger.debug(f"fname: {matplotlib.matplotlib_fname()}") quick: bool = False dtu.check_isinstance(image, np.ndarray) res: Dict[str, dtu.NPImageBGR] = {} stats = {} res["Raw input image"] = image algo_db = get_easy_algo_db() line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR, line_detector_name) lane_filter = algo_db.create_instance(FAMILY_LANE_FILTER, lane_filter_name) image_prep = algo_db.create_instance(ImagePrep.FAMILY, image_prep_name) ai = algo_db.create_instance(AntiInstagramInterface.FAMILY, anti_instagram_name) pts = ProcessingTimingStats() pts.reset() pts.received_message() pts.decided_to_process() if all_details: segment_list = image_prep.process(FakeContext(), image, line_detector, transform=None) res["segments_on_image_input"] = vs_fancy_display( image_prep.image_cv, segment_list) res["segments_on_image_resized"] = vs_fancy_display( image_prep.image_resized, segment_list) with pts.phase("calculate AI transform"): ai.calculateTransform(image) with pts.phase("apply AI transform"): transformed = ai.applyTransform(image) if all_details: res["image_input_transformed"] = transformed with pts.phase("edge detection"): # note: do not apply transform twice! segment_list2 = image_prep.process(pts, image, line_detector, transform=ai.applyTransform) if all_details: res["resized and corrected"] = image_prep.image_corrected logger.debug(f"segment_list2: {len(segment_list2.segments)}") if all_details: res["segments_on_image_input_transformed"] = vs_fancy_display( image_prep.image_cv, segment_list2) if all_details: res["segments_on_image_input_transformed_resized"] = vs_fancy_display( image_prep.image_resized, segment_list2) if all_details: grid = get_grid(image.shape[:2]) res["grid"] = grid res["grid_remapped"] = rectifier.rectify(grid) # res['difference between the two'] = res['image_input']*0.5 + res['image_input_rect']*0.5 with pts.phase("rectify_segments"): segment_list2_rect = rectify_segments(rectifier, gpg, segment_list2) # Project to ground with pts.phase("find_ground_coordinates"): sg = find_ground_coordinates(gpg, segment_list2_rect) lane_filter.initialize() # lane_filter.get_plot_phi_d() if all_details: res["prior"] = lane_filter.get_plot_phi_d() with pts.phase("lane filter update"): logger.info(type(lane_filter).__name__) if type(lane_filter).__name__ == "LaneFilterHistogram": # XXX merging pain _likelihood = lane_filter.update(sg.segments) else: _likelihood = lane_filter.update(sg) if not quick: with pts.phase("lane filter plot"): res["likelihood"] = lane_filter.get_plot_phi_d( ground_truth=ground_truth) easy_algo_db = get_easy_algo_db() if isinstance(lane_filter, LaneFilterMoreGeneric): template_name = lane_filter.localization_template else: template_name = "DT17_template_straight_straight" dtu.logger.debug( f"Using default template {template_name!r} for visualization") localization_template = easy_algo_db.create_instance( FAMILY_LOC_TEMPLATES, template_name) with pts.phase("lane filter get_estimate()"): est = lane_filter.get_estimate() # Coordinates in TILE frame g = localization_template.pose_from_coords(est) tinfo = TransformationsInfo() tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_AXLE, g=g) if all_details: if actual_map is not None: # sm_axle = tinfo.transform_map_to_frame(actual_map, FRAME_AXLE) res["real"] = plot_map_and_segments(actual_map, tinfo, sg.segments, dpi=120, ground_truth=ground_truth) with pts.phase("rectify"): rectified0 = rectifier.rectify(image) rectified = ai.applyTransform(rectified0) if all_details: res["image_input_rect"] = rectified res["segments rectified on image rectified"] = vs_fancy_display( rectified, segment_list2_rect) assumed = localization_template.get_map() if not quick: with pts.phase("plot_map_and_segments"): res["model assumed for localization"] = plot_map_and_segments( assumed, tinfo, sg.segments, dpi=120, ground_truth=ground_truth) assumed_axle = tinfo.transform_map_to_frame(assumed, FRAME_AXLE) with pts.phase("plot_map reprojected"): res["map reprojected on image"] = plot_map( rectified, assumed_axle, gpg, do_ground=False, do_horizon=True, do_faces=False, do_faces_outline=True, do_segments=False, ) with pts.phase("quality computation"): predicted_segment_list_rectified = predict_segments(sm=assumed_axle, gpg=gpg) quality_res, quality_stats = judge_quality( image, segment_list2_rect, predicted_segment_list_rectified) res.update(quality_res) # res['blurred']= cv2.medianBlur(image, 11) stats["estimate"] = est stats.update(quality_stats) dtu.logger.info(pts.get_stats()) return res, stats
def go(self): robot_name = dtu.get_current_robot_name() # noinspection PyUnresolvedReferences output = self.options.output # noinspection PyUnresolvedReferences the_input = self.options.input if output is None: output = "out/calibrate-extrinsics" # + dtu.get_md5(self.options.image)[:6] self.info(f"No --output given, using {output}") if the_input is None: self.info( ("{}\nCalibrating using the ROS image stream...\n".format("*" * 20))) import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join("/", robot_name, "camera_node/image/compressed") logger.info(f"Topic to listen to is: {topic_name}") self.info("Let's wait for an image. Say cheese!") # Dummy for getting a ROS message rospy.init_node("calibrate_extrinsics") try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) self.info("Image captured") except rospy.ROSException as e: print(( "\n\n\n" f"Didn't get any message: {e}\n " "MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER." )) raise img_msg = cast(CompressedImage, img_msg) bgr = dtu.bgr_from_rgb(dru.rgb_from_ros(img_msg)) self.info(f"Picture taken: {str(bgr.shape)} ") else: self.info(f"Loading input image {the_input}") bgr = dtu.bgr_from_jpg_fn(the_input) if bgr.shape[1] != 640: interpolation = cv2.INTER_CUBIC bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation) self.info(f"Resized to: {str(bgr.shape)} ") # Disable the old calibration file self.info("Disableing old homography") disable_old_homography(robot_name) self.info("Obtaining camera info") try: camera_info = get_camera_info_for_robot(robot_name) except Exception as e: msg = "Error on obtaining camera info." raise Exception(msg) from e self.info("Get default homography") try: homography_dummy = get_homography_default() except Exception as e: msg = "Error on getting homography." raise Exception(msg) from e self.info("Rectify image") try: rect = Rectify(camera_info) except Exception as e: msg = "Error rectifying image." raise Exception(msg) from e self.info("Calculate GPG") try: gpg = GroundProjectionGeometry(camera_info.width, camera_info.height, homography_dummy.reshape((3, 3))) except Exception as e: msg = "Error calculating GroundProjectionGeometry." raise Exception(msg) from e self.info("Ordered Dict") res = {} try: bgr_rectified = rect.rectify(bgr, interpolation=cv2.INTER_CUBIC) res["bgr"] = bgr res["bgr_rectified"] = bgr_rectified _new_matrix, res["rectified_full_ratio_auto"] = rect.rectify_full( bgr, ratio=1.65) (result_gpg, status) = gpg.estimate_homography(bgr_rectified) if status is not None: raise Exception(status) save_homography(result_gpg.H, robot_name) msg = """ To check that this worked well, place the robot on the road, and run: rosrun complete_image_pipeline single_image Look at the produced jpgs. """ self.info(msg) except Exception as E: self.error(E) finally: dtu.write_bgr_images_as_jpgs(res, output)