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 __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_dimensions = (self.projector_info.width,
self.projector_info.height)
self.number_of_scanlines = []
for i in range(2):
self.number_of_scanlines.append(
get_number_of_scanlines(self.projector_dimensions[i],
self.pixels_per_scanline))
self.read_scanner_info()
rospy.loginfo("Generating projection patterns...")
graycodes = []
for i in range(max(self.number_of_scanlines)):
graycodes.append(graycodemath.generate_gray_code(i))
self.number_of_projection_patterns = []
for i in range(2):
self.number_of_projection_patterns.append(
int(math.ceil(math.log(self.number_of_scanlines[i], 2))))
self.positive_projections = []
self.negative_projections = []
for i in range(2):
self.positive_projections.append([])
self.negative_projections.append([])
for j in range(self.number_of_projection_patterns[i]):
cross_section = cv.CreateMat(1, self.projector_dimensions[i],
cv.CV_8UC1)
#Fill in cross section with the associated bit of each gray code
for pixel in range(self.projector_dimensions[i]):
scanline = pixel // self.pixels_per_scanline
scanline_value = graycodemath.get_bit(
graycodes[scanline], j) * 255
cross_section[0, pixel] = scanline_value
#If we're doing horizontal scanning, transpose the cross section
if i is 1:
cross_section_transpose = cv.CreateMat(
self.projector_info.height, 1, cv.CV_8UC1)
cv.Transpose(cross_section, cross_section_transpose)
cross_section = cross_section_transpose
#Repeat the cross section over the entire image
positive_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Repeat(cross_section, positive_projection)
#Create a negative of the pattern for thresholding
negative_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Not(positive_projection, negative_projection)
#Fade the borders of the patterns to avoid artifacts at the edges of projection
positive_projection_faded = fade_edges(positive_projection, 20)
negative_projection_faded = fade_edges(negative_projection, 20)
self.positive_projections[i].append(positive_projection_faded)
self.negative_projections[i].append(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.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.pixel_associations_msg is not None:
pixel_associations_pub.publish(self.pixel_associations_msg)
if self.point_cloud_msg is not None:
point_cloud_pub.publish(self.point_cloud_msg)
rate.sleep()
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 __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_dimensions = (self.projector_info.width, self.projector_info.height)
self.number_of_scanlines = []
for i in range(2):
self.number_of_scanlines.append(get_number_of_scanlines(self.projector_dimensions[i], self.pixels_per_scanline))
self.read_scanner_info()
rospy.loginfo("Generating projection patterns...")
graycodes = []
for i in range(max(self.number_of_scanlines)):
graycodes.append(graycodemath.generate_gray_code(i))
self.number_of_projection_patterns = []
for i in range(2):
self.number_of_projection_patterns.append(int(math.ceil(math.log(self.number_of_scanlines[i], 2))))
self.positive_projections = []
self.negative_projections = []
for i in range(2):
self.positive_projections.append([])
self.negative_projections.append([])
for j in range(self.number_of_projection_patterns[i]):
cross_section = cv.CreateMat(1, self.projector_dimensions[i], cv.CV_8UC1)
#Fill in cross section with the associated bit of each gray code
for pixel in range(self.projector_dimensions[i]):
scanline = pixel // self.pixels_per_scanline
scanline_value = graycodemath.get_bit(graycodes[scanline], j) * 255
cross_section[0, pixel] = scanline_value
#If we're doing horizontal scanning, transpose the cross section
if i is 1:
cross_section_transpose = cv.CreateMat(self.projector_info.height, 1, cv.CV_8UC1)
cv.Transpose(cross_section, cross_section_transpose)
cross_section = cross_section_transpose
#Repeat the cross section over the entire image
positive_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Repeat(cross_section, positive_projection)
#Create a negative of the pattern for thresholding
negative_projection = cv.CreateMat(self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Not(positive_projection, negative_projection)
#Fade the borders of the patterns to avoid artifacts at the edges of projection
positive_projection_faded = fade_edges(positive_projection, 20)
negative_projection_faded = fade_edges(negative_projection, 20)
self.positive_projections[i].append(positive_projection_faded)
self.negative_projections[i].append(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.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.pixel_associations_msg is not None:
pixel_associations_pub.publish(self.pixel_associations_msg)
if self.point_cloud_msg is not None:
point_cloud_pub.publish(self.point_cloud_msg)
rate.sleep()