def __init__(self):
self.node_name = "Ground Projection"
self.active = True
self.bridge = CvBridge()
robot_name = rospy.get_param("~config_file_name", None)
if robot_name is None:
robot_name = dtu.get_current_robot_name()
self.robot_name = robot_name
self.gp = GroundProjection(self.robot_name)
self.gpg = get_ground_projection_geometry_for_robot(self.robot_name)
self.image_channel_name = "image_raw"
# Subs and Pubs
self.pub_lineseglist_ = rospy.Publisher("~lineseglist_out", SegmentList, queue_size=1)
self.sub_lineseglist_ = rospy.Subscriber("~lineseglist_in", SegmentList, self.lineseglist_cb)
# TODO prepare services
self.service_homog_ = rospy.Service("~estimate_homography", EstimateHomography, self.estimate_homography_cb)
self.service_gnd_coord_ = rospy.Service("~get_ground_coordinate", GetGroundCoord, self.get_ground_coordinate_cb)
self.service_img_coord_ = rospy.Service("~get_image_coordinate", GetImageCoord, self.get_image_coordinate_cb)
def go(self):
vehicle_name = dtu.get_current_robot_name() if dtu.on_duckiebot(
) else "default"
output = self.options.output
if output is None:
output = 'out-pipeline' # + dtu.get_md5(self.options.image)[:6]
self.info('No --output given, using %s' % output)
if self.options.image is not None:
image_filename = self.options.image
if image_filename.startswith('http'):
image_filename = dtu.get_file_from_url(image_filename)
bgr = dtu.bgr_from_jpg_fn(image_filename)
else:
print("Validating using the ROS image stream...")
import rospy
from sensor_msgs.msg import CompressedImage
topic_name = os.path.join('/', vehicle_name,
'camera_node/image/compressed')
print('Let\'s wait for an image. Say cheese!')
# Dummy to get ROS message
rospy.init_node('single_image')
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\nDidn\'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))
self.info('Picture taken: %s ' % str(bgr.shape))
gp = GroundProjection(vehicle_name)
dtu.DuckietownConstants.show_timeit_benchmarks = True
res, _stats = run_pipeline(
bgr,
gp,
line_detector_name=self.options.line_detector,
image_prep_name=self.options.image_prep,
anti_instagram_name=self.options.anti_instagram,
lane_filter_name=self.options.lane_filter)
self.info('Resizing images..')
res = dtu.resize_small_images(res)
self.info('Writing images..')
dtu.write_bgr_images_as_jpgs(res, output)
def __init__(self):
self.node_name = "Ground Projection"
self.active = True
self.bridge = CvBridge()
robot_name = rospy.get_param("~config_file_name", None)
if robot_name is None:
robot_name = dtu.get_current_robot_name()
self.robot_name = robot_name
self.gp = GroundProjection(self.robot_name)
self.gpg = get_ground_projection_geometry_for_robot(self.robot_name)
camera_info_topic = "/" + self.robot_name + "/camera_node/real_camera_info"
rospy.loginfo("camera info topic is " + camera_info_topic)
rospy.loginfo("waiting for camera info")
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
print(camera_info)
rospy.loginfo("camera info received")
if False:
self.gp.initialize_pinhole_camera_model(camera_info)
# Params
self.gp.robot_name = self.robot_name
self.gp.rectified_input_ = rospy.get_param("rectified_input",
False)
self.image_channel_name = "image_raw"
# Subs and Pubs
self.pub_lineseglist_ = rospy.Publisher("~lineseglist_out",
SegmentList,
queue_size=1)
self.sub_lineseglist_ = rospy.Subscriber("~lineseglist_in",
SegmentList,
self.lineseglist_cb)
# TODO prepare services
self.service_homog_ = rospy.Service("~estimate_homography",
EstimateHomography,
self.estimate_homography_cb)
self.service_gnd_coord_ = rospy.Service("~get_ground_coordinate",
GetGroundCoord,
self.get_ground_coordinate_cb)
self.service_img_coord_ = rospy.Service("~get_image_coordinate",
GetImageCoord,
self.get_image_coordinate_cb)
def go(self):
output = self.options.output
if output is None:
output = 'out-pipeline' # + dtu.get_md5(self.options.image)[:6]
self.info('No --output given, using %s' % output)
if self.options.image is not None:
image_filename = self.options.image
if image_filename.startswith('http'):
image_filename = dtu.get_file_from_url(image_filename)
bgr = dtu.bgr_from_jpg_fn(image_filename)
else:
self.info('Taking a picture. Say cheese!')
out = os.path.join(output, 'frame.jpg')
bgr = dtu.bgr_from_raspistill(out)
self.info('Picture taken: %s ' % str(bgr.shape))
bgr = dtu.d8_image_resize_fit(bgr, 640)
self.info('Resized to: %s ' % str(bgr.shape))
vehicle_name = dtu.get_current_robot_name()
gp = GroundProjection(vehicle_name)
if False:
_res = run_pipeline(
bgr,
gp,
line_detector_name=self.options.line_detector,
image_prep_name=self.options.image_prep,
anti_instagram_name=self.options.anti_instagram,
lane_filter_name=self.options.lane_filter)
dtu.DuckietownConstants.show_timeit_benchmarks = True
res, _stats = run_pipeline(
bgr,
gp,
line_detector_name=self.options.line_detector,
image_prep_name=self.options.image_prep,
anti_instagram_name=self.options.anti_instagram,
lane_filter_name=self.options.lane_filter)
self.info('resizing')
res = dtu.resize_small_images(res)
self.info('writing')
dtu.write_bgr_images_as_jpgs(res, output)
def go(self):
extra = self.options.get_extra()
db = get_easy_algo_db()
if len(extra) == 0:
query = dtu.get_current_robot_name()
else:
query = extra
robots = db.query('robot', query, raise_if_no_matches=True)
self.debug('robots: %s' % sorted(robots))
actual_map_name = 'DT17_scenario_four_way'
out = self.options.output
create_visuals(robots, actual_map_name, out)
for robot_name in robots:
_ok = try_simulated_localization(robot_name)
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 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\nDidn\'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))
self.info('Picture taken: %s ' % str(bgr.shape))
else:
self.info('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)
self.info('Resized to: %s ' % str(bgr.shape))
# Disable the old calibration file
disable_old_homography(robot_name)
camera_info = get_camera_info_for_robot(robot_name)
homography_dummy = get_homography_default()
gpg = GroundProjectionGeometry(camera_info, homography_dummy)
res = OrderedDict()
try:
bgr_rectified = gpg.rectify(bgr, interpolation=cv2.INTER_CUBIC)
res['bgr'] = bgr
res['bgr_rectified'] = bgr_rectified
_new_matrix, res['rectified_full_ratio_auto'] = gpg.rectify_full(
bgr, ratio=1.65)
result = estimate_homography(bgr_rectified)
dtu.check_isinstance(result, HomographyEstimationResult)
if result.bgr_detected_refined is not None:
res['bgr_detected_refined'] = result.bgr_detected_refined
if not result.success:
raise Exception(result.error)
save_homography(result.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)
finally:
dtu.write_bgr_images_as_jpgs(res, output)
def run_pipeline(image, all_details=False, ground_truth=None, actual_map=None):
"""
Image: numpy (H,W,3) == BGR
Returns a dictionary, res with the following fields:
res['input_image']
ground_truth = pose
"""
print('backend: %s' % matplotlib.get_backend())
print('fname: %s' % matplotlib.matplotlib_fname())
quick = False
dtu.check_isinstance(image, np.ndarray)
res = OrderedDict()
stats = OrderedDict()
vehicle_name = dtu.get_current_robot_name()
res['Raw input image'] = image
gp = get_ground_projection(vehicle_name)
gpg = gp.get_ground_projection_geometry
line_detector = LineDetector()
lane_filter = LaneFilterHistogram(None)
print("pass lane_filter")
ai = AntiInstagram()
pts = ProcessingTimingStats()
pts.reset()
pts.received_message()
pts.decided_to_process()
with pts.phase('calculate AI transform'):
[lower, upper] = ai.calculate_color_balance_thresholds(image)
with pts.phase('apply AI transform'):
transformed = ai.apply_color_balance(lower, upper, image)
with pts.phase('edge detection'):
# note: do not apply transform twice!
segment_list2 = image_prep.process(pts,
image,
line_detector,
transform=ai.apply_color_balance)
if all_details:
res['resized and corrected'] = image_prep.image_corrected
dtu.logger.debug('segment_list2: %s' % 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'] = gpg.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(gpg, segment_list2)
# Project to ground
with pts.phase('find_ground_coordinates'):
sg = find_ground_coordinates(gpg, segment_list2_rect)
lane_filter.initialize()
if all_details:
res['prior'] = lane_filter.get_plot_phi_d()
with pts.phase('lane filter update'):
print(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('Using default template %r for visualization' %
template_name)
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 = gpg.rectify(image)
rectified = ai.apply_color_balance(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 get_robot_name(self, name):
robot_name = rospy.get_param(name, None)
if robot_name is None:
robot_name = dtu.get_current_robot_name()
rospy.set_param(name, robot_name)
return robot_name