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)