def cb_camera_info(self, msg: CameraInfo):
"""
Initializes a :py:class:`image_processing.GroundProjectionGeometry` object and a
:py:class:`image_processing.Rectify` object for image rectification
Args:
msg (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera.
"""
if not self.camera_info_received:
self.rectifier = Rectify(msg)
self.ground_projector = GroundProjectionGeometry(
im_width=msg.width, im_height=msg.height, homography=np.array(self.homography).reshape((3, 3))
)
self.camera_info_received = True
def rectify_segment(rectify: Rectify, gpg: GroundProjectionGeometry,
s1: Segment) -> Segment:
pixels_normalized = []
for i in (0, 1):
# normalized coordinates
nc = s1.pixels_normalized[i]
# get pixel coordinates
pixels = gpg.vector2pixel(nc)
# rectify
pr = rectify.rectify_point(pixels)
# recompute normalized coordinates
# t = Pixel(pr[0], pr[1])
v = gpg.pixel2vector(ImageSpaceResdepPoint(pr))
pixels_normalized.append(v)
s2 = Segment(color=s1.color, pixels_normalized=pixels_normalized)
return s2
def simulate_image(sm_orig: SegmentsMap, pose: SE2value,
gpg: GroundProjectionGeometry, rectifier: Rectify,
blur_sigma: float) -> SimulationData:
H, W = gpg.get_shape()
# H_pad = int(0.3*H)
# W_pad = int(0.3*W)
# H_padded = H + H_pad
# W_padded = W + W_pad
blank = np.zeros(dtype="uint8", shape=(H, W, 3))
blank.fill(128)
tinfo = TransformationsInfo()
frames = list(set(_.id_frame for _ in list(sm_orig.points.values())))
id_frame = frames[0]
dtu.logger.debug(f"frames: {frames} choose {id_frame}")
tinfo.add_transformation(frame1=id_frame, frame2=FRAME_AXLE, g=pose)
sm_axle = tinfo.transform_map_to_frame(sm_orig, FRAME_AXLE)
rectified_synthetic = plot_map(blank, sm_axle, gpg, do_segments=False)
rectified_segments = plot_map(blank,
sm_axle,
gpg,
do_segments=True,
do_ground=True,
do_faces=False,
do_horizon=False)
distorted_synthetic = rectifier.distort(rectified_synthetic)
distorted_synthetic = add_noise(distorted_synthetic)
# distorted_synthetic = cv2.medianBlur(distorted_synthetic, 3)
distorted_synthetic = cv2.GaussianBlur(distorted_synthetic, (0, 0),
blur_sigma)
return SimulationData(
distorted_synthetic_bgr=distorted_synthetic,
rectified_synthetic_bgr=rectified_synthetic,
rectified_segments_bgr=rectified_segments,
)
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()
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)
class ObjectDetectionNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ObjectDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Construct publishers
self.pub_obj_dets = rospy.Publisher("~duckie_detected",
BoolStamped,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscribers
self.sub_image = rospy.Subscriber("~image/compressed",
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_thresholds = rospy.Subscriber("~thresholds",
AntiInstagramThresholds,
self.thresholds_cb,
queue_size=1)
self.sub_camera_info = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/camera_node/camera_info",
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.sub_lane_reading = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/lane_filter_node/lane_pose",
LanePose,
self.cbLanePoses,
queue_size=1)
self.initialized = False
self.ai_thresholds_received = False
self.anti_instagram_thresholds = dict()
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.ground_projector = None
self.rectifier = None
self.homography = self.load_extrinsics()
self.camera_info_received = False
self.log(str(self.homography))
self.lane_width = rospy.get_param('~lanewidth', None)
self.safe_distance = rospy.get_param('~safe_distance', None)
model_file = rospy.get_param('~model_file', '.')
rospack = rospkg.RosPack()
model_file_absolute = rospack.get_path('object_detection') + model_file
self.model_wrapper = Wrapper(model_file_absolute)
self.initialized = True
self.image_count = 0
self.obstacle_left_lane = False
self.obstacle_right_lane = False
self.log("Initialized!")
def thresholds_cb(self, thresh_msg):
self.anti_instagram_thresholds["lower"] = thresh_msg.low
self.anti_instagram_thresholds["higher"] = thresh_msg.high
self.ai_thresholds_received = True
def cb_camera_info(self, msg):
"""
Initializes a :py:class:`image_processing.GroundProjectionGeometry` object and a
:py:class:`image_processing.Rectify` object for image rectification
Args:
msg (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera.
"""
if not self.camera_info_received:
self.rectifier = Rectify(msg)
self.ground_projector = GroundProjectionGeometry(
im_width=msg.width,
im_height=msg.height,
homography=np.array(self.homography).reshape((3, 3)))
self.im_width = msg.width
self.im_height = msg.height
self.camera_info_received = True
def cbLanePoses(self, input_pose_msg):
"""Callback receiving pose messages
Computes v and omega using PPController
Args:
input_pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
"""
self.pose_msg = input_pose_msg
def image_cb(self, image_msg):
if not self.initialized:
return
# TODO to get better hz, you might want to only call your wrapper's predict function only once ever 4-5 images?
# This way, you're not calling the model again for two practically identical images. Experiment to find a good number of skipped
# images.
# Decode from compressed image with OpenCV
try:
image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
# Perform color correction
if self.ai_thresholds_received:
image = self.ai.apply_color_balance(
self.anti_instagram_thresholds["lower"],
self.anti_instagram_thresholds["higher"], image)
image = cv2.resize(image, (224, 224))
if self.image_count != 0:
self.image_count = np.mod(self.image_count + 1, 3)
else:
bboxes, classes, scores = self.model_wrapper.predict(image)
im_boxed = self.plotWithBoundingBoxes(image, bboxes[0], classes[0],
scores[0])
cv2.imshow('detected objects', im_boxed)
cv2.waitKey(1)
self.det2bool(
bboxes[0], classes[0]
) # [0] because our batch size given to the wrapper is 1
msg = BoolStamped()
msg.header = image_msg.header
if self.obstacle_right_lane:
msg.data = True
if self.obstacle_left_lane:
pass
else:
## OVERTAKING
# msg.data = overtake
pass
self.pub_obj_dets.publish(msg)
def det2bool(self, bboxes, classes):
# TODO remove these debugging prints
# print(bboxes)
# print(classes)
# This is a dummy solution, remove this next line
# return len(bboxes) > 1
# TODO filter the predictions: the environment here is a bit different versus the data collection environment, and your model might output a bit
# of noise. For example, you might see a bunch of predictions with x1=223.4 and x2=224, which makes
# no sense. You should remove these.
# TODO also filter detections which are outside of the road, or too far away from the bot. Only return True when there's a pedestrian (aka a duckie)
# in front of the bot, which you know the bot will have to avoid. A good heuristic would be "if centroid of bounding box is in the center of the image,
# assume duckie is in the road" and "if bouding box's area is more than X pixels, assume duckie is close to us"
self.obstacle_right_lane = False
self.obstacle_left_lane = False
obj_det_list = []
for i in range(len(bboxes)):
x1, y1, x2, y2 = bboxes[i]
label = classes[i]
if label == 1:
if (x2 - x1 >= 2) and (y2 - y1 >= 2):
low_center = Point((x1 + x2) / 2, y2)
rect_pixel = self.rectifier.rectify_point(low_center)
ground_point = self.ground_projector.pixel2ground(
rect_pixel)
duckie_lane_pose = np.cos(self.pose_msg.phi) * (
ground_point.y + self.pose_msg.d) + np.sin(
self.pose_msg.phi) * ground_point.x
dist = np.sqrt(ground_point.x**2 + ground_point.y**2)
if np.abs(duckie_lane_pose
) <= self.lane_width / 2: #in our lane
if dist <= self.safe_distance:
self.obstacle_right_lane = True
elif np.abs(duckie_lane_pose
) > self.lane_width / 2: #in left lane
if dist <= self.safe_distance * 1.5:
self.obstacle_left_lane = True
# TODO if label isn't a duckie, skip
# TODO if detection is a pedestrian in front of us:
# return True
def plotWithBoundingBoxes(self, seg_im, boxes, labels, scores):
for i in range(len(labels)):
cv2.rectangle(seg_im, (boxes[i][0], boxes[i][1]),
(boxes[i][2], boxes[i][3]), (255, 255, 255), 1)
cv2.rectangle(seg_im, (boxes[i][0], boxes[i][1]),
(boxes[i][0] + 20, boxes[i][1] - 6), (255, 255, 255),
cv.FILLED)
cv2.putText(seg_im, f"{labels[i]} : {scores[i]}",
(boxes[i][0], boxes[i][1]), cv.FONT_HERSHEY_COMPLEX,
0.5, (0, 0, 0), 1)
return seg_im
def load_extrinsics(self):
"""
Loads the homography matrix from the extrinsic calibration file.
Returns:
:obj:`numpy array`: the loaded homography matrix
"""
# load intrinsic calibration
cali_file_folder = '/data/config/calibrations/camera_extrinsic/'
cali_file = cali_file_folder + rospy.get_namespace().strip(
"/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(cali_file):
self.log(
"Can't find calibration file: %s.\n Using default calibration instead."
% cali_file, 'warn')
cali_file = (cali_file_folder + "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(cali_file):
msg = 'Found no calibration file ... aborting'
self.log(msg, 'err')
rospy.signal_shutdown(msg)
try:
with open(cali_file, 'r') as stream:
calib_data = yaml.load(stream)
except yaml.YAMLError:
msg = 'Error in parsing calibration file %s ... aborting' % cali_file
self.log(msg, 'err')
rospy.signal_shutdown(msg)
return calib_data['homography']
class ATPoseNode(DTROS):
"""
Computes an estimate of the Duckiebot pose using the wheel encoders.
Args:
node_name (:obj:`str`): a unique, descriptive name for the ROS node
Configuration:
Publisher:
~/rectified_image (:obj:`Image`): The rectified image
~at_localization (:obj:`PoseStamped`): The computed position broadcasted in TFs
Subscribers:
~/camera_node/image/compressed (:obj:`CompressedImage`): Observation from robot
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATPoseNode, self).__init__(node_name=node_name,
node_type=NodeType.LOCALIZATION)
# get the name of the robot
self.veh = rospy.get_namespace().strip("/")
self.bridge = CvBridge()
self.camera_info = None
self.Rectify = None
self.at_detector = Detector(families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Construct publishers
self.at_estimated_pose = rospy.Publisher(
f'/{self.veh}/at_localization',
Odometry,
queue_size=1,
dt_topic_type=TopicType.LOCALIZATION)
# Camera subscribers:
camera_topic = f'/{self.veh}/camera_node/image/compressed'
self.camera_feed_sub = rospy.Subscriber(camera_topic,
CompressedImage,
self.detectATPose,
queue_size=1,
buff_size=2**24)
camera_info_topic = f'/{self.veh}/camera_node/camera_info'
self.camera_info_sub = rospy.Subscriber(camera_info_topic,
CameraInfo,
self.getCameraInfo,
queue_size=1)
self.image_pub = rospy.Publisher(f'/{self.veh}/rectified_image',
Image,
queue_size=10)
self.tag_pub = rospy.Publisher(f'/{self.veh}/detected_tags',
Int32MultiArray,
queue_size=5)
self.log("Initialized!")
def getCameraInfo(self, cam_msg):
if (self.camera_info == None):
self.camera_info = cam_msg
self.Rectify = Rectify(self.camera_info)
return
def _broadcast_tf(self,
parent_frame_id: str,
child_frame_id: str,
stamp: Optional[float] = None,
translations: Optional[Tuple[float, float,
float]] = None,
euler_angles: Optional[Tuple[float, float,
float]] = None,
quarternion: Optional[List[float]] = None):
br = tf2_ros.StaticTransformBroadcaster()
ts = TransformStamped()
ts.header.frame_id = parent_frame_id
ts.child_frame_id = child_frame_id
if stamp is None:
stamp = rospy.Time.now()
ts.header.stamp = stamp
if translations is None:
translations = 0, 0, 0
ts.transform.translation.x = translations[0]
ts.transform.translation.y = translations[1]
ts.transform.translation.z = translations[2]
if euler_angles is not None:
q = tf_conversions.transformations.quaternion_from_euler(
*euler_angles)
elif quarternion is not None:
q = quarternion
else:
q = 0, 0, 0, 1
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
br.sendTransform(ts)
def _broadcast_detected_tag(self, image_msg, tag_id, tag):
# inverse the rotation and tranformation comming out of the AT detector.
# The AP detector's output is the camera frame relative to the TAG
# According to: https://duckietown.slack.com/archives/C6ZHPV212/p1619876209086300?thread_ts=1619751267.084600&cid=C6ZHPV212
# While the transformation below needs TAG relative to camera
# Therefore we need to reverse it first.
pose_R = np.identity(4)
pose_R[:3, :3] = tag.pose_R
inv_pose_R = np.transpose(pose_R)
pose_t = np.ones((4, 1))
pose_t[:3, :] = tag.pose_t
inv_pose_t = -np.matmul(inv_pose_R, pose_t)
out_q = quaternion_from_matrix(inv_pose_R)
out_t = inv_pose_t[:3, :]
tag_frame_id = 'april_tag_{}'.format(tag_id)
tag_cam_frame_id = 'april_tag_cam_{}'.format(tag_id)
camera_rgb_link_frame_id = 'at_{}_camera_rgb_link'.format(tag_id)
camera_link_frame_id = 'at_{}_camera_link'.format(tag_id)
base_link_frame_id = 'at_{}_base_link'.format(tag_id)
# ATag to ATag cam (this is because AprilTAG pose is different from physical)
self._broadcast_tf(parent_frame_id=tag_frame_id,
child_frame_id=tag_cam_frame_id,
euler_angles=(-90 * math.pi / 180, 0,
-90 * math.pi / 180))
# ATag cam to camera_rgb_link (again this is internal cam frame)
self._broadcast_tf(parent_frame_id=tag_cam_frame_id,
child_frame_id=camera_rgb_link_frame_id,
stamp=image_msg.header.stamp,
translations=out_t,
quarternion=out_q)
# camera_rgb_link to camera_link (internal cam frame to physical cam frame)
self._broadcast_tf(parent_frame_id=camera_rgb_link_frame_id,
child_frame_id=camera_link_frame_id,
stamp=image_msg.header.stamp,
euler_angles=(0, -90 * math.pi / 180,
90 * math.pi / 180))
# camera_link to base_link of robot
self._broadcast_tf(parent_frame_id=camera_link_frame_id,
child_frame_id=base_link_frame_id,
stamp=image_msg.header.stamp,
translations=(-0.066, 0, -0.106),
euler_angles=(0, -15 * math.pi / 180, 0))
def detectATPose(self, image_msg):
"""
Image callback.
Args:
msg_encoder (:obj:`Compressed`) encoder ROS message.
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
new_cam, rectified_img = self.Rectify.rectify_full(cv_image)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rectified_img, "bgr8"))
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
gray_img = cv2.cvtColor(rectified_img, cv2.COLOR_RGB2GRAY)
camera_params = (new_cam[0, 0], new_cam[1, 1], new_cam[0,
2], new_cam[1,
2])
detected_tags = self.at_detector.detect(gray_img,
estimate_tag_pose=True,
camera_params=camera_params,
tag_size=0.065)
detected_tag_ids = list(map(lambda x: fetch_tag_id(x), detected_tags))
array_for_pub = Int32MultiArray(data=detected_tag_ids)
for tag_id, tag in zip(detected_tag_ids, detected_tags):
print('detected {}: ({}, {})'.format(
tag_id, image_msg.header.stamp.to_time(),
rospy.Time.now().to_time()))
self._broadcast_detected_tag(image_msg, tag_id, tag)
self.tag_pub.publish(array_for_pub)
def getCameraInfo(self, cam_msg):
if (self.camera_info == None):
self.camera_info = cam_msg
self.Rectify = Rectify(self.camera_info)
return
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)
class GroundProjectionNode(DTROS):
"""
This node projects the line segments detected in the image to the ground plane and in the robot's
reference frame.
In this way it enables lane localization in the 2D ground plane. This projection is performed using the
homography
matrix obtained from the extrinsic calibration procedure.
Args:
node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
Subscribers:
~camera_info (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera. Needed for
rectifying the segments.
~lineseglist_in (:obj:`duckietown_msgs.msg.SegmentList`): Line segments in pixel space from
unrectified images
Publishers:
~lineseglist_out (:obj:`duckietown_msgs.msg.SegmentList`): Line segments in the ground plane
relative to the robot origin
~debug/ground_projection_image/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug image
that shows the robot relative to the projected segments. Useful to check if the extrinsic
calibration is accurate.
"""
bridge: CvBridge
ground_projector: Optional[GroundProjectionGeometry]
rectifier: Optional[Rectify]
def __init__(self, node_name: str):
# Initialize the DTROS parent class
super(GroundProjectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
self.bridge = CvBridge()
self.ground_projector = None
self.rectifier = None
self.homography = self.load_extrinsics()
self.first_processing_done = False
self.camera_info_received = False
# subscribers
self.sub_camera_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.sub_lineseglist_ = rospy.Subscriber("~lineseglist_in",
SegmentList,
self.lineseglist_cb,
queue_size=1)
# publishers
self.pub_lineseglist = rospy.Publisher(
"~lineseglist_out",
SegmentList,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self.pub_debug_img = rospy.Publisher(
"~debug/ground_projection_image/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.DEBUG,
)
self.bridge = CvBridge()
self.debug_img_bg = None
# Seems to be never used:
# 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 cb_camera_info(self, msg: CameraInfo):
"""
Initializes a :py:class:`image_processing.GroundProjectionGeometry` object and a
:py:class:`image_processing.Rectify` object for image rectification
Args:
msg (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera.
"""
if not self.camera_info_received:
self.rectifier = Rectify(msg)
self.ground_projector = GroundProjectionGeometry(
im_width=msg.width,
im_height=msg.height,
homography=np.array(self.homography).reshape((3, 3)))
self.camera_info_received = True
def pixel_msg_to_ground_msg(self, point_msg) -> PointMsg:
"""
Creates a :py:class:`ground_projection.Point` object from a normalized point message from an
unrectified
image. It converts it to pixel coordinates and rectifies it. Then projects it to the ground plane and
converts it to a ROS Point message.
Args:
point_msg (:obj:`geometry_msgs.msg.Point`): Normalized point coordinates from an unrectified
image.
Returns:
:obj:`geometry_msgs.msg.Point`: Point coordinates in the ground reference frame.
"""
# normalized coordinates to pixel:
norm_pt = Point.from_message(point_msg)
pixel = self.ground_projector.vector2pixel(norm_pt)
# rectify
rect = self.rectifier.rectify_point(pixel)
# convert to Point
rect_pt = Point.from_message(rect)
# project on ground
ground_pt = self.ground_projector.pixel2ground(rect_pt)
# point to message
ground_pt_msg = PointMsg()
ground_pt_msg.x = ground_pt.x
ground_pt_msg.y = ground_pt.y
ground_pt_msg.z = ground_pt.z
return ground_pt_msg
def lineseglist_cb(self, seglist_msg):
"""
Projects a list of line segments on the ground reference frame point by point by
calling :py:meth:`pixel_msg_to_ground_msg`. Then publishes the projected list of segments.
Args:
seglist_msg (:obj:`duckietown_msgs.msg.SegmentList`): Line segments in pixel space from
unrectified images
"""
if self.camera_info_received:
seglist_out = SegmentList()
seglist_out.header = seglist_msg.header
for received_segment in seglist_msg.segments:
new_segment = Segment()
new_segment.points[0] = self.pixel_msg_to_ground_msg(
received_segment.pixels_normalized[0])
new_segment.points[1] = self.pixel_msg_to_ground_msg(
received_segment.pixels_normalized[1])
new_segment.color = received_segment.color
# TODO what about normal and points
seglist_out.segments.append(new_segment)
self.pub_lineseglist.publish(seglist_out)
if not self.first_processing_done:
self.log("First projected segments published.")
self.first_processing_done = True
if self.pub_debug_img.get_num_connections() > 0:
debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(
self.debug_image(seglist_out))
debug_image_msg.header = seglist_out.header
self.pub_debug_img.publish(debug_image_msg)
else:
self.log("Waiting for a CameraInfo message", "warn")
# def get_ground_coordinate_cb(self, req):
# return GetGroundCoordResponse(self.pixel_msg_to_ground_msg(req.uv))
#
# def get_image_coordinate_cb(self, req):
# return GetImageCoordResponse(self.gpg.ground2pixel(req.gp))
#
# def estimate_homography_cb(self, req):
# rospy.loginfo("Estimating homography")
# rospy.loginfo("Waiting for raw image")
# img_msg = rospy.wait_for_message("/" + self.robot_name + "/camera_node/image/raw", Image)
# rospy.loginfo("Got raw image")
# try:
# cv_image = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding="bgr8")
# except CvBridgeError as e:
# rospy.logerr(e)
# self.gp.estimate_homography(cv_image)
# rospy.loginfo("wrote homography")
# return EstimateHomographyResponse()
def load_extrinsics(self):
"""
Loads the homography matrix from the extrinsic calibration file.
Returns:
:obj:`numpy array`: the loaded homography matrix
"""
# load intrinsic calibration
cali_file_folder = "/data/config/calibrations/camera_extrinsic/"
cali_file = cali_file_folder + rospy.get_namespace().strip(
"/") + ".yaml"
# Locate calibration yaml file or use the default otherwise
if not os.path.isfile(cali_file):
self.log(
f"Can't find calibration file: {cali_file}.\n Using default calibration instead.",
"warn")
cali_file = os.path.join(cali_file_folder, "default.yaml")
# Shutdown if no calibration file not found
if not os.path.isfile(cali_file):
msg = "Found no calibration file ... aborting"
self.logerr(msg)
rospy.signal_shutdown(msg)
try:
with open(cali_file, "r") as stream:
calib_data = yaml.load(stream, Loader=yaml.Loader)
except yaml.YAMLError:
msg = f"Error in parsing calibration file {cali_file} ... aborting"
self.logerr(msg)
rospy.signal_shutdown(msg)
return calib_data["homography"]
def debug_image(self, seg_list):
"""
Generates a debug image with all the projected segments plotted with respect to the robot's origin.
Args:
seg_list (:obj:`duckietown_msgs.msg.SegmentList`): Line segments in the ground plane relative
to the robot origin
Returns:
:obj:`numpy array`: an OpenCV image
"""
# dimensions of the image are 1m x 1m so, 1px = 2.5mm
# the origin is at x=200 and y=300
# if that's the first call, generate the background
if self.debug_img_bg is None:
# initialize gray image
self.debug_img_bg = np.ones((400, 400, 3), np.uint8) * 128
# draw vertical lines of the grid
for vline in np.arange(40, 361, 40):
cv2.line(self.debug_img_bg,
pt1=(vline, 20),
pt2=(vline, 300),
color=(255, 255, 0),
thickness=1)
# draw the coordinates
cv2.putText(
self.debug_img_bg,
"-20cm",
(120 - 25, 300 + 15),
cv2.FONT_HERSHEY_PLAIN,
0.8,
(255, 255, 0),
1,
)
cv2.putText(
self.debug_img_bg,
" 0cm",
(200 - 25, 300 + 15),
cv2.FONT_HERSHEY_PLAIN,
0.8,
(255, 255, 0),
1,
)
cv2.putText(
self.debug_img_bg,
"+20cm",
(280 - 25, 300 + 15),
cv2.FONT_HERSHEY_PLAIN,
0.8,
(255, 255, 0),
1,
)
# draw horizontal lines of the grid
for hline in np.arange(20, 301, 40):
cv2.line(self.debug_img_bg,
pt1=(40, hline),
pt2=(360, hline),
color=(255, 255, 0),
thickness=1)
# draw the coordinates
cv2.putText(self.debug_img_bg, "20cm", (2, 220 + 3),
cv2.FONT_HERSHEY_PLAIN, 0.8, (255, 255, 0), 1)
cv2.putText(self.debug_img_bg, " 0cm", (2, 300 + 3),
cv2.FONT_HERSHEY_PLAIN, 0.8, (255, 255, 0), 1)
# draw robot marker at the center
cv2.line(
self.debug_img_bg,
pt1=(200 + 0, 300 - 20),
pt2=(200 + 0, 300 + 0),
color=(255, 0, 0),
thickness=1,
)
cv2.line(
self.debug_img_bg,
pt1=(200 + 20, 300 - 20),
pt2=(200 + 0, 300 + 0),
color=(255, 0, 0),
thickness=1,
)
cv2.line(
self.debug_img_bg,
pt1=(200 - 20, 300 - 20),
pt2=(200 + 0, 300 + 0),
color=(255, 0, 0),
thickness=1,
)
# map segment color variables to BGR colors
color_map = {
Segment.WHITE: (255, 255, 255),
Segment.RED: (0, 0, 255),
Segment.YELLOW: (0, 255, 255)
}
image = self.debug_img_bg.copy()
# plot every segment if both ends are in the scope of the image (within 50cm from the origin)
for segment in seg_list.segments:
if not np.any(
np.abs([
segment.points[0].x, segment.points[0].y,
segment.points[1].x, segment.points[1].y
]) > 0.50):
cv2.line(
image,
pt1=(int(segment.points[0].y * -400) + 200,
int(segment.points[0].x * -400) + 300),
pt2=(int(segment.points[1].y * -400) + 200,
int(segment.points[1].x * -400) + 300),
color=color_map.get(segment.color, (0, 0, 0)),
thickness=1,
)
return image
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()
res['Raw input image'] = image
gpg = GroundProjectionGeometry()
rect = Rectify()
line_detector = LineDetector()
lane_filter = LaneFilterHistogram()
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