def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print 'cam_model = ', np.array(cam_model.intrinsicMatrix())
print 'cam tf = ', cam_model.tfFrame()
return cam_model
def create_cam_model(info_msg):
from image_geometry import PinholeCameraModel
import numpy as np
cam_model = PinholeCameraModel()
# <have an info message >
cam_model.fromCameraInfo(info_msg)
print "cam_model = ", np.array(cam_model.intrinsicMatrix())
print "cam tf = ", cam_model.tfFrame()
return cam_model
def remap(raw_img, camera_info: CameraInfo):
cam = PinholeCameraModel()
cam.fromCameraInfo(camera_info)
map_x, map_y = cv2.initUndistortRectifyMap(
cam.intrinsicMatrix(),
cam.distortionCoeffs(),
cam.R,
cam.projectionMatrix(),
(cam.width, cam.height),
cv2.CV_32FC1,
)
rectified_image = cv2.remap(raw_img, map_x, map_y, cv2.INTER_CUBIC)
return rectified_image
class Scanner:
def __init__(self):
self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
self.scanner_info_file_name = rospy.get_param(
'~scanner_info_file_name')
self.threshold = rospy.get_param('~threshold')
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
self.bridge = CvBridge()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info',
sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
get_projector_info = rospy.ServiceProxy('get_projector_info',
projector.srv.GetProjectorInfo)
self.projector_info = get_projector_info().projector_info
self.projector_model = PinholeCameraModel()
self.projector_model.fromCameraInfo(self.projector_info)
self.read_scanner_info()
self.projector_to_camera_rotation_matrix = cv.CreateMat(
3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_matrix)
predistortmap_x = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_32FC1)
InitPredistortMap(self.projector_model.intrinsicMatrix(),
self.projector_model.distortionCoeffs(),
predistortmap_x, predistortmap_y)
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
cv.AddS(predistortmap_x, -minVal, predistortmap_x)
uncropped_projection_width = int(math.ceil(maxVal - minVal))
self.center_pixel = self.projector_model.cx() - minVal
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
cv.AddS(predistortmap_y, -minVal, predistortmap_y)
uncropped_projection_height = int(math.ceil(maxVal - minVal))
self.number_of_scanlines = int(
math.ceil(
float(uncropped_projection_width) / self.pixels_per_scanline))
rospy.loginfo("Generating projection patterns...")
graycodes = []
for i in range(self.number_of_scanlines):
graycodes.append(graycodemath.generate_gray_code(i))
self.number_of_projection_patterns = int(
math.ceil(math.log(self.number_of_scanlines, 2)))
self.predistorted_positive_projections = []
self.predistorted_negative_projections = []
for i in range(self.number_of_projection_patterns):
cross_section = cv.CreateMat(1, uncropped_projection_width,
cv.CV_8UC1)
#Fill in cross section with the associated bit of each gray code
for pixel in range(uncropped_projection_width):
scanline = pixel // self.pixels_per_scanline
scanline_value = graycodemath.get_bit(graycodes[scanline],
i) * 255
cross_section[0, pixel] = scanline_value
#Repeat the cross section over the entire image
positive_projection = cv.CreateMat(uncropped_projection_height,
uncropped_projection_width,
cv.CV_8UC1)
cv.Repeat(cross_section, positive_projection)
#Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
predistorted_positive_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width,
cv.CV_8UC1)
cv.Remap(positive_projection,
predistorted_positive_projection,
predistortmap_x,
predistortmap_y,
flags=cv.CV_INTER_NN)
#Create a negative of the pattern for thresholding
predistorted_negative_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width,
cv.CV_8UC1)
cv.Not(predistorted_positive_projection,
predistorted_negative_projection)
#Fade the borders of the patterns to avoid artifacts at the edges of projection
predistorted_positive_projection_faded = fade_edges(
predistorted_positive_projection, 20)
predistorted_negative_projection_faded = fade_edges(
predistorted_negative_projection, 20)
self.predistorted_positive_projections.append(
predistorted_positive_projection_faded)
self.predistorted_negative_projections.append(
predistorted_negative_projection_faded)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection',
projector.srv.SetProjection)
self.pixel_associations_msg = None
self.point_cloud_msg = None
rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud,
self.handle_point_cloud_srv)
point_cloud_pub = rospy.Publisher('~point_cloud',
sensor_msgs.msg.PointCloud)
rospy.loginfo("Ready.")
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if self.point_cloud_msg is not None:
point_cloud_pub.publish(self.point_cloud_msg)
rate.sleep()
def read_scanner_info(self):
try:
input_stream = file(self.scanner_info_file_name, 'r')
except IOError:
rospy.loginfo('Specified scanner info file at ' +
self.scanner_info_file_name + ' does not exist.')
return
info_dict = yaml.load(input_stream)
try:
self.projector_to_camera_translation_vector = tuple(
info_dict['projector_to_camera_translation_vector'])
self.projector_to_camera_rotation_vector = list_to_matrix(
info_dict['projector_to_camera_rotation_vector'], 3, 1,
cv.CV_32FC1)
except (TypeError, KeyError):
rospy.loginfo('Scanner info file at ' +
self.scanner_info_file_name +
' is in an unrecognized format.')
return
rospy.loginfo('Scanner info successfully read from ' +
self.scanner_info_file_name)
def update_image(self, imagemsg):
with self.mutex:
if not self.image_update_flag.is_set():
self.latest_image = self.bridge.imgmsg_to_cv(imagemsg, "mono8")
self.image_update_flag.set()
def get_next_image(self):
with self.mutex:
self.image_update_flag.clear()
self.image_update_flag.wait()
with self.mutex:
return self.latest_image
def get_picture_of_projection(self, projection):
image_message = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(image_message)
return self.get_next_image()
def get_projector_line_associations(self):
rospy.loginfo("Scanning...")
positives = []
negatives = []
for i in range(self.number_of_projection_patterns):
positives.append(
self.get_picture_of_projection(
self.predistorted_positive_projections[i]))
negatives.append(
self.get_picture_of_projection(
self.predistorted_negative_projections[i]))
rospy.loginfo("Thresholding...")
strike_sum = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.SetZero(strike_sum)
gray_codes = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.SetZero(gray_codes)
for i in range(self.number_of_projection_patterns):
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positives[i], negatives[i], difference)
absolute_difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_8UC1)
cv.AbsDiff(positives[i], negatives[i], absolute_difference)
#Mark all the pixels that were "too close to call" and add them to the running total
strike_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(absolute_difference, self.threshold, strike_mask,
cv.CV_CMP_LT)
strikes = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.Set(strikes, 1, strike_mask)
cv.Add(strikes, strike_sum, strike_sum)
#Set the corresponding bit in the gray_code
bit_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(difference, 0, bit_mask, cv.CV_CMP_GT)
bit_values = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.Set(bit_values, 2**i, bit_mask)
cv.Or(bit_values, gray_codes, gray_codes)
rospy.loginfo("Decoding...")
# Decode every gray code into binary
projector_line_associations = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
cv.Copy(gray_codes, projector_line_associations)
for i in range(
cv.CV_MAT_DEPTH(cv.GetElemType(projector_line_associations)),
-1, -1):
projector_line_associations_bitshifted_right = cv.CreateMat(
self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
#Using ConvertScale with a shift of -0.5 to do integer division for bitshifting right
cv.ConvertScale(projector_line_associations,
projector_line_associations_bitshifted_right,
(2**-(2**i)), -0.5)
cv.Xor(projector_line_associations,
projector_line_associations_bitshifted_right,
projector_line_associations)
rospy.loginfo("Post processing...")
# Remove all pixels that were "too close to call" more than once
strikeout_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(strike_sum, 1, strikeout_mask, cv.CV_CMP_GT)
cv.Set(projector_line_associations, -1, strikeout_mask)
# Remove all pixels that don't decode to a valid scanline number
invalid_scanline_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_8UC1)
cv.InRangeS(projector_line_associations, 0, self.number_of_scanlines,
invalid_scanline_mask)
cv.Not(invalid_scanline_mask, invalid_scanline_mask)
cv.Set(projector_line_associations, -1, invalid_scanline_mask)
self.display_scanline_associations(projector_line_associations)
return projector_line_associations
def handle_point_cloud_srv(self, req):
response = graycode_scanner.srv.GetPointCloudResponse()
self.get_point_cloud()
response.point_cloud = self.point_cloud_msg
response.success = True
return response
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(
illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection,
encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates,
rectified_camera_coordinates,
camera_model.intrinsicMatrix(),
camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates,
camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
def scanline_numbers_to_planes(self, scanline_numbers):
rows = scanline_numbers.height
cols = scanline_numbers.width
normal_vectors_x = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_x, -1)
normal_vectors_y = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_y, 0)
normal_vectors_z = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Copy(scanline_numbers, normal_vectors_z)
cv.ConvertScale(normal_vectors_z,
normal_vectors_z,
scale=self.pixels_per_scanline)
cv.AddS(normal_vectors_z, -self.center_pixel, normal_vectors_z)
cv.ConvertScale(normal_vectors_z,
normal_vectors_z,
scale=1.0 / self.projector_model.fx())
normal_vectors = cv.CreateMat(rows, cols, cv.CV_32FC3)
cv.Merge(normal_vectors_x, normal_vectors_y, normal_vectors_z, None,
normal_vectors)
# Bring the normal vectors into camera coordinates
cv.Transform(normal_vectors, normal_vectors,
self.projector_to_camera_rotation_matrix)
normal_vectors_split = [None] * 3
for i in range(3):
normal_vectors_split[i] = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Split(normal_vectors, normal_vectors_split[0],
normal_vectors_split[1], normal_vectors_split[2], None)
n_dot_p = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.SetZero(n_dot_p)
for i in range(3):
cv.ScaleAdd(normal_vectors_split[i],
self.projector_to_camera_translation_vector[i],
n_dot_p, n_dot_p)
planes = cv.CreateMat(rows, cols, cv.CV_32FC4)
cv.Merge(normal_vectors_split[0], normal_vectors_split[1],
normal_vectors_split[2], n_dot_p, planes)
return planes
def display_scanline_associations(self, associations):
display_image = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.ConvertScale(associations, display_image,
255.0 / self.number_of_scanlines)
cv.NamedWindow("associations", flags=0)
cv.ShowImage("associations", display_image)
cv.WaitKey(800)
class FollowerDrive(DTROS):
def __init__(self, node_name):
# initialize the DTROS parent class
super(FollowerDrive, self).__init__(node_name=node_name)
self.listener = tf.TransformListener()
#self.sub_follower_pose = rospy.Subscriber('', VehiclePose)
self.sub_image = rospy.Subscriber(
"/duckiesam/camera_node/image/compressed",
CompressedImage,
self.find_marker,
buff_size=921600,
queue_size=1)
self.pub_image = rospy.Publisher('/duckiesam/modified_image',
Image,
queue_size=1)
self.sub_info = rospy.Subscriber("/duckiesam/camera_node/camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
self.pub_move = rospy.Publisher("/duckiesam/joy_mapper_node/car_cmd",
Twist2DStamped,
queue_size=1)
self.pub_pose = rospy.Publisher(
"/duckiesam/velocity_to_pose_node/pose",
Pose2DStamped,
queue_size=1)
self.pub_detection = rospy.Publisher("detect",
BoolStamped,
queue_size=1)
#values for detecting marker
self.starting = 0
self.ending = 0
self.camerainfo = PinholeCameraModel()
self.bridge = CvBridge()
self.gotimage = False
self.imagelast = None
self.processedImg = None
self.detected = False
self.number = 0
#values for calculating pose of robot
self.originalmatrix()
self.solP = False
#self.rotationvector = None
#self.translationvector = None
self.axis = np.float32([[0.0125, 0, 0], [0, 0.0125, 0],
[0, 0, -0.0375]]).reshape(-1, 3)
#self.distance = None
#self.angle_f = None
#self.angle_l = None
self.count = 0
self.add_distance = 0.0
self.add_x = 0.0
self.add_y = 0.0
self.add_angle_l = 0.0
self.needed = True
#values for driving the robot
self.initialvalues()
rospy.on_shutdown(self.my_shutdown)
def initialvalues(self):
self.maxdistance = 0.25
self.speedN = 0
self.e_vB = 0
self.rotationN = 0
self.mindistance = 0.2
self.d_e = 0 #distance error
self.d_e_1 = 0
self.d_e_2 = 0
self.y2 = 0
self.controltime = rospy.Time.now()
self.Kp = 5
self.Ki = 0
self.Kd = 0
self.I = 0
self.r1 = 3
self.r2 = 0
self.r3 = 0
#get camera info for pinhole camera model
def get_camera_info(self, camera_msg):
self.camerainfo.fromCameraInfo(camera_msg)
#step 1 : find the back circle grids using cv2.findCirclesGrid
##### set (x,y) for points and flag for detection
def find_marker(self, image_msg):
try:
self.imagelast = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print(e)
if self.gotimage == False:
self.gotimage = True
#time checking
self.starting = rospy.Time.now()
#from_last_image = (self.starting - self.ending).to_sec()
gray = cv2.cvtColor(self.imagelast, cv2.COLOR_BGR2GRAY)
detection, corners = cv2.findCirclesGrid(gray, (7, 3))
processedImg = self.imagelast.copy()
if detection:
cv2.drawChessboardCorners(processedImg, (7, 3), corners, detection)
self.detected = True
twoone = []
for i in range(0, 21):
point = [corners[i][0][0], corners[i][0][1]]
twoone.append(point)
twoone = np.array(twoone)
rotationvector, translationvector, processedImg = self.gradient(
twoone, processedImg)
self.detected = self.solP
img_out = self.bridge.cv2_to_imgmsg(processedImg, "bgr8")
self.pub_image.publish(img_out)
distance, x, y, angle_l = self.find_distance(
translationvector, rotationvector)
self.ending = rospy.Time.now()
else:
self.detected = False
img_out = self.bridge.cv2_to_imgmsg(self.imagelast, "bgr8")
self.pub_image.publish(img_out)
self.ending = rospy.Time.now()
#cmd.v = 0
#cmd.omega = 0
#self.pub_move.publish(cmd)
#step 2 : makes matrix for 3d original shape
def originalmatrix(self):
#coners and points
self.originalmtx = np.zeros([21, 3])
for i in range(0, 7):
for j in range(0, 3):
self.originalmtx[i + j * 7, 0] = 0.0125 * i - 0.0125 * 3
self.originalmtx[i + j * 7, 1] = 0.0125 * j - 0.0125
#step 3 : use intrinsic matrix and solvePnP, return rvec and tvec, print axis
def gradient(self, imgpts, processedImg):
#using solvePnP to find rotation vector and translation vector and also find 3D point to the image plane
self.solP, rotationvector, translationvector = cv2.solvePnP(
self.originalmtx, imgpts, self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
if self.solP:
pointsin3D, jacoB = cv2.projectPoints(
self.originalmtx, rotationvector, translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
pointaxis, _ = cv2.projectPoints(
self.axis, rotationvector, translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
processedImg = cv2.line(processedImg, tuple(imgpts[10].ravel()),
tuple(pointaxis[0].ravel()), (255, 0, 0),
2)
processedImg = cv2.line(processedImg, tuple(imgpts[10].ravel()),
tuple(pointaxis[1].ravel()), (0, 255, 0),
2)
processedImg = cv2.line(processedImg, tuple(imgpts[10].ravel()),
tuple(pointaxis[2].ravel()), (0, 0, 255),
3)
return rotationvector, translationvector, processedImg
#step 4 : find distance between robot and following robot print out distance and time
def find_distance(self, translationvector, rotationvector):
#use tvec to calculate distance
tvx = translationvector[0]
tvy = translationvector[1]
tvz = translationvector[2]
distance = math.sqrt(tvx * tvx + tvz * tvz)
angle_f = np.arctan2(tvx[0], tvz[0])
R, _ = cv2.Rodrigues(rotationvector)
R_inverse = np.transpose(R)
angle_l = np.arctan2(
-R_inverse[2, 0],
math.sqrt(R_inverse[2, 1]**2 + R_inverse[2, 2]**2))
T = np.array([-np.sin(angle_l), np.cos(angle_l)])
#tvecW is position of camera(follower vehicle) in world frame
tvecW = -np.dot(R_inverse, translationvector)
#desire point [0.20, 0] x,y, now tvz = x tvx = y
x = tvecW[2][0]
y = tvecW[0][0]
#y2 = tvecW[0][0] - 0.05*np.sin(angle_l)
textdistance = "Distance = %s, Angle of Follower = %s, Angle of Leader = %s" % (
distance, angle_f, angle_l)
rospy.loginfo("%s" % textdistance)
if self.count < 10 and self.needed:
self.add_distance = self.add_distance + distance
self.add_x = self.add_x + x
self.add_y = self.add_y + y
self.add_angle_l = self.add_angle_l
self.count += 1
return distance, x, y, angle_l
def initial_pose(self):
distance = self.add_distance / self.count
x = self.add_x / self.count
y = self.add_y / self.count
angle_l = self.add_angle_l / self.count
self.count = 0
pose_init_msg = Pose2DStamped()
pose_init_msg.x = x
pose_init_msg.y = y
pose_init_msg.theta = angle_l
self.pub_pose.publish(pose_init_msg)
self.needed = False
ready = BoolStamped()
ready.header.stamp = rospy.Time.now()
ready.data = True
self.pub_detection.publish(ready)
def move(self):
#from the bag move drive
rate = rospy.Rate(10.0)
self.listener.waitForTransform("/duckiesam", "/needtogo", rospy.Time(),
rospy.Duration(4.0))
while not rospy.is_shutdown():
try:
now = rospy.Time.now()
self.listener.waitForTransform("/duckiesam", "/needtogo", now,
rospy.Duration(4.0))
(trans,
rot) = self.listener.lookupTransform('/duckiesam',
'/needtogo',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt(trans[0]**2 + trans[1]**2)
cmd = Twist2DStamped()
self.number += 1
cmd.header.seq = self.number
cmd.v = linear
cmd.omega = angular
self.pub_move.publish(cmd)
rate.sleep()
0.1639958233797625, -0.271840030972792, 0.001055841660100477,
-0.00166555973740089, 0.0
]
cam_info.K = [
322.0704122808738, 0.0, 199.2680620421962, 0.0, 320.8673986158544,
155.2533082600705, 0.0, 0.0, 1.0
]
cam_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
cam_info.P = [
329.2483825683594, 0.0, 198.4101510452074, 0.0, 0.0, 329.1044006347656,
155.5057121208347, 0.0, 0.0, 0.0, 1.0, 0.0
]
model.fromCameraInfo(cam_info)
print(model.distortionCoeffs())
print(model.intrinsicMatrix())
print('a')
def PointFromPixel(pixel, camera_model, depth=1000):
print('Ã ')
ray = model.projectPixelTo3dRay(pixel)
ray_z = [el / ray[2] for el in ray]
pt = [el * depth for el in ray_z]
print('b')
return pt
pixel1 = (11, 34)
point = PointFromPixel(pixel1, model)
class camera_lidar_calib(object):
def __init__(self):
self._cv_bridge = CvBridge()
self._laser_projector = LaserProjection()
# # Camera rectification?? To be improved: read from .yaml file
# Put here the calibration of the camera
# self.DIM = (1920, 1208)
# self.K = np.array([[693.506921, 0.000000, 955.729444], [0.000000, 694.129349, 641.732500], [0.0, 0.0, 1.0]])
# self.D = np.array([[-0.022636], [ -0.033855], [0.013493], [-0.001831]])
# self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(self.K, self.D, np.eye(3), self.K, self.DIM, cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix
# OR load it from a yaml file
self.cameraModel = PinholeCameraModel()
# See https://github.com/ros-perception/camera_info_manager_py/tree/master/tests
camera_infomanager = CameraInfoManager(
cname='truefisheye',
url='package://ros_camera_lidar_calib/cfg/truefisheye800x503.yaml'
) # Select the calibration file
camera_infomanager.loadCameraInfo()
self.cameraInfo = camera_infomanager.getCameraInfo()
# Crete a camera from camera info
self.cameraModel.fromCameraInfo(self.cameraInfo) # Create camera model
self.DIM = (self.cameraInfo.width, self.cameraInfo.height)
# Get rectification maps
self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(
self.cameraModel.intrinsicMatrix(),
self.cameraModel.distortionCoeffs(), np.eye(3),
self.cameraModel.intrinsicMatrix(),
(self.cameraInfo.width, self.cameraInfo.height),
cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix
# # Declare subscribers to get the latest data
cam0_subs_topic = '/gmsl_camera/port_0/cam_0/image_raw'
cam1_subs_topic = '/gmsl_camera/port_0/cam_1/image_raw'
cam2_subs_topic = '/gmsl_camera/port_0/cam_2/image_raw'
#cam3_subs_topic = '/gmsl_camera/port_0/cam_3/image_raw/compressed'
lidar_subs_topic = '/Sensor/points'
#self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , Image, self.cam0_img_callback, queue_size=1)
#self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , Image, self.cam1_img_callback, queue_size=1)
self.cam2_img_sub = rospy.Subscriber(cam2_subs_topic,
Image,
self.cam2_img_callback,
queue_size=1)
#self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , CompressedImage, self.cam0_img_compre_callback, queue_size=1)
#self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , CompressedImage, self.cam1_img_compre_callback, queue_size=1)
#self.cam2_img_sub = rospy.Subscriber( cam2_subs_topic , CompressedImage, self.cam2_img_compre_callback, queue_size=1)
#self.cam3_img_sub = rospy.Subscriber( cam3_subs_topic , CompressedImage, self.cam3_img_compre_callback, queue_size=1)
self.lidar_sub = rospy.Subscriber(lidar_subs_topic,
PointCloud2,
self.lidar_callback,
queue_size=1)
# Get the tfs
self.tf_listener = tf.TransformListener()
#self.lidar_time = rospy.Subscriber(lidar_subs_topic , PointCloud2, self.readtimestamp)
#self.img0_time = rospy.Subscriber(cam0_subs_topic , CompressedImage, self.readtimestamp)
# # Declare the global variables we will use to get the latest info
self.cam0_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam0_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam1_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam1_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam2_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam2_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam3_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam3_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.pcl_cloud = np.empty(
(500, 4)
) # Do not know the width of a normal scan. might be variable too......
self.now = rospy.Time.now()
# # Main loop: Data projections and alignment on real time
self.lidar_angle_range_interest = [
0, 180
] # From -180 to 180. Front is 0deg. Put the range of angles we may want to get points from. Depending of camera etc
thread.start_new_thread(self.projection_calibrate())
def projection_calibrate(self):
# # Main loop: Data projections and alignment on real time
rate = rospy.Rate(30) # ?
rot_trans_matrix = np.empty(0)
while not rospy.is_shutdown():
# Get the tfs
if not rot_trans_matrix.shape[0]: # Get the tf is empty
try:
(trans, rot) = self.tf_listener.lookupTransform(
"/port0_cam3", "/Sensor", rospy.Time(
0)) # get different protections here, as needed
rot_trans_matrix = self.tf_listener.fromTranslationRotation(
trans, rot)
# print(trans)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
## Projections: see https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
# # project onto camera frame:
cloud_oncam = np.dot(rot_trans_matrix[:3, :],
self.pcl_cloud[:, :4].T)
# Filter points that wont be on the image: remove points behind the camera plane
#cloud_oncam = cloud_oncam[:,cloud_oncam[2,:]>0.01]
cloud_oncam_2d = cloud_oncam / cloud_oncam[
2, :] # project 3D to 2D plane
# # Project onto images pixels
#cloud_pixels = np.dot( self.K , cloud_oncam_2d ).astype(int) # [u,v].T, u->x-.Horizontal, v->y->vertical
cloud_pixels = np.dot(np.array(
self.cameraModel.intrinsicMatrix()), cloud_oncam_2d).astype(
int) # [u,v,1].T, u->x-.Horizontal, v->y->vertical
cloud_pixels[2, :] = cloud_oncam[
2, :] #Append on the last dim the real depth. Not in camera plane dimensions. DEPTH: [u,v,depth in m].T
# filter pixels out of img
padding = 2
cloud_pixels = cloud_pixels[:, (cloud_pixels[0, :] > padding) & (
cloud_pixels[0, :] <
(self.DIM[0] - padding)) & (cloud_pixels[1, :] > padding) & (
cloud_pixels[1, :] <
(self.DIM[1] -
padding))] # Filter those points outside the image
image = self.cam2_undistorted_img # improve loop performance by putting it in a variable
for idx in range(len(cloud_pixels[2])):
image[cloud_pixels[1, idx], cloud_pixels[0, idx], :] = [
0, 0, np.clip(cloud_pixels[2, idx] * 20, 0, 255)
]
self.cam2_undistorted_img = image
# Show imgs
cv2.imshow("undistorted2", self.cam2_undistorted_img)
#cv2.imshow("distorted0",self.cam0_image_np)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Loop!
rate.sleep()
cv2.destroyAllWindows()
def readtimestamp(self, data):
print(data.header.stamp)
#print(data.header.frame_id,data.header.stamp,data.header.seq)
def cam0_img_callback(self, image_msg):
# Get image as np
# self.cam0_image_np = self._cv_bridge.compressed_imgmsg_to_cv2(image_msg)
self.cam0_image_np = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
self.cam0_undistorted_img = cv2.remap(self.cam0_image_np,
self.map1,
self.map2,
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
def cam1_img_callback(self, image_msg):
# Get image as np
# self.cam0_image_np = self._cv_bridge.compressed_imgmsg_to_cv2(image_msg)
self.cam1_image_np = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
self.cam1_undistorted_img = cv2.remap(self.cam1_image_np,
self.map1,
self.map2,
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
def cam2_img_callback(self, image_msg):
# Get image as np
# self.cam0_image_np = self._cv_bridge.compressed_imgmsg_to_cv2(image_msg)
self.cam2_image_np = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
self.cam2_undistorted_img = cv2.remap(self.cam2_image_np,
self.map1,
self.map2,
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
# def cam0_img_compre_callback(self,image_msg):
# # Get image as np
# self.cam0_image_np = self._cv_bridge.compre_imgmsg_to_cv2(image_msg)
# #self.cam0_image_np = cv2.imdecode( np.fromstring(image_msg.data, np.uint8) , cv2.IMREAD_COLOR) # OpenCV >= 3.0:
#
# self.cam0_undistorted_img = cv2.remap(self.cam0_image_np, self.map1, self.map2, interpolation=cv2.INTER_LANCZOS4, borderMode=cv2.BORDER_CONSTANT)
#
# def cam1_img_compre_callback(self,image_msg):
# # Get image as np
# self.cam1_image_np = self._cv_bridge.compressed_imgmsg_to_cv2(image_msg)
# self.cam1_undistorted_img = cv2.remap(self.cam0_image_np, self.map1, self.map2, interpolation=cv2.INTER_LANCZOS4, borderMode=cv2.BORDER_CONSTANT)
#
# def cam2_img_compre_callback(self,image_msg):
# # Get image as np
# self.cam2_image_np = self._cv_bridge.compressed_imgmsg_to_cv2(image_msg)
# self.cam2_undistorted_img = cv2.remap(self.cam0_image_np, self.map1, self.map2, interpolation=cv2.INTER_LANCZOS4, borderMode=cv2.BORDER_CONSTANT)
#
# def cam3_img_compre_callback(self,image_msg):
# # Get image as np
# self.cam3_image_np = self._cv_bridge.compressed_imgmsg_to_cv2(image_msg)
# self.cam3_undistorted_img = cv2.remap(self.cam0_image_np, self.map1, self.map2, interpolation=cv2.INTER_LANCZOS4, borderMode=cv2.BORDER_CONSTANT)
def lidar_callback(self, lidar_scan):
""" Converts a ROS PointCloud2 message to a pcl PointXYZRGB
Args:
lidar_scan (PointCloud2): ROS PointCloud2 message
Returns:
np pointcloud: numpy XYZ1I point cloud
"""
# points_list = []
# for data in pc2.read_points(lidar_scan, skip_nans=True):
# points_list.append([data[0], data[1], data[2],1.0])
# self.pcl_cloud = np.array(points_list)
# 2nd method
#self.pcl_cloud = np.array( list(pc2.read_points(lidar_scan, skip_nans=True)) )
# 3rd method
nparr = pcl2_2_np.msg_to_arr(lidar_scan)
width = nparr.shape[1]
m = width / 360
self.pcl_cloud = self.lidar_map_flatten(
nparr[:,
int(width / 2.0 + self.lidar_angle_range_interest[0] *
m):int(width / 2.0 + self.lidar_angle_range_interest[1] *
m)]) # we cloud filter here by angle!
# Debugging tools
# print " Got cloud @ ", 1.0/float(rospy.Time.now().to_sec() - self.now.to_sec()) ," Hz "
# self.now = rospy.Time.now()
def lidar_map_flatten(self, nparr):
""" Converts a numpy array of (height, width) [x,y,z,i,_] into a flatten np array of [xyz1,numer_of_points]
Args:
nparr (numpy): numpy array of (8, ~5440) [x,y,z,i,_]loud @ ", 1.0/float(rospy.Time.now().to_sec() - self.now.to_sec()) ," Hz "
self.now = ros
Returns:
np pointcloud: numpy [xyz1,numer_of_points] point cloud
"""
pcl_cloud = np.ones((nparr.shape[0] * nparr.shape[1], 4))
pcl_cloud[:, 0] = np.reshape(nparr['x'], -1)
pcl_cloud[:, 1] = np.reshape(nparr['y'], -1)
pcl_cloud[:, 2] = np.reshape(nparr['z'], -1)
return pcl_cloud
def spin(self):
rospy.spin()
class Scanner:
def __init__(self):
self.pixels_per_scanline = rospy.get_param('~pixels_per_scanline')
self.scanner_info_file_name = rospy.get_param('~scanner_info_file_name')
self.threshold = rospy.get_param('~threshold')
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
self.bridge = CvBridge()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image, self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info', sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
get_projector_info = rospy.ServiceProxy('get_projector_info', projector.srv.GetProjectorInfo)
self.projector_info = get_projector_info().projector_info
self.projector_model = PinholeCameraModel()
self.projector_model.fromCameraInfo(self.projector_info)
self.read_scanner_info()
self.projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector, self.projector_to_camera_rotation_matrix)
predistortmap_x = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_32FC1)
InitPredistortMap(self.projector_model.intrinsicMatrix(), self.projector_model.distortionCoeffs(), predistortmap_x, predistortmap_y)
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_x)
cv.AddS(predistortmap_x, -minVal, predistortmap_x)
uncropped_projection_width = int(math.ceil(maxVal - minVal))
self.center_pixel = self.projector_model.cx() - minVal
minVal, maxVal, minLoc, maxLoc = cv.MinMaxLoc(predistortmap_y)
cv.AddS(predistortmap_y, -minVal, predistortmap_y)
uncropped_projection_height = int(math.ceil(maxVal - minVal))
self.number_of_scanlines = int(math.ceil(float(uncropped_projection_width)/self.pixels_per_scanline))
rospy.loginfo("Generating projection patterns...")
graycodes = []
for i in range(self.number_of_scanlines):
graycodes.append(graycodemath.generate_gray_code(i))
self.number_of_projection_patterns = int(math.ceil(math.log(self.number_of_scanlines, 2)))
self.predistorted_positive_projections = []
self.predistorted_negative_projections = []
for i in range(self.number_of_projection_patterns):
cross_section = cv.CreateMat(1, uncropped_projection_width, cv.CV_8UC1)
#Fill in cross section with the associated bit of each gray code
for pixel in range(uncropped_projection_width):
scanline = pixel // self.pixels_per_scanline
scanline_value = graycodemath.get_bit(graycodes[scanline], i) * 255
cross_section[0, pixel] = scanline_value
#Repeat the cross section over the entire image
positive_projection = cv.CreateMat(uncropped_projection_height, uncropped_projection_width, cv.CV_8UC1)
cv.Repeat(cross_section, positive_projection)
#Predistort the projections to compensate for projector optics so that that the scanlines are approximately planar
predistorted_positive_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Remap(positive_projection, predistorted_positive_projection, predistortmap_x, predistortmap_y, flags=cv.CV_INTER_NN)
#Create a negative of the pattern for thresholding
predistorted_negative_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Not(predistorted_positive_projection, predistorted_negative_projection)
#Fade the borders of the patterns to avoid artifacts at the edges of projection
predistorted_positive_projection_faded = fade_edges(predistorted_positive_projection, 20)
predistorted_negative_projection_faded = fade_edges(predistorted_negative_projection, 20)
self.predistorted_positive_projections.append(predistorted_positive_projection_faded)
self.predistorted_negative_projections.append(predistorted_negative_projection_faded)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection', projector.srv.SetProjection)
self.pixel_associations_msg = None
self.point_cloud_msg = None
rospy.Service("~get_point_cloud", graycode_scanner.srv.GetPointCloud, self.handle_point_cloud_srv)
point_cloud_pub = rospy.Publisher('~point_cloud', sensor_msgs.msg.PointCloud)
rospy.loginfo("Ready.")
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if self.point_cloud_msg is not None:
point_cloud_pub.publish(self.point_cloud_msg)
rate.sleep()
def read_scanner_info(self):
try:
input_stream = file(self.scanner_info_file_name, 'r')
except IOError:
rospy.loginfo('Specified scanner info file at ' + self.scanner_info_file_name + ' does not exist.')
return
info_dict = yaml.load(input_stream)
try:
self.projector_to_camera_translation_vector = tuple(info_dict['projector_to_camera_translation_vector'])
self.projector_to_camera_rotation_vector = list_to_matrix(info_dict['projector_to_camera_rotation_vector'], 3, 1, cv.CV_32FC1)
except (TypeError, KeyError):
rospy.loginfo('Scanner info file at ' + self.scanner_info_file_name + ' is in an unrecognized format.')
return
rospy.loginfo('Scanner info successfully read from ' + self.scanner_info_file_name)
def update_image(self, imagemsg):
with self.mutex:
if not self.image_update_flag.is_set():
self.latest_image = self.bridge.imgmsg_to_cv(imagemsg, "mono8")
self.image_update_flag.set()
def get_next_image(self):
with self.mutex:
self.image_update_flag.clear()
self.image_update_flag.wait()
with self.mutex:
return self.latest_image
def get_picture_of_projection(self, projection):
image_message = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(image_message)
return self.get_next_image()
def get_projector_line_associations(self):
rospy.loginfo("Scanning...")
positives = []
negatives = []
for i in range(self.number_of_projection_patterns):
positives.append(self.get_picture_of_projection(self.predistorted_positive_projections[i]))
negatives.append(self.get_picture_of_projection(self.predistorted_negative_projections[i]))
rospy.loginfo("Thresholding...")
strike_sum = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.SetZero(strike_sum)
gray_codes = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.SetZero(gray_codes)
for i in range(self.number_of_projection_patterns):
difference = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.Sub(positives[i], negatives[i], difference)
absolute_difference = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.AbsDiff(positives[i], negatives[i], absolute_difference)
#Mark all the pixels that were "too close to call" and add them to the running total
strike_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(absolute_difference, self.threshold, strike_mask, cv.CV_CMP_LT)
strikes = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Set(strikes, 1, strike_mask)
cv.Add(strikes, strike_sum, strike_sum)
#Set the corresponding bit in the gray_code
bit_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(difference, 0, bit_mask, cv.CV_CMP_GT)
bit_values = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Set(bit_values, 2 ** i, bit_mask)
cv.Or(bit_values, gray_codes, gray_codes)
rospy.loginfo("Decoding...")
# Decode every gray code into binary
projector_line_associations = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Copy(gray_codes, projector_line_associations)
for i in range(cv.CV_MAT_DEPTH(cv.GetElemType(projector_line_associations)), -1, -1):
projector_line_associations_bitshifted_right = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
#Using ConvertScale with a shift of -0.5 to do integer division for bitshifting right
cv.ConvertScale(projector_line_associations, projector_line_associations_bitshifted_right, (2 ** -(2 ** i)), -0.5)
cv.Xor(projector_line_associations, projector_line_associations_bitshifted_right, projector_line_associations)
rospy.loginfo("Post processing...")
# Remove all pixels that were "too close to call" more than once
strikeout_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(strike_sum, 1, strikeout_mask, cv.CV_CMP_GT)
cv.Set(projector_line_associations, -1, strikeout_mask)
# Remove all pixels that don't decode to a valid scanline number
invalid_scanline_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.InRangeS(projector_line_associations, 0, self.number_of_scanlines, invalid_scanline_mask)
cv.Not(invalid_scanline_mask, invalid_scanline_mask)
cv.Set(projector_line_associations, -1, invalid_scanline_mask)
self.display_scanline_associations(projector_line_associations)
return projector_line_associations
def handle_point_cloud_srv(self, req):
response = graycode_scanner.srv.GetPointCloudResponse()
self.get_point_cloud()
response.point_cloud = self.point_cloud_msg
response.success = True
return response
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0;
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates, camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
def scanline_numbers_to_planes(self, scanline_numbers):
rows = scanline_numbers.height
cols = scanline_numbers.width
normal_vectors_x = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_x, -1)
normal_vectors_y = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Set(normal_vectors_y, 0)
normal_vectors_z = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Copy(scanline_numbers, normal_vectors_z)
cv.ConvertScale(normal_vectors_z, normal_vectors_z, scale=self.pixels_per_scanline)
cv.AddS(normal_vectors_z, -self.center_pixel, normal_vectors_z)
cv.ConvertScale(normal_vectors_z, normal_vectors_z, scale=1.0/self.projector_model.fx())
normal_vectors = cv.CreateMat(rows, cols, cv.CV_32FC3)
cv.Merge(normal_vectors_x, normal_vectors_y, normal_vectors_z, None, normal_vectors)
# Bring the normal vectors into camera coordinates
cv.Transform(normal_vectors, normal_vectors, self.projector_to_camera_rotation_matrix)
normal_vectors_split = [None]*3
for i in range(3):
normal_vectors_split[i] = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.Split(normal_vectors, normal_vectors_split[0], normal_vectors_split[1], normal_vectors_split[2], None)
n_dot_p = cv.CreateMat(rows, cols, cv.CV_32FC1)
cv.SetZero(n_dot_p)
for i in range(3):
cv.ScaleAdd(normal_vectors_split[i], self.projector_to_camera_translation_vector[i], n_dot_p, n_dot_p)
planes = cv.CreateMat(rows, cols, cv.CV_32FC4)
cv.Merge(normal_vectors_split[0], normal_vectors_split[1], normal_vectors_split[2], n_dot_p, planes)
return planes
def display_scanline_associations(self, associations):
display_image = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.ConvertScale(associations, display_image, 255.0/self.number_of_scanlines)
cv.NamedWindow("associations", flags=0)
cv.ShowImage("associations", display_image)
cv.WaitKey(800)
class VehicleFilterNode(DTROS):
"""
The vehicle filter node is responsible for estimating the relative pose to a detected back pattern of a robot and
for publishing a stop line message if the other vehicle is too close. We model the vehicle in front as a stop line
and leave the velocity control to the lane control node.
Args:
node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
Configuration:
~distance_between_centers (:obj:`float`): Distance between the centers of the circles on the back pattern, in meters.
~max_reproj_pixelerror_pose_estimation (:obj:`float`): Maximum tolerable reprojection error. If a reprojection error higher than that is observed. A stop message will be published.
~virtual_stop_line_offset (:obj:`int`): Desired distance from the back pattern of the robot in front, in meters.
Subscriber:
~centers (:obj:`duckietown_msgs.msg.VehicleCorners`): Detected pattern (if any)
~camera_info (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera. Needed for rectifying the segments.
Publishers:
~virtual_stop_line (:obj:`duckietown_msgs.msg.StopLineReading`): Message to the lane controller
~debug/visualization_marker (:obj:`visualization_msgs.msg.Marker`): Debug topic that publishes an rViz marker
~stopped (:obj:`BoolStamped`): Message to FSM state machine to identify that vehicle is stopped
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(VehicleFilterNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Add the node parameters to the parameters dictionary and load their default values
self.distance_between_centers = DTParam('~distance_between_centers',
param_type=ParamType.FLOAT)
self.max_reproj_pixelerror_pose_estimation = DTParam(
'~max_reproj_pixelerror_pose_estimation',
param_type=ParamType.FLOAT)
self.virtual_stop_line_offset = DTParam('~virtual_stop_line_offset',
param_type=ParamType.FLOAT)
self.bridge = CvBridge()
# these will be defined on the first call to calc_circle_pattern
self.last_calc_circle_pattern = None
self.circlepattern_dist = None
self.circlepattern = None
self.state = None
# subscribers
self.sub_centers = rospy.Subscriber("~centers",
VehicleCorners,
self.cb_process_centers,
queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.cb_process_camera_info,
queue_size=1)
""" TODO: REMOVE AFTER ROAD_ANOMALY NODE CONSTRUCTION"""
self.sub_state = rospy.Subscriber("~mode", FSMState, self.cbFSMState)
# publishers
self.pub_virtual_stop_line = rospy.Publisher("~virtual_stop_line",
StopLineReading,
queue_size=1)
self.pub_visualize = rospy.Publisher("~debug/visualization_marker",
Marker,
queue_size=1)
self.pub_stopped_flag = rospy.Publisher("~stopped",
BoolStamped,
queue_size=1)
self.pcm = PinholeCameraModel()
self.changePattern = rospy.ServiceProxy("~set_pattern", ChangePattern)
self.log("Initialization completed")
self.last_led_state = None
def cbFSMState(self, msg):
self.state = msg.state
def cb_process_camera_info(self, msg):
"""
Callback that stores the intrinsic calibration into a PinholeCameraModel object.
Args:
msg (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera.
"""
self.pcm.fromCameraInfo(msg)
def cb_process_centers(self, vehicle_centers_msg):
"""
Callback that processes a back pattern detection. If no detection was made, publishes a dummy stop line message.
Args:
vehicle_centers_msg (:obj:`duckietown_msgs.msg.VehicleCorners`): Detected pattern (if any)
"""
# check if there actually was a detection
detection = vehicle_centers_msg.detection.data
if detection:
self.calc_circle_pattern(vehicle_centers_msg.H,
vehicle_centers_msg.W)
points = np.zeros(
(vehicle_centers_msg.H * vehicle_centers_msg.W, 2))
for i in range(len(points)):
points[i] = np.array([
vehicle_centers_msg.corners[i].x,
vehicle_centers_msg.corners[i].y
])
success, rotation_vector, translation_vector = cv2.solvePnP(
objectPoints=self.circlepattern,
imagePoints=points,
cameraMatrix=self.pcm.intrinsicMatrix(),
distCoeffs=self.pcm.distortionCoeffs())
if success:
points_reproj, _ = cv2.projectPoints(
objectPoints=self.circlepattern,
rvec=rotation_vector,
tvec=translation_vector,
cameraMatrix=self.pcm.intrinsicMatrix(),
distCoeffs=self.pcm.distortionCoeffs())
mean_reproj_error = np.mean(
np.sqrt(
np.sum((np.squeeze(points_reproj) - points)**2,
axis=1)))
if mean_reproj_error < self.max_reproj_pixelerror_pose_estimation.value:
(R, jac) = cv2.Rodrigues(rotation_vector)
R_inv = np.transpose(R)
translation_vector = -np.dot(R_inv, translation_vector)
distance_to_vehicle = -translation_vector[2]
# make the message and publish
self.publish_stop_line_msg(
header=vehicle_centers_msg.header,
detected=True,
at=distance_to_vehicle <=
self.virtual_stop_line_offset.value,
x=distance_to_vehicle)
if self.pub_visualize.get_num_connections() > 0:
marker_msg = Marker()
marker_msg.header = vehicle_centers_msg.header
marker_msg.header.frame_id = 'donald'
marker_msg.ns = 'my_namespace'
marker_msg.id = 0
marker_msg.type = Marker.CUBE
marker_msg.action = Marker.ADD
marker_msg.pose.position.x = -translation_vector[2]
marker_msg.pose.position.y = translation_vector[0]
marker_msg.pose.position.z = translation_vector[1]
marker_msg.pose.orientation.x = 0.0
marker_msg.pose.orientation.y = 0.0
marker_msg.pose.orientation.z = 0.0
marker_msg.pose.orientation.w = 1.0
marker_msg.scale.x = 0.1
marker_msg.scale.y = 0.1
marker_msg.scale.z = 0.1
marker_msg.color.r = 1.0
marker_msg.color.g = 1.0
marker_msg.color.b = 0.0
marker_msg.color.a = 1.0
marker_msg.lifetime = rospy.Duration.from_sec(1)
self.pub_visualize.publish(marker_msg)
else:
self.log(
"Pose estimation failed, too high reprojection error. "
"Reporting detection at 0cm for safety.")
self.publish_stop_line_msg(
header=vehicle_centers_msg.header,
detected=True,
at=True)
else:
self.log("Pose estimation failed. "
"Reporting detection at 0cm for safety.")
self.publish_stop_line_msg(header=vehicle_centers_msg.header,
detected=True,
at=True)
else:
# publish empty messages
self.publish_stop_line_msg(header=vehicle_centers_msg.header)
if self.pub_visualize.get_num_connections() > 0:
marker_msg = Marker()
marker_msg.header = vehicle_centers_msg.header
marker_msg.header.frame_id = 'donald'
marker_msg.ns = 'my_namespace'
marker_msg.id = 0
marker_msg.action = Marker.DELETE
self.pub_visualize.publish(marker_msg)
try:
self.trigger_led_hazard_light(
detection,
(distance_to_vehicle <= self.virtual_stop_line_offset.value))
except Exception:
self.trigger_led_hazard_light(detection, False)
def calc_circle_pattern(self, height, width):
"""
Calculates the physical locations of each dot in the pattern.
Args:
height (`int`): number of rows in the pattern
width (`int`): number of columns in the pattern
"""
# check if the version generated before is still valid, if not, or first time called, create
if self.last_calc_circle_pattern is None or self.last_calc_circle_pattern != (
height, width):
self.circlepattern_dist = self.distance_between_centers.value
self.circlepattern = np.zeros([height * width, 3])
for i in range(0, width):
for j in range(0, height):
self.circlepattern[i + j * width, 0] = self.circlepattern_dist * i - \
self.circlepattern_dist * (width - 1) / 2
self.circlepattern[i + j * width, 1] = self.circlepattern_dist * j - \
self.circlepattern_dist * (height - 1) / 2
def trigger_led_hazard_light(self, detection, stopped):
"""
Publish a service message to trigger the hazard light at the back of the robot
"""
msg = String()
if stopped:
msg.data = "OBSTACLE_STOPPED"
if msg.data != self.last_led_state:
self.changePattern(msg)
self.last_led_state = msg.data
elif detection:
msg.data = "OBSTACLE_ALERT"
if msg.data != self.last_led_state:
self.changePattern(msg)
self.last_led_state = msg.data
else:
if self.state == "LANE_FOLLOWING":
msg.data = "CAR_DRIVING"
if msg.data != self.last_led_state:
self.changePattern(msg)
self.last_led_state = "CAR_DRIVING"
elif self.state == "NORMAL_JOYSTICK_CONTROL":
msg.data = "WHITE"
if msg.data != self.last_led_state:
self.changePattern(msg)
self.last_led_state = msg.data
def publish_stop_line_msg(self,
header,
detected=False,
at=False,
x=0.0,
y=0.0):
"""
Makes and publishes a stop line message.
Args:
header: header of a ROS message, usually copied from an incoming message
detected (`bool`): whether a vehicle has been detected
at (`bool`): whether we are closer than the desired distance
x (`float`): distance to the vehicle
y (`float`): lateral offset from the vehicle (not used, always 0)
"""
stop_line_msg = StopLineReading()
stop_line_msg.header = header
stop_line_msg.stop_line_detected = bool(detected)
stop_line_msg.at_stop_line = bool(at)
stop_line_msg.stop_line_point.x = int(x)
stop_line_msg.stop_line_point.y = int(y)
self.pub_virtual_stop_line.publish(stop_line_msg)
"""
Remove once have the road anomaly watcher node
"""
stopped_flag = BoolStamped()
stopped_flag.header = header
stopped_flag.data = bool(at)
self.pub_stopped_flag.publish(stopped_flag)
class PathMarkerFinder():
"""
Node which finds orange path markers in image frame,
estimates the 2d and 3d position/orientation of marker
and returns this estimate when service is called.
Unit tests for this node is in test_path_marker.py
Finding the marker works as follows:
* blur image
* threshold image mostly for highly saturated, orange/yellow/red objects
* run canny edge detection on thresholded image
* find contours on edge frame
* filter contours to find those that may be contours by:
* checking # of sides in appox polygon
* checking ratio of length/width close to known model
* estimates 3D pose using cv2.solvePnP with known object demensions and camera model
TODO: Implement Kalman filter to smooth pose estimate / eliminate outliers
"""
# Model of four corners of path marker, centered around 0 in meters
PATH_MARKER = np.array([[0.6096, -0.0762, 0], [0.6096, 0.0762, 0],
[-0.6096, 0.0762, 0], [-0.6096, -0.0762, 0]],
dtype=np.float)
# Coordinate axes for debugging image
REFERENCE_POINTS = np.array(
[[0, 0, 0], [0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], dtype=np.float)
def __init__(self):
self.debug_gui = False
self.last2d = None
self.last3d = None
self.enabled = False
self.cam = None
self.last_image = None
self.last_found_time = None
# Constants from launch config file
self.debug_ros = rospy.get_param("debug_ros", True)
self.canny_low = rospy.get_param("canny_low", 100)
self.canny_ratio = rospy.get_param("canny_ratio", 3.0)
self.thresh_hue_high = rospy.get_param("thresh_hue_high", 60)
self.thresh_saturation_low = rospy.get_param("thresh_satuation_low",
100)
self.min_contour_area = rospy.get_param("min_contour_area", 100)
self.length_width_ratio_err = rospy.get_param("length_width_ratio_err",
0.2)
self.approx_polygon_thresh = rospy.get_param("approx_polygon_thresh",
10)
camera = rospy.get_param("marker_camera",
"/camera/down/left/image_rect_color")
if self.debug_ros:
self.debug_pub = Image_Publisher("debug_image")
self.markerPub = rospy.Publisher('path_marker_visualization',
Marker,
queue_size=10)
self.service2D = rospy.Service('/vision/path_marker/2D',
VisionRequest2D, self.cb_2d)
self.service3D = rospy.Service('/vision/path_marker/pose',
VisionRequest, self.cb_3d)
self.toggle = rospy.Service('/vision/path_marker/enable', SetBool,
self.enable_cb)
self.image_sub = Image_Subscriber(camera, self.img_cb)
camera_info = self.image_sub.wait_for_camera_info()
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(camera_info)
def sendDebugMarker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the path marker.
This marker is a scaled cube with the demensions and color of the actual marker.
Only called if debug_ros param == True
'''
m = Marker()
m.header.frame_id = self.cam.tfFrame()
m.header.stamp = self.last_found_time
m.ns = "path_markers"
m.id = 0
m.type = 1
m.action = 0
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
# Real demensions of path marker
m.scale.x = 1.2192
m.scale.y = 0.1524
m.scale.z = 0.05
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(0)
self.markerPub.publish(m)
def enable_cb(self, x):
self.enabled = x.data
return SetBoolResponse(success=True)
def sortRect(self, rect):
'''
Given a contour of 4 points, returns the same 4 points sorted in a known way.
Used so that indicies of contour line up to that in model for cv2.solvePnp
p[0] = Top left corner of marker
p[1] = Top right corner of marker
p[2] = Bottom right corner of marker
p[3] = Bottom left cornet of marker
'''
def sort_contour_y(c):
return c[0][1]
def sort_contour_x(c):
return c[0][0]
sorted_y = np.array(sorted(rect, key=sort_contour_y))
sorted_x = np.array(sorted(rect, key=sort_contour_x))
sorted_rect = None
if (np.linalg.norm(sorted_y[0] - sorted_y[1]) >
np.linalg.norm(sorted_y[0] - sorted_y[2])
or np.linalg.norm(sorted_y[0] - sorted_y[1]) >
np.linalg.norm(sorted_y[0] - sorted_y[3])):
if sorted_x[1][0][1] > sorted_x[0][0][1]:
sorted_x[0], sorted_x[1] = sorted_x[1].copy(
), sorted_x[0].copy()
if sorted_x[2][0][1] > sorted_x[3][0][1]:
sorted_x[2], sorted_x[3] = sorted_x[3].copy(
), sorted_x[2].copy()
sorted_rect = sorted_x
else:
if sorted_y[0][0][0] > sorted_y[1][0][0]:
sorted_y[0], sorted_y[1] = sorted_y[1].copy(
), sorted_y[0].copy()
if sorted_y[3][0][0] > sorted_y[2][0][0]:
sorted_y[3], sorted_y[2] = sorted_y[2].copy(
), sorted_y[3].copy()
sorted_rect = sorted_y
return sorted_rect
def cb_3d(self, req):
res = VisionRequestResponse()
if (self.last2d == None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = self.cam.tfFrame()
res.pose.header.stamp = self.last_found_time
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def cb_2d(self, req):
res = VisionRequest2DResponse()
if (self.last3d == None or not self.enabled):
res.found = False
else:
res.header.frame_id = self.cam.tfFrame()
res.header.stamp = self.last_found_time
res.pose.x = self.last2d[0][0]
res.pose.y = self.last2d[0][1]
res.pose.theta = self.last2d[1]
res.found = True
return res
def get_3d_pose(self, p):
i_points = np.array((p[0][0], p[1][0], p[2][0], p[3][0]),
dtype=np.float)
retval, rvec, tvec = cv2.solvePnP(PathMarkerFinder.PATH_MARKER,
i_points, self.cam.intrinsicMatrix(),
np.zeros((5, 1)))
if tvec[2] < 0.3:
return False
self.last3d = (tvec.copy(),
quaternion_from_euler(0.0, 0.0, self.last2d[1]))
if self.debug_ros:
refs, _ = cv2.projectPoints(PathMarkerFinder.REFERENCE_POINTS,
rvec, tvec, self.cam.intrinsicMatrix(),
np.zeros((5, 1)))
refs = np.array(refs, dtype=np.int)
cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]),
(refs[1][0][0], refs[1][0][1]),
(0, 0, 255)) # X axis refs
cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]),
(refs[2][0][0], refs[2][0][1]), (0, 255, 0)) # Y axis ref
cv2.line(self.last_image, (refs[0][0][0], refs[0][0][1]),
(refs[3][0][0], refs[3][0][1]), (255, 0, 0)) # Z axis ref
return True
def get_2d_pose(self, r):
'''
Given a sorted 4 sided polygon, stores the centriod and angle
for the next service call to get 2dpose.
returns False if dx of polygon is 0, otherwise True
'''
top_center = r[0][0] + (r[1][0] - r[0][0]) / 2.0
bot_center = r[2][0] + (r[3][0] - r[2][0]) / 2.0
center = bot_center + (top_center - bot_center) / 2.0
y = top_center[1] - bot_center[1]
x = top_center[0] - bot_center[0]
if x == 0:
rospy.logerr("Contour dx is 0, strange...")
return False
angle = np.arctan(y / x)
self.last2d = (center, angle)
return True
def valid_contour(self, contour):
'''
Does various tests to filter out contours that are clearly not
a valid path marker.
* run approx polygon, check that sides == 4
* find ratio of length to width, check close to known ratio IRL
'''
if cv2.contourArea(contour) < self.min_contour_area:
return False
# Checks that contour is 4 sided
polygon = cv2.approxPolyDP(contour, self.approx_polygon_thresh, True)
if len(polygon) != 4:
return False
rect = self.sortRect(polygon)
for idx, p in enumerate(rect):
cv2.putText(self.last_image, str(idx), (p[0][0], p[0][1]),
cv2.FONT_HERSHEY_SCRIPT_COMPLEX, 1, (0, 0, 255))
length = np.linalg.norm(rect[0][0] - rect[3][0])
width = np.linalg.norm(rect[0][0] - rect[1][0])
if width == 0:
rospy.logerr("Width == 0, strange...")
return False
length_width_ratio = length / width
# Checks that ratio of length to width is similar to known demensions (8:1)
if abs(length_width_ratio - 8.0) / 8.0 > self.length_width_ratio_err:
return False
if not self.get_2d_pose(rect):
return False
if not self.get_3d_pose(rect):
return False
return True
def get_edges(self):
'''
Proccesses latest image to find edges by:
blurring and thresholding for highly saturated orangish objects
then runs canny on threshold images and returns canny's edges
'''
blur = cv2.blur(self.last_image, (5, 5))
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
thresh = cv2.inRange(hsv, (0, self.thresh_saturation_low, 0),
(self.thresh_hue_high, 255, 255))
return cv2.Canny(thresh, self.canny_low,
self.canny_low * self.canny_ratio)
def img_cb(self, img):
if not self.enabled or self.cam == None:
return
self.last_image = img
edges = self.get_edges()
_, contours, _ = cv2.findContours(edges, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# Check if each contour is valid
for idx, c in enumerate(contours):
if self.valid_contour(c):
rospy.loginfo("Found path marker")
self.last_found_time = self.image_sub.last_image_time
if self.debug_ros:
self.sendDebugMarker()
cv2.drawContours(self.last_image, contours, idx,
(0, 255, 0), 3)
break
else:
if self.debug_ros:
cv2.drawContours(self.last_image, contours, idx,
(255, 0, 0), 3)
if self.debug_ros:
self.debug_pub.publish(self.last_image)
if self.debug_gui:
cv2.imshow("debug", self.last_image)
cv2.waitKey(5)
def get_point_cloud(self):
# Scan the scene
pixel_associations = self.get_pixel_associations()
# Project white light onto the scene to get the intensities of each picture (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(
illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection,
encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
projector_model = PinholeCameraModel()
projector_model.fromCameraInfo(self.projector_info)
projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector,
projector_to_camera_rotation_matrix)
pixel_associations_x = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
pixel_associations_y = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
cv.Split(pixel_associations, pixel_associations_x,
pixel_associations_y, None, None)
valid_points_mask_x = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_x, -1, valid_points_mask_x, cv.CV_CMP_NE)
valid_points_mask_y = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_y, -1, valid_points_mask_y, cv.CV_CMP_NE)
valid_points_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.And(valid_points_mask_x, valid_points_mask_y, valid_points_mask)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC2)
rectified_projector_coordinates = cv.CreateMat(1,
number_of_valid_points,
cv.CV_32FC2)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
rectified_projector_coordinates[0, i] = pixel_associations[
row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates,
rectified_camera_coordinates,
camera_model.intrinsicMatrix(),
camera_model.distortionCoeffs())
# Convert scanline numbers to pixel numbers
cv.AddS(rectified_projector_coordinates, 0.5,
rectified_projector_coordinates)
cv.ConvertScale(rectified_projector_coordinates,
rectified_projector_coordinates,
self.pixels_per_scanline)
# Rectify the projector pixels
cv.UndistortPoints(rectified_projector_coordinates,
rectified_projector_coordinates,
projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs())
camera_rays = projectPixelsTo3dRays(rectified_camera_coordinates,
camera_model)
projector_rays = projectPixelsTo3dRays(rectified_projector_coordinates,
projector_model)
# Bring the projector rays into camera coordinates
cv.Transform(projector_rays, projector_rays,
projector_to_camera_rotation_matrix)
camera_centers = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC3)
cv.SetZero(camera_centers)
projector_centers = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC3)
cv.Set(projector_centers, self.projector_to_camera_translation_vector)
intersection_points = line_line_intersections(camera_centers,
camera_rays,
projector_centers,
projector_rays)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
class VehicleFilterNode(object):
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.distance_between_centers = self.setupParam(
'distance_between_centers', 0.0125)
self.max_reproj_pixelerror_pose_estimation = self.setupParam(
'max_reproj_pixelerror_pose_estimation', 1.5)
self.sub_corners = rospy.Subscriber("~corners",
VehicleCorners,
self.processCorners,
queue_size=1)
self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.processCameraInfo,
queue_size=1)
self.pub_time_elapsed = rospy.Publisher("~pose_calculation_time",
Float32,
queue_size=1)
self.pcm = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
self.lock = mutex()
def setupParam(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name, value)
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def processCameraInfo(self, camera_info_msg):
if self.lock.testandset():
self.pcm.fromCameraInfo(camera_info_msg)
self.lock.unlock()
def processCorners(self, vehicle_corners_msg):
# do nothing - just relay the detection
if self.lock.testandset():
start = rospy.Time.now()
# print(start)
self.calcCirclePattern(vehicle_corners_msg.H,
vehicle_corners_msg.W)
points = []
for Point32 in vehicle_corners_msg.corners:
point = [Point32.x, Point32.y]
points.append(point)
points = np.array(points)
# points = np.reshape(points, (2,-1))
# print(points)
# print(self.pcm.distortionCoeffs())
(success, rotation_vector,
translation_vector) = cv2.solvePnP(self.circlepattern, points,
self.pcm.intrinsicMatrix(),
self.pcm.distortionCoeffs())
if success:
points_reproj, _ = cv2.projectPoints(
self.circlepattern, rotation_vector, translation_vector,
self.pcm.intrinsicMatrix(), self.pcm.distortionCoeffs())
error = 0
for i in range(0, len(points_reproj)):
error += cv2.norm(points[i], points_reproj[i, 0],
cv2.NORM_L2)
mean_reproj_error = error / len(points_reproj)
# print(mean_reproj_error)
# print(self.max_reproj_pixelerror_pose_estimation)
if mean_reproj_error < self.max_reproj_pixelerror_pose_estimation:
# print(translation_vector)
(R, jac) = cv2.Rodrigues(rotation_vector)
R_inv = np.transpose(R)
translation_vector = -np.dot(R_inv, translation_vector)
pose_msg_out = VehiclePose()
pose_msg_out.header.stamp = rospy.Time.now()
pose_msg_out.rho.data = sqrt(translation_vector[2]**2 +
translation_vector[0]**2)
pose_msg_out.psi.data = np.arctan2(
-R_inv[2, 0], sqrt(R_inv[2, 1]**2 + R_inv[2, 2]**2))
pose_msg_out.detection.data = vehicle_corners_msg.detection.data
R2 = np.array([[
cos(pose_msg_out.psi.data), -sin(pose_msg_out.psi.data)
], [
sin(pose_msg_out.psi.data),
cos(pose_msg_out.psi.data)
]])
translation_vector = - \
np.array([translation_vector[2],
translation_vector[0]])
translation_vector = np.dot(np.transpose(R2),
translation_vector)
pose_msg_out.theta.data = np.arctan2(
translation_vector[1], translation_vector[0])
self.pub_pose.publish(pose_msg_out)
else:
rospy.loginfo(
"[%s] Pose estimation failed, too high reprojection error."
% (self.node_name))
else:
rospy.loginfo("[%s] Pose estimation failed." %
(self.node_name))
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
self.lock.unlock()
return
def calcCirclePattern(self, height, width):
self.circlepattern_dist = self.distance_between_centers
self.circlepattern = np.zeros([height * width, 3])
for i in range(0, width):
for j in range(0, height):
self.circlepattern[i + j * width, 0] = self.circlepattern_dist * \
i - self.circlepattern_dist * (width - 1) / 2
self.circlepattern[i + j * width, 1] = self.circlepattern_dist * \
j - self.circlepattern_dist * (height - 1) / 2
def get_point_cloud(self):
# Scan the scene
pixel_associations = self.get_pixel_associations()
# Project white light onto the scene to get the intensities of each picture (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
projector_model = PinholeCameraModel()
projector_model.fromCameraInfo(self.projector_info)
projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector, projector_to_camera_rotation_matrix)
pixel_associations_x = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
pixel_associations_y = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
cv.Split(pixel_associations, pixel_associations_x, pixel_associations_y, None, None)
valid_points_mask_x = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_x, -1, valid_points_mask_x, cv.CV_CMP_NE)
valid_points_mask_y = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_y, -1, valid_points_mask_y, cv.CV_CMP_NE)
valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.And(valid_points_mask_x, valid_points_mask_y, valid_points_mask)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
rectified_projector_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0;
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
rectified_projector_coordinates[0, i] = pixel_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())
# Convert scanline numbers to pixel numbers
cv.AddS(rectified_projector_coordinates, 0.5, rectified_projector_coordinates)
cv.ConvertScale(rectified_projector_coordinates, rectified_projector_coordinates, self.pixels_per_scanline)
# Rectify the projector pixels
cv.UndistortPoints(rectified_projector_coordinates, rectified_projector_coordinates, projector_model.intrinsicMatrix(), projector_model.distortionCoeffs())
camera_rays = projectPixelsTo3dRays(rectified_camera_coordinates, camera_model)
projector_rays = projectPixelsTo3dRays(rectified_projector_coordinates, projector_model)
# Bring the projector rays into camera coordinates
cv.Transform(projector_rays, projector_rays, projector_to_camera_rotation_matrix)
camera_centers = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC3)
cv.SetZero(camera_centers)
projector_centers = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC3)
cv.Set(projector_centers, self.projector_to_camera_translation_vector)
intersection_points = line_line_intersections(camera_centers, camera_rays, projector_centers, projector_rays)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
class BackTrackingNode(object):
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.active = True
self.currentImg = None
self.publish_duration = rospy.Duration.from_sec(1.0 / 2.0)
self.last_stamp = rospy.Time.now()
#Subscribers and publishers
self.imagesub = rospy.Subscriber("~image",
CompressedImage,
self.find_marker,
buff_size=921600,
queue_size=1)
self.movepub = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=10)
self.imagepub = rospy.Publisher("~image", Image, queue_size=1)
self.infosub = rospy.Subscriber("~camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
self.camerainfo = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
#get camera info for pinhole camera model
def get_camera_info(self, camera_msg):
self.camerainfo.fromCameraInfo(camera_msg)
#step 1 : find the back circle grids using cv2.findCirclesGrid
##### set (x,y) for points and flag for detection
def find_marker(self, image_msg):
if not self.active:
return
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
try:
self.currentImg = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print e
gray = cv2.cvtColor(currentImg, cv2.COLOR_BGR2GRAY)
detection, corners = cv2.findCirclesGrid(gray, (7, 3))
copy = currentImg.copy()
if detection:
cv2.drawChessboardCorners(copy, (7, 3), corners, detection)
twoone = []
for i in range(0, 21):
point = [corners[i][0][0], corners[i][0][1]]
twoone.append(point)
twoone = np.array(twoone)
self.originalmatrix()
detection, rvec, tvec = self.gradient(twoone)
distance(copy, tvec)
cv2.imshow('detection', copy)
cv2.wait(1)
#alternative step : find the back circle grids using double thresholds blob detector
##### set (x,y) for points and flag for detection
# def find_marker2(self, image_msg):
# if not self.active:
# return
#step 2 : makes matrix for 3d original shape
def originalmatrix(self):
#coners and points
self.originalmtx = np.zeros([21, 3])
for i in range(0, 7):
for j in range(0, 3):
self.originalmtx[i + j * 7, 0] = 0.0125 * i - 0.0125 * 3
self.originalmtx[i + j * 7, 1] = 0.0125 * j - 0.0125
#step 3 : use intrinsic matrix and solvePnP, return rvec and tvec, print (show) norm
def gradient(self, imgpts):
#using solvePnP to find rotation vector and translation vector
works, rvec, tvec = cv2.solvePnP(self.originalmtx, imgpts,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
return works, rvec, tvec
class VehicleFilterNode(object):
def __init__(self):
self.node_name = rospy.get_name()
self.bridge = CvBridge()
self.config = self.setupParam("~config", "baseline")
self.cali_file_name = self.setupParam("~cali_file_name", "default")
rospack = rospkg.RosPack()
self.cali_file = rospack.get_path('duckietown') + \
"/config/" + self.config + \
"/vehicle_detection/vehicle_filter_node/" + \
self.cali_file_name + ".yaml"
if not os.path.isfile(self.cali_file):
rospy.logwarn("[%s] Can't find calibration file: %s.\n" %
(self.node_name, self.cali_file))
self.loadConfig(self.cali_file)
self.sub_corners = rospy.Subscriber("~corners",
VehicleCorners,
self.cbCorners,
queue_size=1)
self.pub_pose = rospy.Publisher("~pose", VehiclePose, queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.cbCameraInfo,
queue_size=1)
self.pub_time_elapsed = rospy.Publisher("~pose_calculation_time",
Float32,
queue_size=1)
self.pcm = PinholeCameraModel()
rospy.loginfo("[%s] Initialization completed" % (self.node_name))
self.lock = mutex()
def setupParam(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name, value)
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def loadConfig(self, filename):
stream = file(filename, 'r')
data = yaml.load(stream)
stream.close()
self.distance_between_centers = data['distance_between_centers']
rospy.loginfo('[%s] distance_between_centers dim : %s' %
(self.node_name, self.distance_between_centers))
self.max_reproj_pixelerror_pose_estimation = data[
'max_reproj_pixelerror_pose_estimation']
rospy.loginfo(
'[%s] max_reproj_pixelerror_pose_estimation : %s' %
(self.node_name, self.max_reproj_pixelerror_pose_estimation))
def cbCameraInfo(self, camera_info_msg):
thread = threading.Thread(target=self.processCameraInfo,
args=(camera_info_msg, ))
thread.setDaemon(True)
thread.start()
def processCameraInfo(self, camera_info_msg):
if self.lock.testandset():
self.pcm.fromCameraInfo(camera_info_msg)
self.lock.unlock()
def cbCorners(self, vehicle_corners_msg):
# Start a daemon thread to process the image
thread = threading.Thread(target=self.processCorners,
args=(vehicle_corners_msg, ))
thread.setDaemon(True)
thread.start()
# Returns rightaway
def processCorners(self, vehicle_corners_msg):
# do nothing - just relay the detection
if self.lock.testandset():
start = rospy.Time.now()
#print(start)
self.calcCirclePattern(vehicle_corners_msg.H,
vehicle_corners_msg.W)
points = []
for Point32 in vehicle_corners_msg.corners:
point = [Point32.x, Point32.y]
points.append(point)
points = np.array(points)
# points = np.reshape(points, (2,-1))
# print(points)
# print(self.pcm.distortionCoeffs())
(success, rotation_vector,
translation_vector) = cv2.solvePnP(self.circlepattern, points,
self.pcm.intrinsicMatrix(),
self.pcm.distortionCoeffs())
if success:
points_reproj, _ = cv2.projectPoints(
self.circlepattern, rotation_vector, translation_vector,
self.pcm.intrinsicMatrix(), self.pcm.distortionCoeffs())
error = 0
for i in range(0, len(points_reproj)):
error += cv2.norm(points[i], points_reproj[i, 0],
cv2.NORM_L2)
mean_reproj_error = error / len(points_reproj)
#print(mean_reproj_error)
#print(self.max_reproj_pixelerror_pose_estimation)
if mean_reproj_error < self.max_reproj_pixelerror_pose_estimation:
# print(translation_vector)
(R, jac) = cv2.Rodrigues(rotation_vector)
R_inv = np.transpose(R)
translation_vector = -np.dot(R_inv, translation_vector)
pose_msg_out = VehiclePose()
pose_msg_out.header.stamp = rospy.Time.now()
pose_msg_out.rho.data = sqrt(translation_vector[2]**2 +
translation_vector[0]**2)
pose_msg_out.psi.data = np.arctan2(
-R_inv[2, 0], sqrt(R_inv[2, 1]**2 + R_inv[2, 2]**2))
pose_msg_out.detection.data = vehicle_corners_msg.detection.data
R2 = np.array([[
cos(pose_msg_out.psi.data), -sin(pose_msg_out.psi.data)
], [
sin(pose_msg_out.psi.data),
cos(pose_msg_out.psi.data)
]])
translation_vector = -np.array(
[translation_vector[2], translation_vector[0]])
translation_vector = np.dot(np.transpose(R2),
translation_vector)
pose_msg_out.theta.data = np.arctan2(
translation_vector[1], translation_vector[0])
self.pub_pose.publish(pose_msg_out)
else:
rospy.loginfo(
"[%s] Pose estimation failed, too high reprojection error."
% (self.node_name))
else:
rospy.loginfo("[%s] Pose estimation failed." %
(self.node_name))
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
self.lock.unlock()
return
def calcCirclePattern(self, height, width):
self.circlepattern_dist = self.distance_between_centers
self.circlepattern = np.zeros([height * width, 3])
for i in range(0, width):
for j in range(0, height):
self.circlepattern[
i + j * width,
0] = self.circlepattern_dist * i - self.circlepattern_dist * (
width - 1) / 2
self.circlepattern[
i + j * width,
1] = self.circlepattern_dist * j - self.circlepattern_dist * (
height - 1) / 2
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(
illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection,
encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates,
rectified_camera_coordinates,
camera_model.intrinsicMatrix(),
camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates,
camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
class MyNode(DTROS):
def __init__(self, node_name):
# initialize the DTROS parent class
super(MyNode, self).__init__(node_name=node_name)
# construct publisher and subsriber
self.pub = rospy.Publisher('chatter', String, queue_size=10)
self.sub_image = rospy.Subscriber(
"/duckiesam/camera_node/image/compressed",
CompressedImage,
self.find_marker,
buff_size=921600,
queue_size=1)
self.pub_image = rospy.Publisher('~image', Image, queue_size=1)
self.sub_info = rospy.Subscriber("/duckiesam/camera_node/camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
self.pub_move = rospy.Publisher('/duckiesam/joy_mapper_node/car_cmd',
Twist2DStamped,
queue_size=1)
#values for detecting marker
self.starting = 0
self.ending = 0
self.camerainfo = PinholeCameraModel()
self.bridge = CvBridge()
self.gotimage = False
self.imagelast = None
self.processedImg = None
self.detected = False
#values for calculating pose of robot
self.solP = False
self.rotationvector = None
self.translationvector = None
self.axis = np.float32([[0.0125, 0, 0], [0, 0.0125, 0],
[0, 0, -0.0375]]).reshape(-1, 3)
self.distance = None
self.angle_f = None
self.angle_l = None
#values for driving the robot
self.maxdistance = 0.2
self.speedN = 0
self.e_vB = 0
self.rotationN = 0
self.mindistance = 0.1
self.d_e = 0 #distance error
self.d_e_1 = 5
self.d_e_2 = 10
self.y2 = 0
self.controltime = rospy.Time.now()
self.Kp = 1
self.Ki = 0.1
self.Kd = 0
self.I = 0
self.Rp = 1
self.Ri = 1
self.Rd = 1
#get camera info for pinhole camera model
def get_camera_info(self, camera_msg):
self.camerainfo.fromCameraInfo(camera_msg)
#step 1 : find the back circle grids using cv2.findCirclesGrid
##### set (x,y) for points and flag for detection
def find_marker(self, image_msg):
try:
self.imagelast = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print(e)
if self.gotimage == False:
self.gotimage = True
#time checking
self.starting = rospy.Time.now()
#from_last_image = (self.starting - self.ending).to_sec()
gray = cv2.cvtColor(self.imagelast, cv2.COLOR_BGR2GRAY)
detection, corners = cv2.findCirclesGrid(gray, (7, 3))
self.processedImg = self.imagelast.copy()
cmd = Twist2DStamped()
cmd.header.stamp = self.starting
if detection:
cv2.drawChessboardCorners(self.processedImg, (7, 3), corners,
detection)
self.detected = True
#self.controltime = rospy.Time.now()
twoone = []
for i in range(0, 21):
point = [corners[i][0][0], corners[i][0][1]]
twoone.append(point)
twoone = np.array(twoone)
self.originalmatrix()
self.gradient(twoone)
self.detected = self.solP
img_out = self.bridge.cv2_to_imgmsg(self.processedImg, "bgr8")
self.pub_image.publish(img_out)
self.find_distance()
self.move(self.y2, self.angle_l, self.distance)
self.ending = rospy.Time.now()
else:
self.detected = False
img_out = self.bridge.cv2_to_imgmsg(gray, "bgr8")
self.pub_image.publish(img_out)
self.ending = rospy.Time.now()
cmd.v = 0
cmd.omega = 0
self.pub_move.publish(cmd)
#step 2 : makes matrix for 3d original shape
def originalmatrix(self):
#coners and points
self.originalmtx = np.zeros([21, 3])
for i in range(0, 7):
for j in range(0, 3):
self.originalmtx[i + j * 7, 0] = 0.0125 * i - 0.0125 * 3
self.originalmtx[i + j * 7, 1] = 0.0125 * j - 0.0125
#step 3 : use intrinsic matrix and solvePnP, return rvec and tvec, print axis
def gradient(self, imgpts):
#using solvePnP to find rotation vector and translation vector and also find 3D point to the image plane
self.solP, self.rotationvector, self.translationvector = cv2.solvePnP(
self.originalmtx, imgpts, self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
if self.solP:
pointsin3D, jacoB = cv2.projectPoints(
self.originalmtx, self.rotationvector, self.translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
pointaxis, _ = cv2.projectPoints(
self.axis, self.rotationvector, self.translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[0].ravel()),
(255, 0, 0), 2)
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[1].ravel()),
(0, 255, 0), 2)
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[2].ravel()),
(0, 0, 255), 3)
#textdistance = "x = %s, y = %s, z = %s" % (self.distance, self.angle_f, self.angle_l, self.y2)
#rospy.loginfo("%s" % textdistance)
#step 4 : find distance between robot and following robot print out distance and time
def find_distance(self):
#use tvec to calculate distance
tvx = self.translationvector[0]
tvy = self.translationvector[1]
tvz = self.translationvector[2]
self.distance = math.sqrt(tvx * tvx + tvz * tvz)
self.angle_f = np.arctan2(tvx[0], tvz[0])
R, _ = cv2.Rodrigues(self.rotationvector)
R_inverse = np.transpose(R)
self.angle_l = np.arctan2(
-R_inverse[2, 0],
math.sqrt(R_inverse[2, 1]**2 + R_inverse[2, 2]**2))
T = np.array([-np.sin(self.angle_l), np.cos(self.angle_l)])
tvecW = -np.dot(R_inverse, self.translationvector)
x_y = np.array([tvz[0], tvx[0]])
self.y2 = -np.dot(T, x_y) - 0.01 * np.sin(self.angle_l)
textdistance = "Distance = %s, Angle of Follower = %s, Angle of Leader = %s, y = %s" % (
self.distance, self.angle_f, self.angle_l, self.y2)
rospy.loginfo("%s" % textdistance)
#self.pub.publish(textdistance)
#step 5 : use joy mapper to control the robot PID controller
def move(self, y_to, angle_to, d):
#y_to is needed y value to be parallel to leader's center line
#angle_to is angle needed to rotate
#d is distance between required position and now position
cmd = Twist2DStamped()
time = rospy.Time.now()
cmd.header.stamp = time
dt = (time - self.controltime).to_sec()
if dt > 3:
if d < self.maxdistance:
cmd.v = 0
cmd.omega = 0
else:
self.d_e = d - self.maxdistance
error_d = (self.d_e - self.d_e_1) / dt
errorB = (self.d_e_1 - self.d_e_2) / dt
e_v = error_d - errorB
P = self.Kp * (e_v)
self.I = self.I + self.Ki * (e_v + self.e_vB) / 2 * dt
D = self.Kd * (e_v + self.e_vB) / dt
self.speedN = P + self.I + D
self.rotationN = self.Rp * (y_to) + self.Ri * (
angle_to) + self.Rd * (np.sin(angle_to))
cmd.v = self.speedN
cmd.omega = self.rotationN
self.e_vB = e_v
self.d_e_2 = self.d_e_1
self.d_e_1 = self.d_e
if self.d_e < 0.05 or self.speedN < 0:
cmd.v = 0
cmd.omega = 0
textdistance = "Velocity = %s, Rotation = %s" % (cmd.v, cmd.omega)
rospy.loginfo("%s" % textdistance)
self.pub_move.publish(cmd)
self.controltime = time
class LaneControllerNode(DTROS):
"""Computes control action.
The node compute the commands in form of linear and angular velocitie.
The configuration parameters can be changed dynamically while the node is running via ``rosparam set`` commands.
Args:
node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
Configuration:
Publisher:
~car_cmd (:obj:`Twist2DStamped`): The computed control action
Subscribers:
~lane_pose (:obj:`LanePose`): The lane pose estimate from the lane filter
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LaneControllerNode, self).__init__(node_name=node_name,
node_type=NodeType.CONTROL)
# Add the node parameters to the parameters dictionary
self.params = dict()
self.pp_controller = PurePursuitLaneController(self.params)
# Construct publishers
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1,
dt_topic_type=TopicType.CONTROL)
# Construct subscribers
# self.sub_lane_reading = rospy.Subscriber("~lane_pose",
# LanePose,
# self.cbLanePoses,
# queue_size=1)
# self.sub_image = rospy.Subscriber(f"/{os.environ['VEHICLE_NAME']}/camera_node/image/compressed", CompressedImage, self.cb_image, queue_size=1)
self.sub_image = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/camera_node/image/compressed",
CompressedImage,
self.cb_image,
queue_size=1)
self.sub_info = rospy.Subscriber(
f"/{os.environ['VEHICLE_NAME']}/camera_node/camera_info",
CameraInfo,
self.cb_process_camera_info,
queue_size=1)
self.log("Initialized!")
self.last_v = 0.2
self.last_w = 0
self.bridge = CvBridge()
self.last_stamp = rospy.Time.now()
self.pcm = PinholeCameraModel()
self.pose_estimator = PoseEstimator(
min_area=CIRCLE_MIN_AREA,
min_dist_between_blobs=CIRCLE_MIN_DISTANCE,
height=CIRCLE_PATTERN_HEIGHT,
width=CIRCLE_PATTERN_WIDTH,
target_distance=TARGET_DIST,
camera_mode=CAMERA_MODE,
)
self.trajectory = Trajectory()
def cb_process_camera_info(self, msg):
"""
Callback that stores the intrinsic calibration into a PinholeCameraModel object.
Args:
msg (:obj:`sensor_msgs.msg.CameraInfo`): Intrinsic properties of the camera.
"""
self.pcm.fromCameraInfo(msg)
self.pose_estimator.initialize_camera_matrix(
self.pcm.intrinsicMatrix(), self.pcm.distortionCoeffs())
def cb_image(self, image_msg):
"""Callback receiving pose messages
Args:
input_pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
"""
now = rospy.Time.now()
dt = (now - self.last_stamp).to_sec()
self.last_stamp = now
if self.pose_estimator.initialized:
image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
target_detected, estimated_pose = self.pose_estimator.get_pose(
image_cv)
# print(estimated_pose)
if target_detected:
self.trajectory.update(estimated_pose)
elif self.trajectory.is_initialized():
self.trajectory.predict(dt)
if self.trajectory.is_initialized():
v, w = self.trajectory.get_commands()
car_control_msg = Twist2DStamped()
car_control_msg.header = image_msg.header
car_control_msg.v = v
car_control_msg.omega = w
self.publishCmd(car_control_msg)
# def cbLanePoses(self, input_pose_msg):
# """Callback receiving pose messages
#
# Args:
# input_pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
# """
# self.pose_msg = input_pose_msg
#
# car_control_msg = Twist2DStamped()
# car_control_msg.header = self.pose_msg.header
#
# # TODO This needs to get changed
#
# v, w = self.pp_controller.pure_pursuit(self.pose_msg.d, self.pose_msg.phi, self.last_v, self.last_w,
# self.vehicle_detection_msg)
#
# self.last_v = v
# self.last_w = w
#
# car_control_msg.v = v
# car_control_msg.omega = w
#
# self.publishCmd(car_control_msg)
def publishCmd(self, car_cmd_msg):
"""Publishes a car command message.
Args:
car_cmd_msg (:obj:`Twist2DStamped`): Message containing the requested control action.
"""
self.pub_car_cmd.publish(car_cmd_msg)
def cbParametersChanged(self):
"""Updates parameters in the controller object."""
self.controller.update_parameters(self.params)
class LandmarkMethod(object):
def __init__(self):
self.subjects = dict()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.model_size_rescale = 30.0
self.head_pitch = 0.0
self.margin = rospy.get_param("~margin", 42)
self.margin_eyes_height = rospy.get_param("~margin_eyes_height", 36)
self.margin_eyes_width = rospy.get_param("~margin_eyes_width", 60)
self.interpupillary_distance = 0.058
self.cropped_face_size = (rospy.get_param("~face_size_height", 224),
rospy.get_param("~face_size_width", 224))
self.gpu_id = rospy.get_param("~gpu_id", default="0")
torch.cuda.set_device(self.gpu_id)
rospy.loginfo("Using GPU for landmark: {}".format(self.gpu_id))
self.rgb_frame_id = rospy.get_param("~rgb_frame_id", "/kinect2_link")
self.rgb_frame_id_ros = rospy.get_param("~rgb_frame_id_ros",
"/kinect2_nonrotated_link")
self.model_points = None
self.eye_image_size = (rospy.get_param("~eye_image_height", 36),
rospy.get_param("~eye_image_width", 60))
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.use_previous_headpose_estimate = True
self.last_rvec = {}
self.last_tvec = {}
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
if np.array_equal(
self.img_proc.intrinsicMatrix(),
np.matrix([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception(
'Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?'
)
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images",
MSG_SubjectImagesList,
queue_size=1)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces",
Image,
queue_size=1)
self.model_points = self._get_full_model_points()
self.sess_bb = None
self.face_net = FaceDetector(device="cuda:{}".format(self.gpu_id))
self.color_sub = rospy.Subscriber("/image",
Image,
self.callback,
buff_size=2**24,
queue_size=1)
self.facial_landmark_nn = face_alignment.FaceAlignment(
landmarks_type=face_alignment.LandmarksType._2D,
device="cuda:{}".format(self.gpu_id),
flip_input=False)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
def _dyn_reconfig_callback(self, config, level):
self.model_points /= (self.model_size_rescale *
self.interpupillary_distance)
self.model_size_rescale = config["model_size"]
self.interpupillary_distance = config["interpupillary_distance"]
self.model_points *= (self.model_size_rescale *
self.interpupillary_distance)
self.head_pitch = config["head_pitch"]
return config
def _get_full_model_points(self):
"""Get all 68 3D model points from file"""
raw_value = []
filename = rospkg.RosPack().get_path(
'rt_gene') + '/model_nets/face_model_68.txt'
with open(filename) as f:
for line in f:
raw_value.append(line)
model_points = np.array(raw_value, dtype=np.float32)
model_points = np.reshape(model_points, (3, -1)).T
# model_points *= 4
model_points[:, -1] *= -1
# index the expansion of the model based.
model_points = model_points * (self.interpupillary_distance *
self.model_size_rescale)
return model_points
def get_face_bb(self, image):
faceboxes = []
start_time = time.time()
fraction = 4
image = scipy.misc.imresize(image, 1.0 / fraction)
detections = self.face_net.detect_from_image(image)
tqdm.write("Face Detector Frequency: {:.2f}Hz".format(
1 / (time.time() - start_time)))
for result in detections:
x_left_top, y_left_top, x_right_bottom, y_right_bottom, confidence = result * fraction
if confidence > 0.8 * fraction:
box = [x_left_top, y_left_top, x_right_bottom, y_right_bottom]
diff_height_width = (box[3] - box[1]) - (box[2] - box[0])
offset_y = int(abs(diff_height_width / 2))
box_moved = self.move_box(box, [0, offset_y])
# Make box square.
facebox = self.get_square_box(box_moved)
faceboxes.append(facebox)
return faceboxes
@staticmethod
def move_box(box, offset):
"""Move the box to direction specified by vector offset"""
left_x = box[0] + offset[0]
top_y = box[1] + offset[1]
right_x = box[2] + offset[0]
bottom_y = box[3] + offset[1]
return [left_x, top_y, right_x, bottom_y]
@staticmethod
def box_in_image(box, image):
"""Check if the box is in image"""
rows = image.shape[0]
cols = image.shape[1]
return box[0] >= 0 and box[1] >= 0 and box[2] <= cols and box[3] <= rows
@staticmethod
def get_square_box(box):
"""Get a square box out of the given box, by expanding it."""
left_x = box[0]
top_y = box[1]
right_x = box[2]
bottom_y = box[3]
box_width = right_x - left_x
box_height = bottom_y - top_y
# Check if box is already a square. If not, make it a square.
diff = box_height - box_width
delta = int(abs(diff) / 2)
if diff == 0: # Already a square.
return box
elif diff > 0: # Height > width, a slim box.
left_x -= delta
right_x += delta
if diff % 2 == 1:
right_x += 1
else: # Width > height, a short box.
top_y -= delta
bottom_y += delta
if diff % 2 == 1:
bottom_y += 1
return [left_x, top_y, right_x, bottom_y]
def process_image(self, color_msg):
tqdm.write('Time now: ' + str(rospy.Time.now().to_sec()) +
' message color: ' + str(color_msg.header.stamp.to_sec()) +
' diff: ' + str(rospy.Time.now().to_sec() -
color_msg.header.stamp.to_sec()))
start_time = time.time()
color_img = gaze_tools.convert_image(color_msg, "bgr8")
timestamp = color_msg.header.stamp
self.detect_landmarks(color_img, timestamp) # update self.subjects
tqdm.write('Elapsed after detecting transformed_landmarks: ' +
str(time.time() - start_time))
if not self.subjects:
tqdm.write("No face found")
return
self.get_eye_image() # update self.subjects
final_head_pose_images = None
for subject_id, subject in self.subjects.items():
if subject.left_eye_color is None or subject.right_eye_color is None:
continue
if subject_id not in self.last_rvec:
self.last_rvec[subject_id] = np.array([[0.01891013],
[0.08560084],
[-3.14392813]])
if subject_id not in self.last_tvec:
self.last_tvec[subject_id] = np.array([[-14.97821226],
[-10.62040383],
[-2053.03596872]])
if subject_id not in self.pose_stabilizers:
self.pose_stabilizers[subject_id] = [
Stabilizer(state_num=2,
measure_num=1,
cov_process=0.1,
cov_measure=0.1) for _ in range(6)
]
success, rotation_vector, translation_vector = self.get_head_pose(
subject.marks, subject_id)
# Publish all the data
translation_headpose_tf = self.get_head_translation(
timestamp, subject_id)
if success:
if translation_headpose_tf is not None:
euler_angles_head = self.publish_pose(
timestamp, translation_headpose_tf, rotation_vector,
subject_id)
if euler_angles_head is not None:
head_pose_image = self.visualize_headpose_result(
subject.face_color,
gaze_tools.get_phi_theta_from_euler(
euler_angles_head))
head_pose_image_resized = cv2.resize(
head_pose_image,
dsize=(224, 224),
interpolation=cv2.INTER_CUBIC)
if final_head_pose_images is None:
final_head_pose_images = head_pose_image_resized
else:
final_head_pose_images = np.concatenate(
(final_head_pose_images,
head_pose_image_resized),
axis=1)
else:
if not success:
tqdm.write("Could not get head pose properly")
if final_head_pose_images is not None:
self.publish_subject_list(timestamp, self.subjects)
headpose_image_ros = self.bridge.cv2_to_imgmsg(
final_head_pose_images, "bgr8")
headpose_image_ros.header.stamp = timestamp
self.subject_faces_pub.publish(headpose_image_ros)
tqdm.write('Elapsed total: ' + str(time.time() - start_time) + '\n\n')
def get_head_pose(self, landmarks, subject_id):
"""
We are given a set of 2D points in the form of landmarks. The basic idea is that we assume a generic 3D head
model, and try to fit the 2D points to the 3D model based on the Levenberg-Marquardt optimization. We can use
OpenCV's implementation of SolvePnP for this.
This method is inspired by http://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/
We are planning to replace this with our own head pose estimator.
:param landmarks: Landmark positions to be used to determine head pose
:return: success - Whether the pose was successfully extracted
rotation_vector - rotation vector that along with translation vector brings points from the model
coordinate system to the camera coordinate system
translation_vector - see rotation_vector
"""
image_points_headpose = landmarks
camera_matrix = self.img_proc.intrinsicMatrix()
dist_coeffs = self.img_proc.distortionCoeffs()
try:
if self.last_rvec[subject_id] is not None and self.last_tvec[
subject_id] is not None and self.use_previous_headpose_estimate:
(success, rotation_vector_unstable, translation_vector_unstable) = \
cv2.solvePnP(self.model_points, image_points_headpose, camera_matrix, dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE, useExtrinsicGuess=True,
rvec=self.last_rvec[subject_id], tvec=self.last_tvec[subject_id])
else:
(success, rotation_vector_unstable, translation_vector_unstable) = \
cv2.solvePnP(self.model_points, image_points_headpose, camera_matrix, dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)
except Exception:
print('Could not estimate head pose')
return False, None, None
if not success:
print('Could not estimate head pose')
return False, None, None
# Apply Kalman filter
stable_pose = []
pose_np = np.array(
(rotation_vector_unstable, translation_vector_unstable)).flatten()
for value, ps_stb in zip(pose_np, self.pose_stabilizers[subject_id]):
ps_stb.update([value])
stable_pose.append(ps_stb.state[0])
stable_pose = np.reshape(stable_pose, (-1, 3))
rotation_vector = stable_pose[0]
translation_vector = stable_pose[1]
self.last_rvec[subject_id] = rotation_vector
self.last_tvec[subject_id] = translation_vector
rotation_vector_swapped = [
-rotation_vector[2], -rotation_vector[1] + np.pi + self.head_pitch,
rotation_vector[0]
]
rot_head = tf_transformations.quaternion_from_euler(
*rotation_vector_swapped)
return success, rot_head, translation_vector
def publish_subject_list(self, timestamp, subjects):
assert (subjects is not None)
subject_list_message = self.__subject_bridge.images_to_msg(
subjects, timestamp)
self.subject_pub.publish(subject_list_message)
@staticmethod
def visualize_headpose_result(face_image, est_headpose):
"""Here, we take the original eye eye_image and overlay the estimated gaze."""
output_image = np.copy(face_image)
center_x = face_image.shape[1] / 2
center_y = face_image.shape[0] / 2
endpoint_x, endpoint_y = gaze_tools.get_endpoint(
est_headpose[1], est_headpose[0], center_x, center_y, 100)
cv2.line(output_image, (int(center_x), int(center_y)),
(int(endpoint_x), int(endpoint_y)), (0, 0, 255), 3)
return output_image
def get_head_translation(self, timestamp, subject_id):
trans_reshaped = self.last_tvec[subject_id].reshape(3, 1)
nose_center_3d_rot = [
-float(trans_reshaped[0] / 1000.0),
-float(trans_reshaped[1] / 1000.0),
-float(trans_reshaped[2] / 1000.0)
]
nose_center_3d_rot_frame = self.rgb_frame_id
try:
nose_center_3d_rot_pt = PointStamped()
nose_center_3d_rot_pt.header.frame_id = nose_center_3d_rot_frame
nose_center_3d_rot_pt.header.stamp = timestamp
nose_center_3d_rot_pt.point = Point(x=nose_center_3d_rot[0],
y=nose_center_3d_rot[1],
z=nose_center_3d_rot[2])
nose_center_3d = self.tf_listener.transformPoint(
self.rgb_frame_id_ros, nose_center_3d_rot_pt)
nose_center_3d.header.stamp = timestamp
nose_center_3d_tf = gaze_tools.position_ros_to_tf(
nose_center_3d.point)
print('Translation based on landmarks', nose_center_3d_tf)
return nose_center_3d_tf
except ExtrapolationException as e:
print(e)
return None
def publish_pose(self, timestamp, nose_center_3d_tf, rot_head, subject_id):
self.tf_broadcaster.sendTransform(
nose_center_3d_tf, rot_head, timestamp,
self.tf_prefix + "/head_pose_estimated" + str(subject_id),
self.rgb_frame_id_ros)
return gaze_tools.get_head_pose(nose_center_3d_tf, rot_head)
def callback(self, color_msg):
"""Simply call process_image."""
try:
self.process_image(color_msg)
except CvBridgeError as e:
print(e)
except rospy.ROSException as e:
if str(e) == "publish() to a closed topic":
pass
else:
raise e
def __update_subjects(self, new_faceboxes):
"""
Assign the new faces to the existing subjects (id tracking)
:param new_faceboxes: new faceboxes detected
:return: update self.subjects
"""
assert (self.subjects is not None)
assert (new_faceboxes is not None)
if len(new_faceboxes) == 0:
self.subjects = dict()
return
if len(self.subjects) == 0:
for j, b_new in enumerate(new_faceboxes):
self.subjects[j] = SubjectDetected(b_new)
return
distance_matrix = np.ones((len(self.subjects), len(new_faceboxes)))
for i, subject in enumerate(self.subjects.values()):
for j, b_new in enumerate(new_faceboxes):
distance_matrix[i][j] = np.sqrt(
np.mean(
((np.array(subject.face_bb) - np.array(b_new))**2)))
ids_to_assign = range(len(new_faceboxes))
self.subjects = dict()
for id in ids_to_assign:
subject = np.argmin(distance_matrix[:, id])
while subject in self.subjects:
subject += 1
self.subjects[subject] = SubjectDetected(new_faceboxes[id])
def __detect_landmarks_one_box(self, facebox, color_img):
try:
_bb = map(int, facebox)
face_img = color_img[_bb[1]:_bb[3], _bb[0]:_bb[2]]
marks_orig = np.array(
self.__detect_facial_landmarks(color_img, facebox)[0])
eye_indices = np.array([36, 39, 42, 45])
transformed_landmarks = marks_orig[eye_indices]
transformed_landmarks[:, 0] -= facebox[0]
transformed_landmarks[:, 1] -= facebox[1]
return face_img, transformed_landmarks, marks_orig
except Exception:
return None, None, None
def __detect_facial_landmarks(self, color_img, facebox):
marks = self.facial_landmark_nn.get_landmarks(color_img,
detected_faces=[facebox])
return marks
def detect_landmarks(self, color_img, timestamp):
faceboxes = self.get_face_bb(color_img)
self.__update_subjects(faceboxes)
for subject in self.subjects.values():
res = self.__detect_landmarks_one_box(subject.face_bb, color_img)
subject.face_color = res[0]
subject.transformed_landmarks = res[1]
subject.marks = res[2]
def __get_eye_image_one(self, transformed_landmarks, face_aligned_color):
margin_ratio = 1.0
try:
# Get the width of the eye, and compute how big the margin should be according to the width
lefteye_width = transformed_landmarks[3][
0] - transformed_landmarks[2][0]
righteye_width = transformed_landmarks[1][
0] - transformed_landmarks[0][0]
lefteye_margin, righteye_margin = lefteye_width * margin_ratio, righteye_width * margin_ratio
# lefteye_center_x = transformed_landmarks[2][0] + lefteye_width / 2
# righteye_center_x = transformed_landmarks[0][0] + righteye_width / 2
lefteye_center_y = (transformed_landmarks[2][1] +
transformed_landmarks[3][1]) / 2
righteye_center_y = (transformed_landmarks[1][1] +
transformed_landmarks[0][1]) / 2
desired_ratio = self.eye_image_size[0] / self.eye_image_size[1] / 2
# Now compute the bounding boxes
# The left / right x-coordinates are computed as the landmark position plus/minus the margin
# The bottom / top y-coordinates are computed according to the desired ratio, as the width of the image is known
left_bb = np.zeros(4, dtype=np.int32)
left_bb[0] = transformed_landmarks[2][0] - lefteye_margin / 2
left_bb[1] = lefteye_center_y - (
lefteye_width + lefteye_margin) * desired_ratio * 1.25
left_bb[2] = transformed_landmarks[3][0] + lefteye_margin / 2
left_bb[3] = lefteye_center_y + (
lefteye_width + lefteye_margin) * desired_ratio * 1.25
right_bb = np.zeros(4, dtype=np.int32)
right_bb[0] = transformed_landmarks[0][0] - righteye_margin / 2
right_bb[1] = righteye_center_y - (
righteye_width + righteye_margin) * desired_ratio * 1.25
right_bb[2] = transformed_landmarks[1][0] + righteye_margin / 2
right_bb[3] = righteye_center_y + (
righteye_width + righteye_margin) * desired_ratio * 1.25
# Extract the eye images from the aligned image
left_eye_color = face_aligned_color[left_bb[1]:left_bb[3],
left_bb[0]:left_bb[2], :]
right_eye_color = face_aligned_color[right_bb[1]:right_bb[3],
right_bb[0]:right_bb[2], :]
# for p in transformed_landmarks: # For debug visualization only
# cv2.circle(face_aligned_color, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)
# So far, we have only ensured that the ratio is correct. Now, resize it to the desired size.
left_eye_color_resized = scipy.misc.imresize(left_eye_color,
self.eye_image_size,
interp='lanczos')
right_eye_color_resized = scipy.misc.imresize(right_eye_color,
self.eye_image_size,
interp='lanczos')
except (ValueError, TypeError) as e:
print(e)
return None, None, None, None
return left_eye_color_resized, right_eye_color_resized, left_bb, right_bb
# noinspection PyUnusedLocal
def get_eye_image(self):
"""Extract the left and right eye images given the (dlib) transformed_landmarks and the source image.
First, align the face. Then, extract the width of the eyes given the landmark positions.
The height of the images is computed according to the desired ratio of the eye images."""
start_time = time.time()
for subject in self.subjects.values():
res = self.__get_eye_image_one(subject.transformed_landmarks,
subject.face_color)
subject.left_eye_color = res[0]
subject.right_eye_color = res[1]
subject.left_eye_bb = res[2]
subject.right_eye_bb = res[3]
tqdm.write('New get_eye_image time: ' + str(time.time() - start_time))
@staticmethod
def get_image_points_eyes_nose(landmarks):
landmarks_x, landmarks_y = landmarks.T[0], landmarks.T[1]
left_eye_center_x = landmarks_x[42] + (landmarks_x[45] -
landmarks_x[42]) / 2.0
left_eye_center_y = (landmarks_y[42] + landmarks_y[45]) / 2.0
right_eye_center_x = landmarks_x[36] + (landmarks_x[40] -
landmarks_x[36]) / 2.0
right_eye_center_y = (landmarks_y[36] + landmarks_y[40]) / 2.0
nose_center_x, nose_center_y = (landmarks_x[33] + landmarks_x[31] + landmarks_x[35]) / 3.0, \
(landmarks_y[33] + landmarks_y[31] + landmarks_y[35]) / 3.0
return (nose_center_x, nose_center_y), \
(left_eye_center_x, left_eye_center_y), (right_eye_center_x, right_eye_center_y)
class LandmarkMethodROS(LandmarkMethodBase):
def __init__(self, img_proc=None):
super(LandmarkMethodROS,
self).__init__(device_id_facedetection=rospy.get_param(
"~device_id_facedetection", default="cuda:0"))
self.subject_tracker = FaceEncodingTracker() if rospy.get_param(
"~use_face_encoding_tracker",
default=True) else SequentialTracker()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.camera_frame = rospy.get_param("~camera_frame", "kinect2_link")
self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
"kinect2_ros_frame")
self.tf2_broadcaster = TransformBroadcaster()
self.tf2_buffer = Buffer()
self.tf2_listener = TransformListener(self.tf2_buffer)
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.visualise_headpose = rospy.get_param("~visualise_headpose",
default=True)
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
if img_proc is None:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
else:
self.img_proc = img_proc
if np.array_equal(
self.img_proc.intrinsicMatrix().A,
np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception(
'Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?'
)
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images",
MSG_SubjectImagesList,
queue_size=3)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces",
Image,
queue_size=3)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
# have the subscriber the last thing that's run to avoid conflicts
self.color_sub = rospy.Subscriber("/image",
Image,
self.process_image,
buff_size=2**24,
queue_size=3)
def _dyn_reconfig_callback(self, config, _):
self.model_points /= (self.model_size_rescale *
self.interpupillary_distance)
self.model_size_rescale = config["model_size"]
self.interpupillary_distance = config["interpupillary_distance"]
self.model_points *= (self.model_size_rescale *
self.interpupillary_distance)
self.head_pitch = config["head_pitch"]
return config
def process_image(self, color_msg):
color_img = gaze_tools.convert_image(color_msg, "bgr8")
timestamp = color_msg.header.stamp
self.update_subject_tracker(color_img)
if not self.subject_tracker.get_tracked_elements():
tqdm.write("\033[2K\033[1;31mNo face found\033[0m", end="\r")
return
self.subject_tracker.update_eye_images(self.eye_image_size)
final_head_pose_images = []
for subject_id, subject in self.subject_tracker.get_tracked_elements(
).items():
if subject.left_eye_color is None or subject.right_eye_color is None:
continue
if subject_id not in self.pose_stabilizers:
self.pose_stabilizers[subject_id] = [
Stabilizer(state_num=2,
measure_num=1,
cov_process=0.1,
cov_measure=0.1) for _ in range(6)
]
success, head_rpy, translation_vector = self.get_head_pose(
subject.marks, subject_id)
if success:
# Publish all the data
self.publish_pose(timestamp, translation_vector, head_rpy,
subject_id)
if self.visualise_headpose:
# pitch roll yaw
trans_msg = self.tf2_buffer.lookup_transform(
self.ros_tf_frame, self.tf_prefix +
"/head_pose_estimated" + str(subject_id), timestamp)
rotation = trans_msg.transform.rotation
euler_angles_head = list(
transformations.euler_from_quaternion(
[rotation.x, rotation.y, rotation.z, rotation.w]))
euler_angles_head = gaze_tools.limit_yaw(euler_angles_head)
face_image_resized = cv2.resize(
subject.face_color,
dsize=(224, 224),
interpolation=cv2.INTER_CUBIC)
final_head_pose_images.append(
LandmarkMethodROS.visualize_headpose_result(
face_image_resized,
gaze_tools.get_phi_theta_from_euler(
euler_angles_head)))
if len(self.subject_tracker.get_tracked_elements().items()) > 0:
self.publish_subject_list(
timestamp, self.subject_tracker.get_tracked_elements())
if len(final_head_pose_images) > 0:
headpose_image_ros = self.bridge.cv2_to_imgmsg(
np.hstack(final_head_pose_images), "bgr8")
headpose_image_ros.header.stamp = timestamp
self.subject_faces_pub.publish(headpose_image_ros)
def get_head_pose(self, landmarks, subject_id):
"""
We are given a set of 2D points in the form of landmarks. The basic idea is that we assume a generic 3D head
model, and try to fit the 2D points to the 3D model based on the Levenberg-Marquardt optimization. We can use
OpenCV's implementation of SolvePnP for this.
This method is inspired by http://www.learnopencv.com/head-pose-estimation-using-opencv-and-dlib/
We are planning to replace this with our own head pose estimator.
:param landmarks: Landmark positions to be used to determine head pose
:param subject_id: ID of the subject that corresponds to the given landmarks
:return: success - Whether the pose was successfully extracted
rotation_vector - rotation vector that along with translation vector brings points from the model
coordinate system to the camera coordinate system
translation_vector - see rotation_vector
"""
camera_matrix = self.img_proc.intrinsicMatrix()
dist_coeffs = self.img_proc.distortionCoeffs()
try:
success, rodrigues_rotation, translation_vector, _ = cv2.solvePnPRansac(
self.model_points,
landmarks.reshape(len(self.model_points), 1, 2),
cameraMatrix=camera_matrix,
distCoeffs=dist_coeffs,
flags=cv2.SOLVEPNP_DLS)
except cv2.error as e:
tqdm.write(
'\033[2K\033[1;31mCould not estimate head pose: {}\033[0m'.
format(e),
end="\r")
return False, None, None
if not success:
tqdm.write(
'\033[2K\033[1;31mUnsuccessful in solvingPnPRanscan\033[0m',
end="\r")
return False, None, None
# this is generic point stabiliser, the underlying representation doesn't matter
rotation_vector, translation_vector = self.apply_kalman_filter_head_pose(
subject_id, rodrigues_rotation, translation_vector / 1000.0)
rotation_vector[0] += self.head_pitch
_rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
_rotation_matrix = np.matmul(
_rotation_matrix, np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))
_m = np.zeros((4, 4))
_m[:3, :3] = _rotation_matrix
_m[3, 3] = 1
_rpy_rotation = np.array(transformations.euler_from_matrix(_m))
return success, _rpy_rotation, translation_vector
def apply_kalman_filter_head_pose(self, subject_id,
rotation_vector_unstable,
translation_vector_unstable):
stable_pose = []
pose_np = np.array(
(rotation_vector_unstable, translation_vector_unstable)).flatten()
for value, ps_stb in zip(pose_np, self.pose_stabilizers[subject_id]):
ps_stb.update([value])
stable_pose.append(ps_stb.state[0])
stable_pose = np.reshape(stable_pose, (-1, 3))
rotation_vector = stable_pose[0]
translation_vector = stable_pose[1]
return rotation_vector, translation_vector
def publish_subject_list(self, timestamp, subjects):
assert (subjects is not None)
subject_list_message = self.__subject_bridge.images_to_msg(
subjects, timestamp)
self.subject_pub.publish(subject_list_message)
def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id):
t = TransformStamped()
t.header.frame_id = self.camera_frame
t.header.stamp = timestamp
t.child_frame_id = self.tf_prefix + "/head_pose_estimated" + str(
subject_id)
t.transform.translation.x = nose_center_3d_tf[0]
t.transform.translation.y = nose_center_3d_tf[1]
t.transform.translation.z = nose_center_3d_tf[2]
rotation = transformations.quaternion_from_euler(*head_rpy)
t.transform.rotation.x = rotation[0]
t.transform.rotation.y = rotation[1]
t.transform.rotation.z = rotation[2]
t.transform.rotation.w = rotation[3]
try:
self.tf2_broadcaster.sendTransform([t])
except rospy.ROSException as exc:
if str(exc) == "publish() to a closed topic":
pass
else:
raise exc
self.tf2_buffer.set_transform(t, 'extract_landmarks')
def update_subject_tracker(self, color_img):
faceboxes = self.get_face_bb(color_img)
if len(faceboxes) == 0:
self.subject_tracker.clear_elements()
return
tracked_subjects = self.get_subjects_from_faceboxes(
color_img, faceboxes)
# track the new faceboxes according to the previous ones
self.subject_tracker.track(tracked_subjects)
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection, encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height, self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0;
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates, rectified_camera_coordinates, camera_model.intrinsicMatrix(), camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates, camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
class BuoyRecognitionNode(object):
def __init__(self):
self._tf_buffer = tf2_ros.Buffer(
rospy.Duration(1200.0)) # tf buffer length
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
self._camera_input_image_raw_topic = rospy.get_param(
"~camera_input_image_raw_topic",
"/cora/sensors/cameras/front_left_camera/image_raw")
self._camera_input_camera_info_topic = rospy.get_param(
"~camera_input_camera_info_topic",
"/cora/sensors/cameras/front_left_camera/camera_info")
self._tagged_image_output_topic = rospy.get_param(
"~tagged_image_output_topic", "/tij/camera/tagged_image")
self._point_cloud_data_topic = rospy.get_param(
"~pointcloud_data", "/cora/sensors/lidars/front_lidar/points")
self._buoy_markers_topic = rospy.get_param("~buoy_markers_topic",
"/tij/markers/buoys")
self._detection_array_topic = rospy.get_param(
"~detection_array_topic", "/tij/detections/detection_array")
self._buoy_color_samples_database_file = rospy.get_param(
"~buoy_color_samples_database_file", "samples.csv")
self._depth_sensor_min_distance = rospy.get_param(
"~depth_sensor_min_distance", 5)
self._depth_sensor_max_distance = rospy.get_param(
"~depth_sensor_max_distance", 100)
self._depth_sensor_clustering_range = rospy.get_param(
"~depth_sensor_clustering_range", 1.0)
self._max_perception_reach = rospy.get_param("~max_perception_reach",
100)
self._max_classification_reach = rospy.get_param(
"~max_classification_reach", 40)
self._canny_threshold_min = rospy.get_param("~canny_threshold_min",
100)
self._canny_threshold_max = rospy.get_param("~canny_threshold_max",
300)
self._border_reduction_kernel_size = rospy.get_param(
"~border_reduction_kernel_size", 7)
self._map_frame_id = rospy.get_param("~map_frame_id", "cora/odom")
self._next_known_object_id = 0
# don't let anything uninitialized
self._image_mask = None
self._laser_detected_objects = None
self._cv_bridge = CvBridge()
self._camera_model = None
self._known_objects = {}
self._buoy_classifier = BuoyClassifier(
self._buoy_color_samples_database_file)
# publishers
self._tagged_image_output_pub = rospy.Publisher(
self._tagged_image_output_topic, Image, queue_size=10)
self._marker_publisher = rospy.Publisher(self._buoy_markers_topic,
MarkerArray,
queue_size=10)
self._detection_array_pub = rospy.Publisher(
self._detection_array_topic, DetectionDataArray, queue_size=10)
# subscribers
self._image_raw_sub = rospy.Subscriber(
self._camera_input_image_raw_topic,
Image,
self._image_raw_callback,
queue_size=1)
self._camera_info_sub = rospy.Subscriber(
self._camera_input_camera_info_topic, CameraInfo,
self._camera_info_callback)
self._camera_info_sub = rospy.Subscriber(
self._point_cloud_data_topic, PointCloud2,
self._point_cloud_data_callback)
def _get_laser_detections_in_camera_frame(self, camera_frame_id,
image_timestamp):
# Convert objects detected in the point cloud data to the camera frame and
# pixel coordiantes
laser_detected_objects_in_camera_frame = []
if not self._laser_detected_objects:
return laser_detected_objects_in_camera_frame
if not self._can_transform_between_frames(
self._laser_detected_objects["frame_id"],
self._laser_detected_objects["timestamp"], camera_frame_id,
image_timestamp):
return laser_detected_objects_in_camera_frame
locations_in_world = [
self._transform_location_between_frames(
self._laser_detected_objects["frame_id"],
self._laser_detected_objects["timestamp"], camera_frame_id,
image_timestamp, detection_location) for detection_location in
self._laser_detected_objects["detections"]
]
laser_detected_objects_in_camera_frame = [{
"location_in_world":
loc,
"location_in_image":
self._camera_model.project3dToPixel((loc.x, loc.y, loc.z))
} for loc in locations_in_world]
return laser_detected_objects_in_camera_frame
def _find_rectangle_center(self, rect):
x, y, w, h = rect
return (x + w / 2.0), (y + h / 2.0)
def _is_within_rectangle(self, coord, rect):
xt, yt = coord
x1, y1, w, h = rect
x2 = x1 + w
y2 = y1 + h
if ((xt >= x1) and (xt <= x2) and (yt >= y1) and (yt <= y2)):
return True
return False
def _get_distance_between_points(self, coord_1, coord_2):
x1, y1, = coord_1
x2, y2, = coord_2
return np.sqrt((x1 - x2)**2 + (y1 - y2)**2)
def _match_laser_to_camera(self, laser_detection_data,
camera_detection_data, camera_frame_id,
image_timestamp):
objects_found_data = []
contours, rectangles, areas, rgb_colors, hsv_colors = camera_detection_data
for laser_hit in laser_detection_data:
loc_in_world = laser_hit["location_in_world"]
loc_in_image = laser_hit["location_in_image"]
# Don't look of laser hits behind the camera
if loc_in_world.z < 0.0:
continue
# Look for the camera detection that's located closest to the point of impact
# of the laser
best_rgb = None
best_hsv = None
best_rect = None
best_distance = 1e6 # should be larger than anything possible
for rect, rgb, hsv in zip(rectangles, rgb_colors, hsv_colors):
rect_center = self._find_rectangle_center(rect)
distance_to_center = self._get_distance_between_points(
rect_center, loc_in_image)
if self._is_within_rectangle(
loc_in_image,
rect) and (distance_to_center < best_distance):
best_rgb = rgb
best_hsv = hsv
best_rect = rect
best_distance = distance_to_center
# if we found a match, we create a register
if best_rgb is not None and loc_in_world.z < self._max_perception_reach:
object_width = best_rect[2] * \
loc_in_world.z / self._camera_model.fx()
object_height = best_rect[3] * \
loc_in_world.z / self._camera_model.fy()
hit_info = {}
hit_info[
"location_in_world"] = self._transform_location_between_frames(
camera_frame_id, image_timestamp, self._map_frame_id,
image_timestamp, loc_in_world)
hit_info["rectangle_in_image"] = best_rect
hit_info["rgb_color"] = best_rgb
hit_info["hsv_color"] = best_hsv
hit_info["z_distance"] = loc_in_world.z
hit_info["object_width"] = object_width
hit_info["object_height"] = object_height
hit_info[
"object_type"] = self._buoy_classifier.classify_buoy_sample_hsv(
(object_width, object_height), best_hsv)
objects_found_data.append(hit_info)
return objects_found_data
def _check_if_objecs_is_already_known(self, new_entry):
# walk through the elemets known to see if they match in the same location
def distance_between_map_locations(p1, p2):
# Don't measure distance in z, since that's height
distance = np.sqrt((p1.x - p2.x)**2 + (p1.y - p2.y)**2)
return distance
new_entry_location = new_entry["location_in_world"]
for _, known_object in self._known_objects.items():
known_entry_location = known_object.get_location()
distance = distance_between_map_locations(new_entry_location,
known_entry_location)
identity_distance = max(1.0, new_entry["object_width"])
if (distance < identity_distance):
if (new_entry["z_distance"] < self._max_classification_reach):
known_object.add_positive_observation(
new_entry["object_type"])
return True
return False
def _insert_new_object_in_database(self, new_entry):
next_index = self._next_known_object_id
self._next_known_object_id += 1
self._known_objects[next_index] = PhysicalBuoy(
new_entry["location_in_world"])
if new_entry["z_distance"] < self._max_classification_reach:
self._known_objects[next_index].add_positive_observation(
new_entry["object_type"])
def _match_known_objects_to_image(self, camera_detection_data,
camera_frame_id, image_timestamp):
contours, rectangles, areas, rgb_colors, hsv_colors = camera_detection_data
image_height, image_width = self._image_mask.shape
for _, known_object in self._known_objects.items():
p = self._transform_location_between_frames(
self._map_frame_id, image_timestamp, camera_frame_id,
image_timestamp, known_object.get_location())
# is this object within visual range?
if p.z <= 0 or p.z >= self._max_perception_reach:
continue
# if it were there, would it be visible in the image?
u, v = self._camera_model.project3dToPixel((p.x, p.y, p.z))
u, v = int(u), int(v)
if (u < 0) or (u >= image_width) or (v < 0) or (
v >= image_height) or (self._image_mask[v, u] != 0):
continue
# it should be visible in the image, but is it?
if not any([
self._is_within_rectangle((u, v), rect)
for rect in rectangles
]):
known_object.add_negative_observation()
def _remove_dead_objects(self):
objects_to_remove = [
index for index, known_object in self._known_objects.items()
if known_object.is_dead()
]
for key in objects_to_remove:
classification = self._known_objects[
key].get_most_likely_classification()
loc_str = str(self._known_objects[key].get_location())
if classification:
category, prob = classification
rospy.logwarn(
"Removing dead object of type {} ({}) at location {}".
format(category, prob, loc_str))
else:
rospy.logwarn(
"Removing unclassified dead object at location {}".format(
loc_str))
self._known_objects.pop(key, None)
def _publish_known_objects_info(self):
if not len(self._known_objects):
return
rospy.loginfo("There are {} objects that have been located".format(
len(self._known_objects)))
detections_array_msg = DetectionDataArray()
detections_array_msg.header.stamp = rospy.Time.now()
detections_array_msg.header.frame_id = self._map_frame_id
for det_id, known_object in self._known_objects.items():
classification = known_object.get_most_likely_classification()
detection_msg = DetectionData()
detection_msg.id = det_id
detection_msg.location = known_object.get_location()
location_str = " ".join(str(known_object.get_location()).split())
if classification:
category, probability = classification
detection_msg.classified = True
detection_msg.probability = probability
detection_msg.category = category
rospy.loginfo(
" - category: {:20s} - probability: {:5.2f} - location: {:30s}"
.format(category, probability, location_str))
else:
detection_msg.classified = False
detection_msg.probability = 0.0
detection_msg.category = ""
rospy.loginfo(
" - category: {:20s} - probability: {:5.2f} - location: {:30s}"
.format("UNKNOWN", 0.0, location_str))
detections_array_msg.detections.append(detection_msg)
self._detection_array_pub.publish(detections_array_msg)
def _process_objects_known_and_new(self, objects_found_data,
camera_detections_data, camera_frame_id,
image_timestamp):
for new_entry in objects_found_data:
known = self._check_if_objecs_is_already_known(new_entry)
if not known:
self._insert_new_object_in_database(new_entry)
self._match_known_objects_to_image(camera_detections_data,
camera_frame_id, image_timestamp)
self._remove_dead_objects()
self._publish_known_objects_info()
def _publish_markers(self):
marray = MarkerArray()
for index, known_object in self._known_objects.items():
loc = known_object.get_location()
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = self._map_frame_id
m.ns = "tij"
m.id = index
m.type = Marker.CUBE
m.action = Marker.MODIFY
m.pose.position = loc
m.pose.orientation.w = 1.0
m.scale = Vector3(x=1, y=1, z=1)
m.lifetime = rospy.Duration(1.0)
m.frame_locked = True
marray.markers.append(m)
if known_object.get_most_likely_classification() is None:
m.color = ColorRGBA(1.0, 0.0, 0, 1.0)
else:
m.color = ColorRGBA(0, 1.0, 0, 1.0)
self._marker_publisher.publish(marray)
def _tag_image_detections_on_output_image(self, output_cv_image,
camera_detections_data):
contours, rectangles, areas, rgb_colors, hsv_colors = camera_detections_data
# Create the mask only once, then reuse
for cnt, rect in zip(contours, rectangles):
x, y, w, h = rect
# mark the visual detection in the output image
cv.rectangle(output_cv_image, (x, y), (x + w, y + h), (255, 0, 0),
1)
cv.drawContours(output_cv_image, [cnt], -1, (0, 255, 0), 2)
def _tag_laser_detections_on_output_image(self, output_cv_image,
laser_detections_data):
locations_in_image = [
item["location_in_image"] for item in laser_detections_data
if item["location_in_world"].z >= 0
]
for loc in locations_in_image:
x, y = loc
x, y = int(x), int(y)
cv.circle(output_cv_image, (x, y), 5, (255, 255, 0), 2)
def _tag_hit_objects_in_image(self, output_cv_image, objects_found_data):
# Create the mask only once, then reuse
for detection in objects_found_data:
x, y, w, h = detection["rectangle_in_image"]
# mark the visual detection in the output image
cv.putText(output_cv_image, detection["object_type"], (x + 5, y),
cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255))
def _image_raw_callback(self, image_msg):
# Convert the image from ros msg to opencv domain
input_cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg,
desired_encoding='rgb8')
# If camera model is not ready, we can't do anything
if self._camera_model is None:
rospy.logwarn("Waiting for camera model information to arrive...")
return
# Generate the image mask only once.
if self._image_mask is None:
self._image_mask = self._create_view_for_image(input_cv_image)
# Recover information from objects detected in the pointcloud
laser_detections_data = self._get_laser_detections_in_camera_frame(
image_msg.header.frame_id, image_msg.header.stamp)
# process image to detect objects
camera_detections_data = self._get_image_contours(
input_cv_image,
(self._canny_threshold_min, self._canny_threshold_max),
self._border_reduction_kernel_size)
# match laser hits to camera, and filter remaining data
objects_found_data = self._match_laser_to_camera(
laser_detections_data, camera_detections_data,
image_msg.header.frame_id, image_msg.header.stamp)
# process objects to determine which are new and which are not
self._process_objects_known_and_new(objects_found_data,
camera_detections_data,
image_msg.header.frame_id,
image_msg.header.stamp)
# publish markers
self._publish_markers()
# create output image by tagging known information in it
output_cv_image = input_cv_image.copy()
# add information recovered through detection to the image
self._tag_image_detections_on_output_image(output_cv_image,
camera_detections_data)
self._tag_laser_detections_on_output_image(output_cv_image,
laser_detections_data)
self._tag_hit_objects_in_image(output_cv_image, objects_found_data)
# Convert the image from opencv domain to ros msg
output_image = self._cv_bridge.cv2_to_imgmsg(output_cv_image,
encoding='rgb8')
# Publish information
self._tagged_image_output_pub.publish(output_image)
def _camera_info_callback(self, camera_info_msg):
if not self._camera_model:
self._camera_model = PinholeCameraModel()
self._camera_model.fromCameraInfo(camera_info_msg)
rospy.loginfo("Got camera description message:")
rospy.loginfo(" K = {0}, D = {1}".format(
self._camera_model.intrinsicMatrix(),
self._camera_model.distortionCoeffs()))
def _point_cloud_data_callback(self, point_cloud_2_msg):
# Convert the pointcloud2 data to a list of detected clusters
# with coordinates, so that we deal with a smaller number of measurements
detected_objects = self._clusterize_pointcloud_into_objects(
point_cloud_2_msg)
def np_vector_to_point(v):
return Point(x=v[0], y=v[1], z=v[2])
self._laser_detected_objects = {
"frame_id": point_cloud_2_msg.header.frame_id,
"timestamp": point_cloud_2_msg.header.stamp,
"detections": [np_vector_to_point(v) for v in detected_objects]
}
def _clusterize_pointcloud_into_objects(self, point_cloud_2_msg):
# Object count will be used to generate unique ids for clusters
object_count = 0
detected_objects = {}
for hit in point_cloud2.read_points(point_cloud_2_msg, skip_nans=True):
hit_pos = np.asarray(hit[0:3])
# Reject measurements too close or too far
laser_distance = np.sqrt(sum(hit_pos**2))
if laser_distance < self._depth_sensor_min_distance or laser_distance > self._depth_sensor_max_distance:
continue
# Check if any of the previously known objects is within clustering distance of
# the new detection. Also, for known objects, we average coordinates of multiple
# hits within the cluster, to get a better idea of the center location.
binned = False
for object_id, obj_data in detected_objects.items():
# accum_pos is the sum of all previous hits in this cluster, multiplicity
# is the number of hits in the cluster
accum_pos, multiplicity = obj_data
distance = np.sqrt(
np.sum((accum_pos / multiplicity - hit_pos)**2))
if (distance < self._depth_sensor_clustering_range):
# add this hit to the cluster
detected_objects[object_id] = (accum_pos + hit_pos,
multiplicity + 1)
binned = True
break
# if we did not find a previous cluster near the hit, create a new cluster
if not binned:
detected_objects[object_count] = (hit_pos, 1)
object_count += 1
# average all the hits within each cluster
objects = [
value[0] / value[1] for key, value in detected_objects.items()
]
return objects
def _create_view_for_image(self, input_image):
mask = np.zeros(input_image.shape[:2], np.uint8)
height, width, _ = input_image.shape
y1 = int(height * 0.25)
y2 = int(height * 0.55)
x1 = int(width * 0.2)
x2 = int(width / 2.0)
x3 = width - x1
pts = np.array([(x1, height), (x2, y2), (x3, height)], np.int32)
pts = pts.reshape((-1, 1, 2))
cv.fillPoly(mask, [pts], True, 255)
cv.rectangle(mask, (0, 0), (width, y1), 255, -1)
return mask
def _get_image_contours(self,
input_image,
canny_args=(200, 300),
kernel_size=7):
hsv_image = cv.cvtColor(input_image, cv.COLOR_RGB2HSV)
edges = cv.Canny(input_image, canny_args[0], canny_args[1])
blurred = cv.GaussianBlur(edges, (3, 9), 0)
_, contours, __ = cv.findContours(blurred, cv.RETR_EXTERNAL,
cv.CHAIN_APPROX_SIMPLE)
rectangles = [cv.boundingRect(cnt) for cnt in contours]
areas = [cv.contourArea(cnt) for cnt in contours]
rgb = []
hsv = []
dilate_kernel = np.ones((kernel_size, kernel_size), np.uint8)
erode_kernel = np.ones((2 * kernel_size, 2 * kernel_size), np.uint8)
for cnt in contours:
mask = np.zeros(input_image.shape[:2], np.uint8)
cv.drawContours(mask, [cnt], 0, 255, -1)
dilated_mask = cv.dilate(mask, dilate_kernel, iterations=1)
eroded_mask = cv.erode(dilated_mask, erode_kernel, iterations=1)
rgb_color = cv.mean(input_image, mask=eroded_mask)
hsv_color = cv.mean(hsv_image, mask=eroded_mask)
rgb.append(rgb_color[:3])
hsv.append(hsv_color[:3])
def masked_out(x, y, w, h):
xc, yc = self._find_rectangle_center((x, y, w, h))
xc, yc = int(xc), int(yc)
return (self._image_mask is None or self._image_mask[yc, xc] != 0)
visibility_filter = [not masked_out(*rect) for rect in rectangles]
fcontours = [
contours[i] for i in range(len(contours)) if visibility_filter[i]
]
frectangles = [
rectangles[i] for i in range(len(contours)) if visibility_filter[i]
]
fareas = [
areas[i] for i in range(len(contours)) if visibility_filter[i]
]
frgb = [rgb[i] for i in range(len(contours)) if visibility_filter[i]]
fhsv = [hsv[i] for i in range(len(contours)) if visibility_filter[i]]
return fcontours, frectangles, fareas, frgb, fhsv
def _transform_location_between_frames(self, source_frame,
source_timestamp, dest_frame,
dest_timestamp, location):
transform = self._tf_buffer.lookup_transform_full(
target_frame=dest_frame,
target_time=dest_timestamp,
source_frame=source_frame,
source_time=source_timestamp,
fixed_frame="cora/odom",
timeout=rospy.Duration(1.0))
original = PointStamped()
original.header.frame_id = source_frame
original.header.stamp = source_timestamp
original.point = location
pose = tf2_geometry_msgs.do_transform_point(original, transform)
return Point(x=pose.point.x, y=pose.point.y, z=pose.point.z)
def _can_transform_between_frames(self, source_frame, source_timestamp,
dest_frame, dest_timestamp):
return self._tf_buffer.can_transform_full(target_frame=dest_frame,
target_time=dest_timestamp,
source_frame=source_frame,
source_time=source_timestamp,
fixed_frame="cora/odom",
timeout=rospy.Duration(1.0))
def run(self):
rospy.spin()