def __init__(self, parent):
""""""
ExpandablePanel.__init__(self,
parent,
_("Scan Parameters"),
hasUndo=False,
hasRestore=False)
self.driver = Driver.Instance()
self.simpleScan = SimpleScan.Instance()
self.textureScan = TextureScan.Instance()
self.pcg = PointCloudGenerator.Instance()
self.main = self.GetParent().GetParent().GetParent().GetParent()
self.parent = parent
self.lastScan = profile.getProfileSetting('scan_type')
self.clearSections()
section = self.createSection('scan_parameters')
section.addItem(
ComboBox,
'scan_type',
tooltip=
_("Simple Scan algorithm captures only the geometry using one image. Texture Scan algorithm captures also the texture using two images"
))
section.addItem(ComboBox, 'use_laser')
if not sys.isWindows() and not sys.isDarwin():
section.addItem(CheckBox, 'fast_scan')
def serialList(self):
baselist = []
if sys.isWindows():
import _winreg
try:
key = _winreg.OpenKey(_winreg.HKEY_LOCAL_MACHINE,
"HARDWARE\\DEVICEMAP\\SERIALCOMM")
i = 0
while True:
try:
values = _winreg.EnumValue(key, i)
except:
return baselist
if 'USBSER' in values[0] or 'VCP' in values[
0] or '\Device\Serial' in values[0]:
baselist.append(values[1])
i += 1
except:
return baselist
else:
for device in [
'/dev/ttyACM*', '/dev/ttyUSB*', "/dev/tty.usb*",
"/dev/tty.wchusb*", "/dev/cu.*", "/dev/rfcomm*"
]:
baselist = baselist + glob.glob(device)
return baselist
def onPreferences(self, event):
if sys.isWindows():
self.simpleScan.stop()
self.textureScan.stop()
self.laserTriangulation.cancel()
self.platformExtrinsics.cancel()
self.controlWorkbench.videoView.stop()
self.calibrationWorkbench.videoView.stop()
self.calibrationWorkbench.cameraIntrinsicsMainPage.videoView.stop()
self.calibrationWorkbench.laserTriangulationMainPage.videoView.stop()
self.calibrationWorkbench.platformExtrinsicsMainPage.videoView.stop()
self.scanningWorkbench.videoView.stop()
self.driver.board.setUnplugCallback(None)
self.driver.camera.setUnplugCallback(None)
self.controlWorkbench.updateStatus(False)
self.calibrationWorkbench.updateStatus(False)
self.scanningWorkbench.updateStatus(False)
self.driver.disconnect()
waitCursor = wx.BusyCursor()
prefDialog = PreferencesDialog(self)
prefDialog.ShowModal()
self.updateDriverProfile()
self.controlWorkbench.updateCallbacks()
self.calibrationWorkbench.updateCallbacks()
self.scanningWorkbench.updateCallbacks()
def onPreferences(self, event):
if sys.isWindows():
self.simpleScan.stop()
self.textureScan.stop()
self.laserTriangulation.cancel()
self.platformExtrinsics.cancel()
self.controlWorkbench.videoView.stop()
self.calibrationWorkbench.videoView.stop()
self.calibrationWorkbench.cameraIntrinsicsMainPage.videoView.stop()
self.calibrationWorkbench.laserTriangulationMainPage.videoView.stop()
self.calibrationWorkbench.platformExtrinsicsMainPage.videoView.stop()
self.scanningWorkbench.videoView.stop()
self.driver.board.setUnplugCallback(None)
self.driver.camera.setUnplugCallback(None)
self.controlWorkbench.updateStatus(False)
self.calibrationWorkbench.updateStatus(False)
self.scanningWorkbench.updateStatus(False)
self.driver.disconnect()
waitCursor = wx.BusyCursor()
prefDialog = PreferencesDialog(self)
prefDialog.ShowModal()
self.updateDriverProfile()
self.controlWorkbench.updateCallbacks()
self.calibrationWorkbench.updateCallbacks()
self.scanningWorkbench.updateCallbacks()
def onMachineSettings(self, event):
if sys.isWindows():
self.simpleScan.stop()
self.textureScan.stop()
self.laserTriangulation.cancel()
self.platformExtrinsics.cancel()
self.controlWorkbench.videoView.stop()
self.calibrationWorkbench.videoView.stop()
self.calibrationWorkbench.cameraIntrinsicsMainPage.videoView.stop()
self.calibrationWorkbench.laserTriangulationMainPage.videoView.stop()
self.calibrationWorkbench.platformExtrinsicsMainPage.videoView.stop()
self.scanningWorkbench.videoView.stop()
self.driver.board.setUnplugCallback(None)
self.driver.camera.setUnplugCallback(None)
self.controlWorkbench.updateStatus(False)
self.calibrationWorkbench.updateStatus(False)
self.scanningWorkbench.updateStatus(False)
self.driver.disconnect()
waitCursor = wx.BusyCursor()
MachineDialog = MachineSettingsDialog(self)
ret = MachineDialog.ShowModal()
if ret == wx.ID_OK:
self.scanningWorkbench.sceneView._drawMachine()
profile.saveMachineSettings(os.path.join(profile.getBasePath(), profile.getMachineSettingFileName()))
self.scanningWorkbench.controls.panels["point_cloud_generation"].updateProfile()
self.controlWorkbench.updateCallbacks()
self.calibrationWorkbench.updateCallbacks()
self.scanningWorkbench.updateCallbacks()
def getPathForTools(name):
if system.isWindows():
path = getPathForResource(resourceBasePath, 'tools/windows', name)
elif system.isDarwin():
path = getPathForResource(resourceBasePath, 'tools/darwin', name)
else:
path = getPathForResource(resourceBasePath, 'tools/linux', name)
return path
def getPathForTools(name):
if system.isWindows():
path = getPathForResource(resourceBasePath, "tools/windows", name)
elif system.isDarwin():
path = getPathForResource(resourceBasePath, "tools/darwin", name)
else:
path = getPathForResource(resourceBasePath, "tools/linux", name)
return path
def setExposure(self, value): if self.isConnected: if sys.isDarwin(): ctl = self.controls['UVCC_REQ_EXPOSURE_ABS'] value = int(value * self.relExposure) ctl.set_val(value) elif sys.isWindows(): value = int(round(-math.log(value)/math.log(2))) self.capture.set(cv2.cv.CV_CAP_PROP_EXPOSURE, value) else: value = int(value) / self.maxExposure self.capture.set(cv2.cv.CV_CAP_PROP_EXPOSURE, value)
def setExposure(self, value):
if self.isConnected:
if sys.isDarwin():
ctl = self.controls['UVCC_REQ_EXPOSURE_ABS']
value = int(value * self.relExposure)
ctl.set_val(value)
elif sys.isWindows():
value = int(round(-math.log(value) / math.log(2)))
self.capture.set(cv2.cv.CV_CAP_PROP_EXPOSURE, value)
else:
value = int(value) / self.maxExposure
self.capture.set(cv2.cv.CV_CAP_PROP_EXPOSURE, value)
def getExposure(self): if self.isConnected: if sys.isDarwin(): ctl = self.controls['UVCC_REQ_EXPOSURE_ABS'] value = ctl.get_val() value /= self.relExposure elif sys.isWindows(): value = self.capture.get(cv2.cv.CV_CAP_PROP_EXPOSURE) value = 2**-value else: value = self.capture.get(cv2.cv.CV_CAP_PROP_EXPOSURE) value *= self.maxExposure return value
def getExposure(self):
if self.isConnected:
if sys.isDarwin():
ctl = self.controls['UVCC_REQ_EXPOSURE_ABS']
value = ctl.get_val()
value /= self.relExposure
elif sys.isWindows():
value = self.capture.get(cv2.cv.CV_CAP_PROP_EXPOSURE)
value = 2**-value
else:
value = self.capture.get(cv2.cv.CV_CAP_PROP_EXPOSURE)
value *= self.maxExposure
return value
def videoList(self):
baselist = []
if sys.isWindows():
count = self.countCameras()
for i in xrange(count):
baselist.append(str(i))
elif sys.isDarwin():
for device in Camera_List():
baselist.append(str(device.src_id))
else:
for device in ['/dev/video*']:
baselist = baselist + glob.glob(device)
return baselist
def videoList(self):
baselist=[]
if sys.isWindows():
count = self.countCameras()
for i in xrange(count):
baselist.append(str(i))
elif sys.isDarwin():
for device in Camera_List():
baselist.append(str(device.src_id))
else:
for device in ['/dev/video*']:
baselist = baselist + glob.glob(device)
return baselist
##-- END TODO
def getBasePath():
"""
:return: The path in which the current configuration files are stored. This depends on the used OS.
"""
if system.isWindows():
basePath = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), ".."))
#If we have a frozen python install, we need to step out of the library.zip
if hasattr(sys, 'frozen'):
basePath = os.path.normpath(os.path.join(basePath, ".."))
else:
basePath = os.path.expanduser('~/.horus/')
if not os.path.isdir(basePath):
try:
os.makedirs(basePath)
except:
print "Failed to create directory: %s" % (basePath)
return basePath
def __init__(self, parent):
""""""
ExpandablePanel.__init__(self, parent, _("Scan Parameters"), hasUndo=False, hasRestore=False)
self.driver = Driver.Instance()
self.simpleScan = SimpleScan.Instance()
self.textureScan = TextureScan.Instance()
self.pcg = PointCloudGenerator.Instance()
self.main = self.GetParent().GetParent().GetParent().GetParent()
self.parent = parent
self.lastScan = profile.getProfileSetting('scan_type')
self.clearSections()
section = self.createSection('scan_parameters')
section.addItem(ComboBox, 'scan_type', tooltip=_("Simple Scan algorithm captures only the geometry using one image. Texture Scan algorithm captures also the texture using two images"))
section.addItem(ComboBox, 'use_laser')
if not sys.isWindows() and not sys.isDarwin():
section.addItem(CheckBox, 'fast_scan')
def getPatternPosition(self, step, board, camera): t = None if sys.isWindows(): flush = 2 elif sys.isDarwin(): flush = 2 else: flush = 1 image = camera.captureImage(flush=True, flushValue=flush) if image is not None: self.image = image ret = self.solvePnp(image, self.objpoints, self.cameraMatrix, self.distortionVector, self.patternColumns, self.patternRows) if ret is not None: if ret[0]: t = ret[2] board.setRelativePosition(step) board.moveMotor() return t
def getPatternPosition(self, step, board, camera): t = None if sys.isWindows(): flush = 2 elif sys.isDarwin(): flush = 2 else: flush = 1 image = camera.captureImage(flush=True, flushValue=flush) if image is not None: self.image = image ret = self.solvePnp(image, self.objpoints, self.cameraMatrix, self.distortionVector, self.patternColumns, self.patternRows) if ret is not None: self.patron += 1 if ret[0]: t = ret[2] board.setRelativePosition(step) board.moveMotor() print 'Patron leido ',self.patron,' veces' return t
def serialList(self):
baselist=[]
if sys.isWindows():
import _winreg
try:
key=_winreg.OpenKey(_winreg.HKEY_LOCAL_MACHINE,"HARDWARE\\DEVICEMAP\\SERIALCOMM")
i=0
while True:
try:
values = _winreg.EnumValue(key, i)
except:
return baselist
if 'USBSER' in values[0] or 'VCP' in values[0] or '\Device\Serial' in values[0]:
baselist.append(values[1])
i+=1
except:
return baselist
else:
for device in ['/dev/ttyACM*', '/dev/ttyUSB*', "/dev/tty.usb*", "/dev/tty.wchusb*", "/dev/cu.*", "/dev/rfcomm*"]:
baselist = baselist + glob.glob(device)
return baselist
def _getExecutableUrl(version):
url = None
import platform
if sys.isLinux():
url = "https://launchpad.net/~bqopensource/+archive/ubuntu/horus/+files/"
url += "horus_"
url += version+"-bq1~"
url += platform.linux_distribution()[2]+"1_"
if platform.architecture()[0] == '64bit':
url += "amd64.deb"
elif platform.architecture()[0] == '32bit':
url += "i386.deb"
elif sys.isWindows():
url = "storage.googleapis.com/bq-horus/releases/"
url += "Horus_"
url += version+".exe"
elif sys.isDarwin():
url = "https://storage.googleapis.com/bq-horus/releases/"
url += "Horus_"
url += version+".dmg"
del platform
return url
def __init__(self, parent=None, cameraId=0):
self.parent = parent
self.cameraId = cameraId
self.capture = None
self.isConnected = False
self.reading = False
self.framerate = 30
self.width = 800
self.height = 600
self.useDistortion = False
self.cameraMatrix = None
self.distortionVector = None
self.distCameraMatrix = None
if sys.isWindows():
self.numberFramesFail = 3
self.maxBrightness = 1.
self.maxContrast = 1.
self.maxSaturation = 1.
elif sys.isDarwin():
self.numberFramesFail = 1
self.maxBrightness = 255.
self.maxContrast = 255.
self.maxSaturation = 255.
self.relExposure = 10.
else:
self.numberFramesFail = 3
self.maxBrightness = 255.
self.maxContrast = 255.
self.maxSaturation = 255.
self.maxExposure = 1000.
self.unplugCallback = None
self._n = 0 # Check if command fails
def __init__(self, parent=None, cameraId=0): self.parent = parent self.cameraId = cameraId self.capture = None self.isConnected = False self.reading = False self.framerate = 30 self.width = 800 self.height = 600 self.useDistortion = False self.cameraMatrix = None self.distortionVector = None self.distCameraMatrix = None if sys.isWindows(): self.numberFramesFail = 3 self.maxBrightness = 1. self.maxContrast = 1. self.maxSaturation = 1. elif sys.isDarwin(): self.numberFramesFail = 1 self.maxBrightness = 255. self.maxContrast = 255. self.maxSaturation = 255. self.relExposure = 10. else: self.numberFramesFail = 3 self.maxBrightness = 255. self.maxContrast = 255. self.maxSaturation = 255. self.maxExposure = 1000. self.unplugCallback = None self._n = 0 # Check if command fails
def updateCallbacks(self):
section = self.sections['scan_parameters']
section.updateCallback('scan_type', self.setCurrentScan)
section.updateCallback('use_laser', self.setUseLaser)
if not sys.isWindows() and not sys.isDarwin():
section.updateCallback('fast_scan', self.setFastScan)
def _captureThread(self):
""""""
linea = 0
if sys.isWindows() or sys.isDarwin():
flush_both = 3
flush_single = 1
else:
flush_both = 1
flush_single = 0
#-- Switch off the lasers
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
self.driver.board.setLeftLaserOn()
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.driver.board.setRightLaserOn()
while self.runCapture:
if not self.inactive:
if abs(self.theta * 180.0 / np.pi) <= 360.0:
begin = time.time()
#-- Left laser
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
imageLaserLeft = self.driver.camera.captureImage(
flush=True, flushValue=flush_single)
linea += 1
#-- Right laser
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
imageLaserRight = self.driver.camera.captureImage(
flush=True, flushValue=flush_single)
linea += 1
##-- Both laser
if self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.driver.board.setLeftLaserOn()
self.driver.board.setRightLaserOff()
imgLaserLeft = self.driver.camera.captureImage(
flush=True, flushValue=flush_both)
linea += 1
self.driver.board.setRightLaserOn()
self.driver.board.setLeftLaserOff()
imgLaserRight = self.driver.camera.captureImage(
flush=True, flushValue=flush_both)
linea += 1
print "> {0} deg < > {1} lin <".format(
self.theta * 180.0 / np.pi, linea)
self.theta -= self.pcg.degrees * self.pcg.rad
#-- Move motor
if self.moveMotor:
self.driver.board.setRelativePosition(self.pcg.degrees)
self.driver.board.moveMotor()
else:
time.sleep(0.05)
end = time.time()
print "Capture: {0} ms".format(int((end - begin) * 1000))
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
self.imagesQueue.put(('left', imageLaserLeft))
del imageLaserLeft
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.imagesQueue.put(('right', imageLaserRight))
del imageLaserRight
if self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.imagesQueue.put(('both_left', imgLaserLeft))
self.imagesQueue.put(('both_right', imgLaserRight))
del imgLaserLeft
del imgLaserRight
else:
if self.generatePointCloud:
self._stopCapture()
break
else:
time.sleep(0.1)
#-- Disable board
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
self.driver.board.disableMotor()
def _start(self, progressCallback, afterCallback):
XL = None
XR = None
if sys.isWindows():
flush = 2
elif sys.isDarwin():
flush = 2
else:
flush = 1
if self.driver.isConnected:
board = self.driver.board
camera = self.driver.camera
##-- Switch off lasers
board.setLeftLaserOff()
board.setRightLaserOff()
##-- Setup motor
step = 5
angle = 0
board.setSpeedMotor(1)
board.enableMotor()
board.setSpeedMotor(150)
board.setAccelerationMotor(150)
time.sleep(0.1)
if progressCallback is not None:
progressCallback(0)
while self.isCalibrating and abs(angle) < 180:
if progressCallback is not None:
progressCallback(1.11*abs(angle/2.))
angle += step
camera.setExposure(profile.getProfileSettingNumpy('exposure_calibration'))
#-- Image acquisition
imageRaw = camera.captureImage(flush=True, flushValue=flush)
#-- Pattern detection
ret = self.getPatternPlane(imageRaw)
if ret is not None:
step = 4 #2
d, n, corners = ret
camera.setExposure(profile.getProfileSettingNumpy('exposure_calibration')/2.)
#-- Image laser acquisition
imageRawLeft = camera.captureImage(flush=True, flushValue=flush)
board.setLeftLaserOn()
imageLeft = camera.captureImage(flush=True, flushValue=flush)
board.setLeftLaserOff()
self.image = imageLeft
if imageLeft is None:
break
imageRawRight = camera.captureImage(flush=True, flushValue=flush)
board.setRightLaserOn()
imageRight = camera.captureImage(flush=True, flushValue=flush)
board.setRightLaserOff()
self.image = imageRight
if imageRight is None:
break
#-- Pattern ROI mask
imageRaw = self.cornersMask(imageRaw, corners)
imageLeft = self.cornersMask(imageLeft, corners)
imageRawLeft = self.cornersMask(imageRawLeft, corners)
imageRight = self.cornersMask(imageRight, corners)
imageRawRight = self.cornersMask(imageRawRight, corners)
#-- Line segmentation
uL, vL = self.getLaserLine(imageLeft, imageRawLeft)
uR, vR = self.getLaserLine(imageRight, imageRawRight)
#-- Point Cloud generation
xL = self.getPointCloudLaser(uL, vL, d, n)
if xL is not None:
if XL is None:
XL = xL
else:
XL = np.concatenate((XL,xL))
xR = self.getPointCloudLaser(uR, vR, d, n)
if xR is not None:
if XR is None:
XR = xR
else:
XR = np.concatenate((XR,xR))
else:
step = 5
self.image = imageRaw
board.setRelativePosition(step)
board.moveMotor()
time.sleep(0.1)
# self.saveScene('XL.ply', XL)
# self.saveScene('XR.ply', XR)
#-- Compute planes
dL, nL, stdL = self.computePlane(XL, 'l')
dR, nR, stdR = self.computePlane(XR, 'r')
##-- Switch off lasers
board.setLeftLaserOff()
board.setRightLaserOff()
#-- Disable motor
board.disableMotor()
#-- Restore camera exposure
camera.setExposure(profile.getProfileSettingNumpy('exposure_calibration'))
if self.isCalibrating and nL is not None and nR is not None:
response = (True, ((dL, nL, stdL), (dR, nR, stdR)))
if progressCallback is not None:
progressCallback(100)
else:
if self.isCalibrating:
response = (False, Error.CalibrationError)
else:
response = (False, Error.CalibrationCanceled)
self.image = None
if afterCallback is not None:
afterCallback(response)
def updateCallbacks(self):
section = self.sections['scan_parameters']
section.updateCallback('scan_type', self.setCurrentScan)
section.updateCallback('use_laser', self.setUseLaser)
if not sys.isWindows() and not sys.isDarwin():
section.updateCallback('fast_scan', self.setFastScan)
def _captureThread(self):
""""""
imgRaw = None
imgLaserLeft = None
imgLaserRight = None
#-- Switch off the lasers
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
if sys.isWindows() or sys.isDarwin():
flush = 3
else:
flush = 1
while self.runCapture:
if not self.inactive:
if abs(self.theta * 180.0 / np.pi) <= 360.0:
begin = time.time()
if self.fastScan: #-- FAST METHOD (only for linux)
if self.driver.camera.isConnected:
self.driver.camerareading = True
#-- Left laser
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
self.driver.board.setLeftLaserOff()
imgLaserLeft = self.driver.camera.capture.read()[1]
imgRaw = self.driver.camera.capture.read()[1]
self.driver.board.setLeftLaserOn()
self.driver.camerareading = False
if imgRaw is None or imgLaserLeft is None:
self.driver.camera._fail()
else:
imgRaw = cv2.transpose(imgRaw)
imgRaw = cv2.flip(imgRaw, 1)
imgRaw = cv2.cvtColor(imgRaw, cv2.COLOR_BGR2RGB)
imgLaserRight = None
imgLaserLeft = cv2.transpose(imgLaserLeft)
imgLaserLeft = cv2.flip(imgLaserLeft, 1)
imgLaserLeft = cv2.cvtColor(imgLaserLeft, cv2.COLOR_BGR2RGB)
#-- Right laser
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.driver.board.setRightLaserOff()
imgLaserRight = self.driver.camera.capture.read()[1]
imgRaw = self.driver.camera.capture.read()[1]
self.driver.board.setRightLaserOn()
self.driver.camerareading = False
if imgRaw is None or imgLaserRight is None:
self.driver.camera._fail()
else:
imgRaw = cv2.transpose(imgRaw)
imgRaw = cv2.flip(imgRaw, 1)
imgRaw = cv2.cvtColor(imgRaw, cv2.COLOR_BGR2RGB)
imgLaserLeft = None
imgLaserRight = cv2.transpose(imgLaserRight)
imgLaserRight = cv2.flip(imgLaserRight, 1)
imgLaserRight = cv2.cvtColor(imgLaserRight, cv2.COLOR_BGR2RGB)
##-- Both laser
if self.pcg.useLeftLaser and self.pcg.useRightLaser:
imgRaw = self.driver.camera.capture.read()[1]
self.driver.board.setLeftLaserOn()
imgLaserLeft = self.driver.camera.capture.read()[1]
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOn()
imgLaserRight = self.driver.camera.capture.read()[1]
self.driver.board.setRightLaserOff()
imgRaw = self.driver.camera.capture.read()[1]
self.driver.camerareading = False
if imgRaw is None or imgLaserLeft is None or imgLaserRight is None:
self.driver.camera._fail()
else:
#Begin modification
rightlasername= './rightlaser-%f.tif' %(self._theta)
leftlasername= './leftlaser-%f.tif' %(self._theta)
texturename= './texture-%f.tif' %(self.theta)
cv2.imwrite(rightlasername,imgLaserRight)
cv2.imwrite(leftlasername,imgLaserLeft)
cv2.imwrite(texturename,imgRaw)
#End modification
imgRaw = cv2.transpose(imgRaw)
imgRaw = cv2.flip(imgRaw, 1)
imgRaw = cv2.cvtColor(imgRaw, cv2.COLOR_BGR2RGB)
imgLaserLeft = cv2.transpose(imgLaserLeft)
imgLaserLeft = cv2.flip(imgLaserLeft, 1)
imgLaserLeft = cv2.cvtColor(imgLaserLeft, cv2.COLOR_BGR2RGB)
imgLaserRight = cv2.transpose(imgLaserRight)
imgLaserRight = cv2.flip(imgLaserRight, 1)
imgLaserRight = cv2.cvtColor(imgLaserRight, cv2.COLOR_BGR2RGB)
else: #-- SLOW METHOD
#-- Switch off laser
if self.pcg.useLeftLaser:
self.driver.board.setLeftLaserOff()
if self.pcg.useRightLaser:
self.driver.board.setRightLaserOff()
#-- Capture images
imgRaw = self.driver.camera.captureImage(flush=True, flushValue=flush)
if self.pcg.useLeftLaser:
self.driver.board.setLeftLaserOn()
self.driver.board.setRightLaserOff()
imgLaserLeft = self.driver.camera.captureImage(flush=True, flushValue=flush)
else:
imgLaserLeft = None
if self.pcg.useRightLaser:
self.driver.board.setRightLaserOn()
self.driver.board.setLeftLaserOff()
imgLaserRight = self.driver.camera.captureImage(flush=True, flushValue=flush)
else:
imgLaserRight = None
#Begin modification
rightlasername= './rightlaser-%f.tif' %(self._theta)
leftlasername= './leftlaser-%f.tif' %(self._theta)
texturename= './texture-%f.tif' %(self.theta)
cv2.imwrite(rightlasername,imgLaserRight)
cv2.imwrite(leftlasername,imgLaserLeft)
cv2.imwrite(texturename,imgRaw)
#End modification
print "> {0} deg <".format(self.theta * 180.0 / np.pi)
self.theta -= self.pcg.degrees * self.pcg.rad
#-- Move motor
if self.moveMotor:
self.driver.board.setRelativePosition(self.pcg.degrees)
self.driver.board.moveMotor()
else:
time.sleep(0.05)
end = time.time()
print "Capture: {0} ms".format(int((end-begin)*1000))
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
if imgRaw is not None and imgLaserLeft is not None:
self.imagesQueue.put(('left',imgRaw,imgLaserLeft))
del imgLaserLeft
elif self.pcg.useRightLaser and not self.pcg.useLeftLaser:
if imgRaw is not None and imgLaserRight is not None:
self.imagesQueue.put(('right',imgRaw,imgLaserRight))
del imgLaserRight
elif self.pcg.useRightLaser and self.pcg.useLeftLaser:
if imgRaw is not None and imgLaserLeft is not None and imgLaserRight is not None:
self.imagesQueue.put(('both_left',imgRaw,imgLaserLeft))
self.imagesQueue.put(('both_right',imgRaw,imgLaserRight))
del imgLaserLeft
del imgLaserRight
del imgRaw
else:
if self.generatePointCloud:
self._stopCapture()
break
else:
time.sleep(0.1)
#-- Disable board
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
self.driver.board.disableMotor()
def _captureThread(self):
""""""
if sys.isWindows() or sys.isDarwin():
flush_both = 3
flush_single = 1
else:
flush_both = 1
flush_single = 0
#-- Switch off the lasers
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
self.driver.board.setLeftLaserOn()
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.driver.board.setRightLaserOn()
while self.runCapture:
if not self.inactive:
if abs(self.theta * 180.0 / np.pi) <= 360.0:
begin = time.time()
#-- Left laser
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
imageLaserLeft = self.driver.camera.captureImage(flush=True, flushValue=flush_single)
#-- Right laser
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
imageLaserRight = self.driver.camera.captureImage(flush=True, flushValue=flush_single)
##-- Both laser
if self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.driver.board.setLeftLaserOn()
self.driver.board.setRightLaserOff()
imgLaserLeft = self.driver.camera.captureImage(flush=True, flushValue=flush_both)
self.driver.board.setRightLaserOn()
self.driver.board.setLeftLaserOff()
imgLaserRight = self.driver.camera.captureImage(flush=True, flushValue=flush_both)
print "> {0} deg <".format(self.theta * 180.0 / np.pi)
self.theta -= self.pcg.degrees * self.pcg.rad
#-- Move motor
if self.moveMotor:
self.driver.board.setRelativePosition(self.pcg.degrees)
self.driver.board.moveMotor()
else:
time.sleep(0.05)
end = time.time()
print "Capture: {0} ms".format(int((end-begin)*1000))
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
self.imagesQueue.put(('left',imageLaserLeft))
del imageLaserLeft
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.imagesQueue.put(('right',imageLaserRight))
del imageLaserRight
if self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.imagesQueue.put(('both_left',imgLaserLeft))
self.imagesQueue.put(('both_right',imgLaserRight))
del imgLaserLeft
del imgLaserRight
else:
if self.generatePointCloud:
self._stopCapture()
break
else:
time.sleep(0.1)
#-- Disable board
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
self.driver.board.disableMotor()
def _captureThread(self):
""""""
linea = 0
imgRaw = None
imgLaserLeft = None
imgLaserRight = None
#-- Switch off the lasers
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
if sys.isWindows() or sys.isDarwin():
flush = 3
else:
flush = 1
while self.runCapture:
if not self.inactive:
if abs(self.theta * 180.0 / np.pi) <= 360.0:
begin = time.time()
if self.fastScan: #-- FAST METHOD (only for linux)
if self.driver.camera.isConnected:
self.driver.camerareading = True
#-- Left laser
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
self.driver.board.setLeftLaserOff()
imgLaserLeft = self.driver.camera.capture.read(
)[1]
imgRaw = self.driver.camera.capture.read()[1]
self.driver.board.setLeftLaserOn()
self.driver.camerareading = False
linea += 1
if imgRaw is None or imgLaserLeft is None:
self.driver.camera._fail()
else:
imgRaw = cv2.transpose(imgRaw)
imgRaw = cv2.flip(imgRaw, 1)
imgRaw = cv2.cvtColor(
imgRaw, cv2.COLOR_BGR2RGB)
imgLaserRight = None
imgLaserLeft = cv2.transpose(imgLaserLeft)
imgLaserLeft = cv2.flip(imgLaserLeft, 1)
imgLaserLeft = cv2.cvtColor(
imgLaserLeft, cv2.COLOR_BGR2RGB)
#-- Right laser
if not self.pcg.useLeftLaser and self.pcg.useRightLaser:
self.driver.board.setRightLaserOff()
imgLaserRight = self.driver.camera.capture.read(
)[1]
imgRaw = self.driver.camera.capture.read()[1]
self.driver.board.setRightLaserOn()
self.driver.camerareading = False
linea += 1
if imgRaw is None or imgLaserRight is None:
self.driver.camera._fail()
else:
imgRaw = cv2.transpose(imgRaw)
imgRaw = cv2.flip(imgRaw, 1)
imgRaw = cv2.cvtColor(
imgRaw, cv2.COLOR_BGR2RGB)
imgLaserLeft = None
imgLaserRight = cv2.transpose(
imgLaserRight)
imgLaserRight = cv2.flip(imgLaserRight, 1)
imgLaserRight = cv2.cvtColor(
imgLaserRight, cv2.COLOR_BGR2RGB)
##-- Both laser
if self.pcg.useLeftLaser and self.pcg.useRightLaser:
imgRaw = self.driver.camera.capture.read()[1]
self.driver.board.setLeftLaserOn()
imgLaserLeft = self.driver.camera.capture.read(
)[1]
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOn()
imgLaserRight = self.driver.camera.capture.read(
)[1]
self.driver.board.setRightLaserOff()
imgRaw = self.driver.camera.capture.read()[1]
self.driver.camerareading = False
linea += 2
if imgRaw is None or imgLaserLeft is None or imgLaserRight is None:
self.driver.camera._fail()
else:
imgRaw = cv2.transpose(imgRaw)
imgRaw = cv2.flip(imgRaw, 1)
imgRaw = cv2.cvtColor(
imgRaw, cv2.COLOR_BGR2RGB)
imgLaserLeft = cv2.transpose(imgLaserLeft)
imgLaserLeft = cv2.flip(imgLaserLeft, 1)
imgLaserLeft = cv2.cvtColor(
imgLaserLeft, cv2.COLOR_BGR2RGB)
imgLaserRight = cv2.transpose(
imgLaserRight)
imgLaserRight = cv2.flip(imgLaserRight, 1)
imgLaserRight = cv2.cvtColor(
imgLaserRight, cv2.COLOR_BGR2RGB)
else: #-- SLOW METHOD
#-- Switch off laser
if self.pcg.useLeftLaser:
self.driver.board.setLeftLaserOff()
if self.pcg.useRightLaser:
self.driver.board.setRightLaserOff()
#-- Capture images
imgRaw = self.driver.camera.captureImage(
flush=True, flushValue=flush)
if self.pcg.useLeftLaser:
self.driver.board.setLeftLaserOn()
self.driver.board.setRightLaserOff()
imgLaserLeft = self.driver.camera.captureImage(
flush=True, flushValue=flush)
linea += 1
else:
imgLaserLeft = None
if self.pcg.useRightLaser:
self.driver.board.setRightLaserOn()
self.driver.board.setLeftLaserOff()
imgLaserRight = self.driver.camera.captureImage(
flush=True, flushValue=flush)
linea += 1
else:
imgLaserRight = None
print "> {0} deg < > {1} lin <".format(
self.theta * 180.0 / np.pi, linea)
self.theta -= self.pcg.degrees * self.pcg.rad
#-- Move motor
if self.moveMotor:
self.driver.board.setRelativePosition(self.pcg.degrees)
self.driver.board.moveMotor()
else:
time.sleep(0.05)
end = time.time()
print "Capture: {0} ms".format(int((end - begin) * 1000))
if self.pcg.useLeftLaser and not self.pcg.useRightLaser:
if imgRaw is not None and imgLaserLeft is not None:
self.imagesQueue.put(
('left', imgRaw, imgLaserLeft))
del imgLaserLeft
elif self.pcg.useRightLaser and not self.pcg.useLeftLaser:
if imgRaw is not None and imgLaserRight is not None:
self.imagesQueue.put(
('right', imgRaw, imgLaserRight))
del imgLaserRight
elif self.pcg.useRightLaser and self.pcg.useLeftLaser:
if imgRaw is not None and imgLaserLeft is not None and imgLaserRight is not None:
self.imagesQueue.put(
('both_left', imgRaw, imgLaserLeft))
self.imagesQueue.put(
('both_right', imgRaw, imgLaserRight))
del imgLaserLeft
del imgLaserRight
del imgRaw
else:
if self.generatePointCloud:
self._stopCapture()
break
else:
time.sleep(0.1)
#-- Disable board
self.driver.board.setLeftLaserOff()
self.driver.board.setRightLaserOff()
self.driver.board.disableMotor()
def _start(self, progressCallback, afterCallback):
XL = None
XR = None
planol = 0
planor = 0
if sys.isWindows():
flush = 2
elif sys.isDarwin():
flush = 2
else:
flush = 1
if self.driver.isConnected:
board = self.driver.board
camera = self.driver.camera
##-- Switch off lasers
board.setLeftLaserOff()
board.setRightLaserOff()
##-- Setup motor
step = 2
angle = 0
board.setSpeedMotor(1)
board.enableMotor()
board.setSpeedMotor(150)
board.setAccelerationMotor(150)
time.sleep(0.2)
board.setRelativePosition(-90)
board.moveMotor()
time.sleep(0.2)
if progressCallback is not None:
progressCallback(0)
while self.isCalibrating and abs(angle) < 180:
if progressCallback is not None:
progressCallback(1.11*abs(angle/2.))
angle += step
print '> ',angle,' deg <'
camera.setExposure(profile.getProfileSettingNumpy('exposure_calibration'))
#-- Image acquisition
imageRaw = camera.captureImage(flush=True, flushValue=flush)
#-- Pattern detection
ret = self.getPatternPlane(imageRaw)
if ret is not None:
step = 1 #2
d, n, corners = ret
camera.setExposure(profile.getProfileSettingNumpy('exposure_calibration')/2.)
#-- Image laser acquisition
imageRawLeft = camera.captureImage(flush=True, flushValue=flush)
board.setLeftLaserOn()
#time.sleep(0.2)
imageLeft = camera.captureImage(flush=True, flushValue=flush)
#time.sleep(0.2)
board.setLeftLaserOff()
self.image = imageLeft
if imageLeft is None:
break
imageRawRight = camera.captureImage(flush=True, flushValue=flush)
board.setRightLaserOn()
#time.sleep(0.2)
imageRight = camera.captureImage(flush=True, flushValue=flush)
#time.sleep(0.2)
board.setRightLaserOff()
self.image = imageRight
if imageRight is None:
break
#-- Pattern ROI mask
imageRaw = self.cornersMask(imageRaw, corners)
imageLeft = self.cornersMask(imageLeft, corners)
imageRawLeft = self.cornersMask(imageRawLeft, corners)
imageRight = self.cornersMask(imageRight, corners)
imageRawRight = self.cornersMask(imageRawRight, corners)
#-- Line segmentation
uL, vL = self.getLaserLine(imageLeft, imageRawLeft)
uR, vR = self.getLaserLine(imageRight, imageRawRight)
#-- Point Cloud generation
xL = self.getPointCloudLaser(uL, vL, d, n)
if xL is not None:
if XL is None:
XL = xL
else:
XL = np.concatenate((XL,xL))
planol += 1
print 'Plano Izquierdo registrado ',planol,' veces'
xR = self.getPointCloudLaser(uR, vR, d, n)
if xR is not None:
if XR is None:
XR = xR
else:
XR = np.concatenate((XR,xR))
planor += 1
print 'Plano Derecho registrado ',planol,' veces'
else:
step = 2
self.image = imageRaw
print 'No se ha detectado el patron'
board.setRelativePosition(step)
board.moveMotor()
time.sleep(0.2)
# self.saveScene('XL.ply', XL)
# self.saveScene('XR.ply', XR)
#-- Compute planes
print '>>> Computando plano Izquierdo <<<'
dL, nL, stdL = self.computePlane(XL, 'l')
print '>>> Computando plano Derecho <<<'
dR, nR, stdR = self.computePlane(XR, 'r')
##-- Switch off lasers
board.setLeftLaserOff()
board.setRightLaserOff()
#-- Disable motor
board.setRelativePosition(-90)
board.moveMotor()
time.sleep(0.2)
board.disableMotor()
#-- Restore camera exposure
camera.setExposure(profile.getProfileSettingNumpy('exposure_calibration'))
if self.isCalibrating and nL is not None and nR is not None:
response = (True, ((dL, nL, stdL), (dR, nR, stdR)))
if progressCallback is not None:
progressCallback(100)
else:
if self.isCalibrating:
response = (False, Error.CalibrationError)
else:
response = (False, Error.CalibrationCanceled)
self.image = None
if afterCallback is not None:
afterCallback(response)
