def _capture_scanner_calibration(self, position):
image = self._capture_pattern()
#if (position > self.laser_calib_start and position < self.laser_calib_end):
flags = cv2.CALIB_CB_FAST_CHECK | cv2.CALIB_CB_ADAPTIVE_THRESH #| cv2.CALIB_CB_NORMALIZE_IMAGE
#else:
# flags = cv2.CALIB_CB_FAST_CHECK
try:
pose = self._imageprocessor.detect_pose(image, flags)
plane = self._imageprocessor.detect_pattern_plane(pose)
except StandardError as e:
plane = None
self._logger.error(e)
if plane is not None:
distance, normal, corners = plane
self._logger.debug("Pose detected... ")
# Laser triangulation ( Between 60 and 115 degree )
# angel/(360/3200)
try:
exc_info = sys.exc_info()
#Laser Calibration
#if (position > self.laser_calib_start and position < self.laser_calib_end):
alpha = np.rad2deg(math.acos(normal[2] / np.linalg.norm((normal[0], normal[2])))) * math.copysign(1,normal[0])
self._logger.debug("Current Angle is:" + str(alpha))
if abs(alpha) < 35:
#self.settings.camera.contrast = 40
#self.settings.camera.brightness = 70
self._hardwarecontroller.led.off()
for i in xrange(self.config.laser.numbers):
image = self._capture_laser(i)
image = self._imageprocessor.pattern_mask(image, corners)
self.image = image
fs_image = FSImage()
fs_image.save_image(image, self.current_position, "laser", dir_name="calibration")
points_2d, _ = self._imageprocessor.compute_2d_points(image, roi_mask=False, refinement_method='SGF')
point_3d = self._imageprocessor.compute_camera_point_cloud(points_2d, distance, normal)
if self._point_cloud[i] is None:
self._point_cloud[i] = point_3d.T
else:
self._point_cloud[i] = np.concatenate(
(self._point_cloud[i], point_3d.T))
#self.settings.camera.contrast = 40
#self.settings.camera.brightness = 50
# Platform extrinsics
origin = corners[self.config.calibration.pattern.columns * (self.config.calibration.pattern.rows - 1)][0]
origin = np.array([[origin[0]], [origin[1]]])
t = self._imageprocessor.compute_camera_point_cloud(
origin, distance, normal)
except StandardError, e:
self._logger.exception(e)
self._logger.error("Laser Capture Error: "+str(e))
message = {
"message": "LASER_CALIBRATION_ERROR",
"level": "error"
}
# #self._eventmanager.broadcast_client_message(FSEvents.ON_INFO_MESSAGE, message)
t = None
if t is not None:
self.x += [t[0][0]]
self.y += [t[1][0]]
self.z += [t[2][0]]
else:
self.image = image
def _capture_scanner_calibration(self, position):
pattern_image = self._capture_pattern()
if (position >= self.laser_calib_start and position <= self.laser_calib_end):
flags = cv2.CALIB_CB_FAST_CHECK | cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
else:
flags = cv2.CALIB_CB_FAST_CHECK
try:
pose = self._imageprocessor.detect_pose(pattern_image, flags)
plane = self._imageprocessor.detect_pattern_plane(pose)
except Exception as e:
plane = None
self._logger.error(e)
if plane is not None:
distance, normal, corners = plane
self._logger.debug("Pose detected... ")
# Laser triangulation ( Between 60 and 115 degree )
# angel/(360/3200)
try:
exc_info = sys.exc_info()
#Laser Calibration
alpha = np.rad2deg(math.acos(normal[2] / np.linalg.norm((normal[0], normal[2])))) * math.copysign(1, normal[0])
self._logger.debug("Current Angle is:" + str(alpha))
if ((abs(alpha) <= self.LASER_PLANE_CALIBRATION_START_POS_DEGREE) and (position > 0)):
self._hardwarecontroller.led.off()
for i in range(self.config.file.laser.numbers):
image = self._capture_laser(i)
if self.config.file.laser.interleaved == "True":
image = cv2.subtract(image, pattern_image)
image = self._imageprocessor.pattern_mask(image, corners)
# TODO: make image saving contitional in confg --> calibration -> debug -> true/false
fs_image = FSImage()
fs_image.save_image(image, alpha, "laser_"+str(i), dir_name="calibration")
points_2d, _ = self._imageprocessor.compute_2d_points(image, roi_mask=False, refinement_method='RANSAC')
point_3d = self._imageprocessor.compute_camera_point_cloud(points_2d, distance, normal)
if self._point_cloud[i] is None:
self._point_cloud[i] = point_3d.T
else:
self._point_cloud[i] = np.concatenate(
(self._point_cloud[i], point_3d.T))
# Platform extrinsics
origin = corners[self.config.file.calibration.pattern.columns * (self.config.file.calibration.pattern.rows - 1)][0]
origin = np.array([[origin[0]], [origin[1]]])
t = self._imageprocessor.compute_camera_point_cloud(
origin, distance, normal)
except Exception as e:
self._logger.exception(e)
self._logger.error("Laser Capture Error: "+str(e))
message = {
"message": "LASER_CALIBRATION_ERROR",
"level": "error"
}
# #self._eventmanager.broadcast_client_message(FSEvents.ON_INFO_MESSAGE, message)
t = None
if t is not None:
self.x += [t[0][0]]
self.y += [t[1][0]]
self.z += [t[2][0]]
if self.config.file.laser.interleaved == "False":
self._hardwarecontroller.led.on(self.calibration_brightness[0], self.calibration_brightness[1], self.calibration_brightness[2])
class FSImageWorkerProcess(multiprocessing.Process):
def __init__(self, image_task_q, event_q, config, settings, imageprocessor):
super(FSImageWorkerProcess, self).__init__(group=None)
self.image_task_q = image_task_q
self.settings = settings
self.config = config
self.exit = False
self.event_q = event_q
self.image = FSImage()
self.log = logging.getLogger('IMAGE_PROCESSOR THREAD')
self.log.setLevel(logging.DEBUG)
self.image_processor = imageprocessor
self._logger = logging.getLogger(__name__)
self._logger.setLevel(logging.DEBUG)
def run(self):
'''
Image Process runner
'''
self._logger.debug("process "+str(self.pid)+" started")
#import pydevd
#pydevd.settrace('192.168.98.104', port=12011, stdoutToServer=True, stderrToServer=True)
while not self.exit:
if not self.image_task_q.empty():
data = dict()
try:
image_task = self.image_task_q.get_nowait()
if image_task:
# we got a kill pill
if image_task.task_type == "KILL":
self._logger.debug("Killed Worker Process with PID "+str(self.pid))
self.exit = True
break
#print "process "+str(self.pid)+" task "+str(image_task.progress)
if (image_task.task_type == "PROCESS_COLOR_IMAGE"):
self.image.save_image(image_task.image, image_task.progress, image_task.prefix, dir_name=image_task.prefix+'/color_'+image_task.raw_dir)
data['points'] = []
data['image_type'] = 'color'
event = dict()
event['event'] = "ON_IMAGE_PROCESSED"
event['data'] = data
self.event_q.put(event)
if (image_task.task_type == "PROCESS_DEPTH_IMAGE"):
#self._logger.debug("process " + str(self.pid) + " handle image")
try:
angle = float(image_task.progress * 360) / float(image_task.resolution)
#self._logger.debug("Progress "+str(image_task.progress)+" Resolution "+str(image_task.resolution)+" angle "+str(angle))
self.image.save_image(image_task.image, image_task.progress, image_task.prefix, dir_name=image_task.prefix+'/laser_'+image_task.raw_dir)
color_image = self.image.load_image(image_task.progress, image_task.prefix, dir_name=image_task.prefix+'/color_'+image_task.raw_dir)
point_cloud, texture = self.image_processor.process_image(angle, image_task.image, color_image)
# FIXME: Only send event if points is non-empty
except StandardError as e:
self._logger.debug(e)
data['point_cloud'] = point_cloud
data['texture'] = texture
data['image_type'] = 'depth'
#data['progress'] = image_task.progress
#data['resolution'] = image_task.resolution
event = dict()
event['event'] = "ON_IMAGE_PROCESSED"
event['data'] = data
self.event_q.put(event)
except Empty:
time.sleep(0.05)
pass
else:
# thread idle
time.sleep(0.05)
class FSImageWorkerProcess(multiprocessing.Process):
def __init__(self, image_task_q, output_q, config, settings, scanprocessor, imageprocessor):
super(FSImageWorkerProcess, self).__init__(group=None)
self.image_task_q = image_task_q
self.output_q = output_q
self.settings = settings
self.config = config
self.exit = False
self.scanprocessor = scanprocessor
self.image = FSImage()
self.log = logging.getLogger('IMAGE_PROCESSOR THREAD')
self.log.setLevel(logging.DEBUG)
self.image_processor = imageprocessor
self._logger = logging.getLogger(__name__)
self._logger.setLevel(logging.DEBUG)
def run(self):
'''
Image Process runner
'''
self._logger.debug("process "+str(self.pid)+" started")
while not self.exit:
if not self.image_task_q.empty():
data = dict()
try:
image_task = self.image_task_q.get_nowait()
if image_task:
if image_task.task_type == "KILL":
self._logger.debug("Killed Worker Process with PID "+str(self.pid))
self.exit = True
if (image_task.task_type == "PROCESS_COLOR_IMAGE"):
#image = self.image_processor.decode_image(image_task.image)
self.image.save_image(image_task.image, image_task.progress, image_task.prefix, dir_name=image_task.prefix+'/color_'+image_task.raw_dir)
data['points'] = []
data['image_type'] = 'color'
data['laser_index'] = None
self.output_q.put(data)
if (image_task.task_type == "PROCESS_DEPTH_IMAGE"):
self._logger.debug('Image Processing starts.')
try:
#self.image.save_image(image_task.image, image_task.progress, image_task.prefix,
# dir_name=image_task.prefix + '/raw_' + image_task.raw_dir)
image_task.image = self.image_processor.decode_image(image_task.image)
angle = float(image_task.progress * 360) / float(image_task.resolution)
color_image = self.image.load_image(image_task.progress, image_task.prefix, dir_name=image_task.prefix+'/color_'+image_task.raw_dir)
point_cloud, texture = self.image_processor.process_image(angle, image_task.image, color_image, index=image_task.index)
data['point_cloud'] = point_cloud
data['texture'] = texture
data['image_type'] = 'depth'
data['laser_index'] = image_task.index
except Exception as e:
self._logger.debug(e)
self.output_q.put(data)
color_image = None
point_cloud = None
texture = None
self._logger.debug('Image Processing finished.')
image_task = None
except Empty:
time.sleep(0.1)
pass
else:
# thread idle
time.sleep(0.1)