class Camera(Product.Product):
'''
classdocs
'''
def __init__(self, parent = None):
super(Camera, self).__init__(parent)
'''
Constructor
'''
self._vfov = 45.0
self._near = 0.1
self._far = 400
self._eye = QVector3D(10,0,0)
self._up = QVector3D(0,0,1)
self._center = QVector3D(0,0,0)
self._transform = Transforms.Transform()
self._transform.set_producer(self)
self.perspective_override = None
Product.ConstProperty(vars(), Transforms.Transform, 'transform')
Product.InputProperty(vars(), float, 'vfov')
Product.InputProperty(vars(), float, 'near')
Product.InputProperty(vars(), float, 'far')
Product.InputProperty(vars(), QVector3D, 'eye')
Product.InputProperty(vars(), QVector3D, 'up')
Product.InputProperty(vars(), QVector3D, 'center')
@Slot(float, float)
def pan_tilt(self, _eye, _up, dx, dy):
_front = self._center - _eye
_right = QVector3D.crossProduct(_front, _up).normalized()
q = QQuaternion.fromAxisAndAngle(_up, dx) * QQuaternion.fromAxisAndAngle(_right, dy)
self.eye = self._center - q.rotatedVector(_front)
self.up = q.rotatedVector(_up)
def roll(self, _eye, _up, delta):
self.up = QQuaternion.fromAxisAndAngle(self._center - _eye, delta).rotatedVector(_up)
def translate(self, _eye, _center, dx, dy):
_left = QVector3D.crossProduct((_center - _eye), self._up).normalized()
d = _left * dx + self._up * dy
d *= (_eye - _center).length()
self.eye = _eye + d
self.center = _center + d
def view_matrix(self):
mat_v = QMatrix4x4()
mat_v.lookAt(self._eye, self._center, self._up)
return mat_v
def _update(self):
self._transform.set_local_transform(self.view_matrix().inverted()[0])
def __init__(self, parent = None):
super(SensorCapture, self).__init__(parent)
self._traces = Product.VariantProduct()
self._traces.set_producer(self)
self._calibration = ""
self._calibration_data = None
self._specs = None
self._nChannels = 0
class MatrixTransform(Transform):
def __init__(self, parent=None, matrix=QMatrix4x4()):
super(MatrixTransform, self).__init__(parent)
self._matrix = None
self.matrix = matrix
def matrix_cb(self):
self.set_local_transform(self._matrix)
Product.InputProperty(vars(), QMatrix4x4, 'matrix', matrix_cb)
class Rotation(Transform):
def __init__(self, parent=None):
super(Rotation, self).__init__(parent)
self._quaternion = QQuaternion()
def quaternion_cb(self):
m = QMatrix4x4()
m.rotate(self._quaternion)
self.set_local_transform(m)
Product.InputProperty(vars(), QQuaternion, 'quaternion', quaternion_cb)
class Translation(Transform):
def __init__(self, parent=None):
super(Translation, self).__init__(parent)
self._translate = QVector3D()
def translate_cb(self):
m = QMatrix4x4()
m.translate(self._translate)
self.set_local_transform(m)
Product.InputProperty(vars(), QVector3D, 'translate', translate_cb)
class Effect(Product.Product):
def __init__(self,
parent=None,
shader0=None,
point_size=1,
line_width=1,
name=""):
super(Effect, self).__init__(parent)
self._shader0 = None
self._pointSize = 1
self._lineWidth = 1
self.setObjectName(name)
self.shader0 = shader0
self.pointSize = point_size
self.lineWidth = line_width
Product.InputProperty(vars(), GLSLProgram, 'shader0')
Product.InputProperty(vars(), int, 'pointSize')
Product.InputProperty(vars(), int, 'lineWidth')
class Attribs( Product.Product ):
def __init__( self, parent=None, vertices = None, normals = None ):
super(Attribs, self).__init__( parent )
self._vertices = None
self._normals = None
self.vertices = vertices
self.normals = normals
Product.InputProperty(vars(), Array, 'vertices')
Product.InputProperty(vars(), Array, 'normals')
def get_attributes(self):
'''
override this method to add all your attribs
'''
assert self._vertices.ndarray.dtype.type in [np.float32, np.float64], "not float32/64"
assert len(self._vertices.ndarray.shape) == 2, "not 2d matrix"
assert self._vertices.ndarray.shape[1] in [2,3], "not 2d or 3d"
return {"vertices": self._vertices, "normals" : self._normals}
class SegmentationLabelsAttribs(Attribs):
def __init__(self, parent=None):
super(SegmentationLabelsAttribs, self).__init__(parent)
self._labels = None
Product.InputProperty(vars(), Array, 'labels')
def get_attributes(self):
'''
override this method to add all your attribs
'''
a = super(SegmentationLabelsAttribs, self).get_attributes()
a["labels"] = self._labels
return a
class PackageFilter(Product.VariantProduct):
def __init__(self, parent = None):
super(PackageFilter, self).__init__(parent)
self._packages = None
self._specs = None
self._filter = {'ampMin': -2**14, 'ampMax': 2**14, 'distMin': 0, 'distMax': 300, 'rejectedFlags' : []}
Product.InputProperty(vars(), 'QVariant', 'filter')
Product.InputProperty(vars(), Product.VariantProduct, 'packages')
def _update(self):
if self._packages is not None and self._packages._variant is not None:
data = self._packages._variant['data']
f = (self._filter['ampMin'] < data['amplitudes']) \
& (data['amplitudes'] < self._filter['ampMax']) \
& (self._filter['distMin'] < data['distances']) \
& (data['distances'] < self._filter['distMax']) \
& (data['flags'] == 1) #, self._filter['rejectedFlags'], invert = True)
self._variant = self._packages._variant.copy()
self._variant['data'] = np.ascontiguousarray(data[f])
class Transform(Product.Product):
def __init__(self, parent=None):
super(Transform, self).__init__(parent)
self._localTransform = QMatrix4x4()
self._parentTransform = None
Q_CLASSINFO('DefaultProperty', 'parentTransform')
localTransformChanged = Signal()
def local_transform(self):
return self._localTransform
localTransform = Property(QMatrix4x4,
local_transform,
notify=localTransformChanged)
def set_local_transform(self, matrix4x4):
if matrix4x4 is None:
matrix4x4 = QMatrix4x4()
Product.assign_input(self, "localTransform", matrix4x4)
Product.InputProperty(vars(), Product.Product, 'parentTransform')
@Slot(result=QMatrix4x4)
def worldTransform(self, update=False):
if update:
self.update()
assert not self.dirty
return self.localTransform if self.parentTransform is None \
else self.parentTransform.worldTransform() * self.localTransform
@Slot(QVector3D, float, result=QQuaternion)
def qFromAA(self, axis, angle_rad):
return QQuaternion.fromAxisAndAngle(axis, math.degrees(angle_rad))
@Slot(float, float, float, result=QQuaternion)
def qFromEuler(self, roll, pitch, yaw):
return QQuaternion.fromEulerAngles(math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw))
@Slot(QQuaternion, QVector3D, result=QMatrix4x4)
def mFromTQ(self, t=QVector3D(), q=QQuaternion()):
m = QMatrix4x4()
m.rotate(q)
m.translate(t)
return m
class ColorsAttribs(Attribs):
def __init__(self, parent=None, vertices=None, normals=None, colors=None):
super(ColorsAttribs, self).__init__(parent, vertices, normals)
self._colors = None
self.colors = colors
Product.InputProperty(vars(), Array, 'colors')
def get_attributes(self):
'''
override this method to add all your attribs
'''
a = super(ColorsAttribs, self).get_attributes()
a["colors"] = self._colors
return a
class GLSLProgram(Product.Product):
def __init__(self,
parent=None,
vertex_shader=None,
geometry_shader=None,
fragment_shader=None,
uniforms={},
textures={},
outputTextures={}):
super(GLSLProgram, self).__init__(parent)
self._vertexShader = None
self._geometryShader = None
self._fragmentShader = None
self._uniforms = {}
self._textures = {}
self._outputTextures = {}
self._linkDirty = False
self.vertexShader = vertex_shader
self.geometryShader = geometry_shader
self.fragmentShader = fragment_shader
self.uniforms = uniforms
self.textures = textures
self.outputTextures = outputTextures
def _update(self):
if self._linkDirty:
self._program = QOpenGLShaderProgram()
if self._vertexShader is not None:
self._program.addShaderFromSourceCode(QOpenGLShader.Vertex,
self._vertexShader)
if self._geometryShader is not None:
self._program.addShaderFromSourceCode(QOpenGLShader.Geometry,
self._geometryShader)
if self._fragmentShader is not None:
self._program.addShaderFromSourceCode(QOpenGLShader.Fragment,
self._fragmentShader)
self._program.link()
self._linkDirty = False
Product.InputProperty(vars(), 'QVariant', 'uniforms')
''' supports dict of Array, subject to the constraints associated with Image.to_QImage() '''
Product.InputProperty(vars(), 'QVariant', 'textures')
Product.InputProperty(vars(), 'QVariant', 'outputTextures')
def relink(self):
self._linkDirty = True
Product.InputProperty(vars(), str, 'geometryShader', relink)
Product.InputProperty(vars(), str, 'vertexShader', relink)
Product.InputProperty(vars(), str, 'fragmentShader', relink)
class Platform(Product.Product):
def __init__(self, parent=None, path=''):
super(Platform, self).__init__(parent)
self._path = path
self._platform = None
Product.InputProperty(vars(), str, 'path')
sensorNamesChanged = Signal()
@Property(QVariant, notify=sensorNamesChanged)
def sensorNames(self):
if self._platform is None:
return QVariant([])
return QVariant(list(self._platform.sensors.keys()))
datasourcesNamesChanged = Signal()
@Property(QVariant, notify=datasourcesNamesChanged)
def datasourcesNames(self):
if self._platform is None:
return QVariant([])
return QVariant(self._platform.datasource_names())
orientationChanged = Signal()
@Property(QVariant, notify=orientationChanged)
def orientation(self):
if self._platform is None:
return QVariant([])
return QVariant(self._platform.orientation)
@Slot(list, result=list)
def expandWildcards(self, labels):
if self._platform is not None:
return self._platform.expand_wildcards(labels)
def _update(self):
if self._platform is None:
self._platform = platform.Platform(dataset=self._path)
self.orientationChanged.emit()
self.sensorNamesChanged.emit()
self.datasourcesNamesChanged.emit()
def __getitem__(self, key):
return self._platform[key]
class ExtractImage(Array.Array):
def __init__(self, parent=None):
super(ExtractImage, self).__init__(parent)
self._packages = None
self.productClean.connect(self.sizeChanged)
Product.InputProperty(vars(), Product.VariantProduct, 'packages')
sizeChanged = Signal()
@Property(QSize, notify=sizeChanged)
def size(self):
return QSize(self.ndarray.width, self.ndarray.height)
def _update(self):
if self._packages is not None and self._packages._variant is not None:
self.ndarray = self._packages._variant.copy()
class AmplitudeAttribs(Attribs):
def __init__(self,
parent=None,
vertices=None,
normals=None,
amplitude=None):
super(AmplitudeAttribs, self).__init__(parent, vertices, normals)
self._amplitude = None
self.amplitude = amplitude
Product.InputProperty(vars(), Array, 'amplitude')
def get_attributes(self):
'''
override this method to add all your attribs
'''
a = super(AmplitudeAttribs, self).get_attributes()
a["amplitude"] = self._amplitude
return a
class TexcoordsAttribs(Attribs):
def __init__(self,
parent=None,
vertices=None,
normals=None,
texcoords0=None):
super(TexcoordsAttribs, self).__init__(parent, vertices, normals)
self._texcoords0 = None
self.texcoords0 = texcoords0
Product.InputProperty(vars(), Array, 'texcoords0')
def get_attributes(self):
'''
override this method to add all your attribs
'''
a = super(TexcoordsAttribs, self).get_attributes()
a["texcoords0"] = self._texcoords0
return a
class Undistort(Image.ImageFilter):
def __init__(self, parent=None):
super(Undistort, self).__init__(parent)
self._path = None
self._yaml = None
self._map = None
self._roi = []
self._shape = []
def path_cb(self):
with tarfile.open(self._path) as tar:
try:
calib = pickle.loads(
tar.extractfile(tar.getmember('calib.pkl')).read())
cam_specs = pickle.loads(
tar.extractfile(tar.getmember('cam_specs.pkl')).read())
w = cam_specs['h']
h = cam_specs['v']
self._shape = (h, w)
mtx = calib['matrix']
dist = calib['distortion']
newcameramtx, self._roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, (w, h), 1, (w, h))
self._map = cv2.initUndistortRectifyMap(
mtx, dist, None, newcameramtx, (w, h), 5)
except:
print(traceback.format_exc())
Product.InputProperty(vars(), str, 'path', path_cb)
def _update(self):
if self._imageArray is not None and self._map is not None:
image = self._imageArray.ndarray
if self._shape == image.shape[:2]:
dst = cv2.remap(image, self._map[0], self._map[1],
cv2.INTER_LINEAR)
# crop the image
x, y, w, h = self._roi
self.ndarray = np.copy(dst[y:y + h, x:x + w])
class SensorReferential(CloudBase):
def __init__(self, parent=None):
'''
It may appears bit deceiving that this class derive from CloudBase,
but this way we ensure we use 'quad_directions' as computed for 3d projection,
therefore keeping referential naturally in sync with projection model
'''
super(SensorReferential, self).__init__(parent)
self._depth = 5
## 0,5, 0,6, 0,7, 0,8 mid-lines
self._indices.ndarray = np.array(
[0, 1, 0, 2, 0, 3, 0, 4, 1, 2, 2, 4, 4, 3, 3, 1], dtype="uint32")
Product.InputProperty(vars(), float, 'depth')
def _update(self):
self._fill_empty_specs()
self.ndarray.resize((9, 3))
v, h = [self._specs[x] for x in ["v", "h"]]
mid_v, mid_h = (v + 1) // 2, (h + 1) // 2
n = v * h
def index(v_i, h_i, offset):
return v_i * h + h_i + offset
self.ndarray[0] = [0, 0, 0]
self.ndarray[1:] = self._quad_directions[[
index(0, 0, 0),
index(0, h - 1, 3 * n),
index(v - 1, 0, n),
index(v - 1, h - 1, 2 * n),
index(0, mid_h, 0),
index(v - 1, mid_h, 0),
index(mid_v, 0, 0),
index(mid_v, h - 1, 0)
]] * self._depth
class ColorMapFliter(ImageFilter):
def __init__(self, parent = None):
super(ColorMapFliter, self).__init__(parent)
self._colorMap = 'jet'
self._min = None
self._max = None
self._adaptative = 0
self._log = False
self._interpolate = False
def colorMap_cb(self):
assert hasattr(cm, self._colorMap)
Product.InputProperty(vars(), str, 'colorMap', colorMap_cb)
Product.InputProperty(vars(), float, 'min')
Product.InputProperty(vars(), float, 'max')
Product.InputProperty(vars(), bool, 'log')
Product.InputProperty(vars(), float, 'adaptative')
#Test, addition of interpolation for missing values
Product.InputProperty(vars(), bool, 'interpolate')
def _update(self):
if self.imageArray is not None and self.imageArray.ndarray.size > 0:
image = self.imageArray.ndarray #shape (8,32)
#test interpolation placed here
if self._interpolate:
image = images.interpolate_missings(image)
# if self._adaptative > 0:
# image = basics.clean_bleed_amp(image, image, self._adaptative)
_min = self._min if self._min is not None else image.min()
_max = self._max if self._max is not None else image.max()
self.ndarray = images.to_color_image(image, self._colorMap, _min, _max, self._log)
class DasSampleToCloud(Array.ArrayDouble3):
def __init__(self, parent=None):
super(DasSampleToCloud, self).__init__(parent)
self._sample = None
self._seg3DSample = None
self._referential = None
self._undistort = False
self._undistortRefTs = -1
self._primitiveType = Geometry.PrimitiveType.POINTS
self._indices = Array.ArrayUInt1()
self._indices.set_producer(self)
self._amplitudes = Array.ArrayFloat1()
self._amplitudes.set_producer(self)
self._colors = Array.ArrayFloat4()
self._colors.set_producer(self)
self._transform = Transforms.Transform()
self._transform.set_producer(self)
self._minAmplitude = np.nan
self._maxAmplitude = np.nan
self._hasReferential = True
self._logScale = False
self._method = None
self._amplitudeRatio = 100
#call setters:
self.method = 'point_cloud'
self.sample = Product.VariantProduct()
self.seg3DSample = Product.VariantProduct()
# inputs:
Product.InputProperty(vars(), Product.VariantProduct, 'sample')
Product.InputProperty(vars(), Product.VariantProduct, 'seg3DSample')
Product.InputProperty(vars(), str, 'referential')
Product.InputProperty(vars(), bool, 'undistort')
Product.InputProperty(vars(), int, 'undistortRefTs')
def cb_method(self):
if self._method not in ['point_cloud', 'quad_cloud']:
raise RuntimeError(f"Unexpected method: {self._method}")
self.set_primitiveType(
self, Geometry.PrimitiveType.POINTS if self._method
== 'point_cloud' else Geometry.PrimitiveType.TRIANGLES)
Product.InputProperty(vars(), str, 'method', cb_method)
Product.InputProperty(vars(), float, 'minAmplitude')
Product.InputProperty(vars(), float, 'maxAmplitude')
Product.InputProperty(vars(), float, 'amplitudeRatio')
Product.InputProperty(vars(), str, 'amplitudeType')
Product.InputProperty(vars(), bool, 'logScale')
#outputs:
Product.ConstProperty(vars(), Array.ArrayUInt1, 'indices')
Product.ConstProperty(vars(), Array.ArrayFloat1, 'amplitudes')
Product.ConstProperty(vars(), Array.ArrayFloat4, 'colors')
Product.ConstProperty(vars(), Transforms.Transform, 'transform')
Product.ROProperty(vars(), bool, 'hasReferential')
Product.ROProperty(vars(), int, 'primitiveType')
#slots:
@Slot(int, result=int)
def channel(self, n):
if self._primitiveType == Geometry.PrimitiveType.TRIANGLES:
triangle = self._triangle_at(n)
data = self.filter_banks()
return data['indices'][clouds.triangle_to_echo_index(triangle)]
elif self._primitiveType == Geometry.PrimitiveType.POINTS:
return self.indices.ndarray[n]
else:
raise RuntimeError(
f"Unsupported primitiveType {self._primitiveType}")
@Slot(int, result=QVariant)
def channelInfo(self, n):
triangle = self._triangle_at(n)
echo_i = clouds.triangle_to_echo_index(triangle)
sample = self._get_sample().masked
data = sample['data']
channel_i = data['indices'][echo_i]
h = sample['h']
try:
coeff = sample['timestamps_to_us_coeff']
except:
coeff = 1
rv = {
'v': int(channel_i // h),
'h': int(channel_i % h),
'i': int(channel_i),
'distance': float(data['distances'][echo_i]),
'amplitude': float(data['amplitudes'][echo_i]),
'timestamp': int(data['timestamps'][echo_i] * coeff),
'flag': int(data['flags'][echo_i]),
'category': ''
}
if self._seg3DSample._variant is not None:
seg_source = categories.get_source(
self._seg3DSample._variant.datasource.ds_type)
category_number = self._seg3DSample._variant.raw['data'][int(
echo_i)]['classes']
category_name, _ = categories.get_name_color(
seg_source, category_number)
rv['category'] = category_name
return QVariant(rv)
# private:
def _triangle_at(self, n):
return self._indices.ndarray[n * 3:n * 3 + 3]
def _normalize_amplitudes(self):
min_ = self._minAmplitude if not np.isnan(
self._minAmplitude
) or self._amplitudes.ndarray.size == 0 else self._amplitudes.ndarray.min(
)
max_ = self._maxAmplitude if not np.isnan(
self._maxAmplitude
) or self._amplitudes.ndarray.size == 0 else self._amplitudes.ndarray.max(
)
if self._logScale:
norm = colors.LogNorm(1 + min_, 1 + min_ + max_)
else:
norm = colors.Normalize(min_, max_)
self._amplitudes.ndarray = norm(self._amplitudes.ndarray +
((1 + min_) if self._logScale else 0))
def _get_sample(self):
if self._sample is None or self._sample._variant is None:
raise RuntimeError("No sample found!")
return self._sample._variant
def _update(self):
sample = self._get_sample()
f = getattr(sample, self._method) #point_cloud() or quad_cloud()
tf_Ref_from_Local = linalg.tf_eye(np.float64)
try:
rv = f(referential=self._referential,
ignore_orientation=False,
undistort=self._undistort,
reference_ts=self._undistortRefTs,
dtype=np.float64)
self.set_hasReferential(self, True)
tf_Ref_from_Local = sample.compute_transform(
self._referential, ignore_orientation=True, dtype=np.float64)
except sensors.Sensor.NoPathToReferential as e:
rv = f(referential=None,
undistort=self._undistort,
dtype=np.float64)
self.set_hasReferential(self, False)
self._transform.set_local_transform(
QMatrix4x4(
tf_Ref_from_Local.astype(np.float32).flatten().tolist()))
if self._method == "quad_cloud":
v, a, i = rv
self.set_ndarray(v)
self._amplitudes.set_ndarray(a)
self._indices.set_ndarray(i)
self._normalize_amplitudes()
elif self._method == "point_cloud":
amp = sample.amplitudes
nb_points = np.max(
[1, int(amp.shape[0] * self._amplitudeRatio / 100.0)])
if self._amplitudeRatio < 100.0:
amp_order = np.argsort(amp)
self.set_ndarray(rv[amp_order][-nb_points:])
self._amplitudes.set_ndarray(amp[amp_order][-nb_points:])
self._indices.set_ndarray(
np.arange(rv[amp_order][-nb_points:].shape[0],
dtype=np.uint32))
else:
self.set_ndarray(rv[-nb_points:])
self._amplitudes.set_ndarray(amp[-nb_points:])
self._indices.set_ndarray(
np.arange(rv[-nb_points:].shape[0], dtype=np.uint32))
if self._logScale and np.min(self._amplitudes.ndarray) < 0:
self._amplitudes.set_ndarray(self._amplitudes.ndarray -
np.min(self._amplitudes.ndarray))
self._normalize_amplitudes()
if self._seg3DSample._variant is not None:
self._colors.set_ndarray(
self._seg3DSample._variant.colors(self._method))
def prettyPrint(self, d):
return pprint.pformat(Product.cvt_if_js_value(d))
class LiveCapture( SensorCapture ):
def __init__(self, parent = None):
super(LiveCapture, self).__init__(parent)
self._dev = None
self._ip = ""
self._oversampling = -1
self._accumulation = -1
self._basePoints = -1
self._tracesROI = [0,1] #offset, count
self._rejectedFalgs = [0, 3, 4, 9, 3072]
self._received_echoes = []
self._n_received = 0
self._received_traces_raw = []
self._received_traces_proc = []
self._received_states = []
self._running = False
self._locked = False
self._tracesMode = TracesMode.NONE
self._delay = 5000
self._nDroppedPackages = 0
self._ts_prev = None
self._triggerMode = TriggerMode.LONG
self._lcpgIndex = 0x08
self._overriden_specs = False
self._connected = False
self._emitConnectedChanged = False
self._properties = list(leddar.property_ids.keys())
self._calibrating = False
self._restoringCal = False
self._calibration_samples = []
self._calibration_states_samples = []
self._n_calibration_samples = 100
self._n_drop_calib_samples = 5
self._calibrationProgress = 0.0
self._calibration_wall_distance = 2.0
self._interpCal = False
self._dTCorrection = False
self._dTGainFactor = 0.00489
self._dTFix = 0.0
self._aTCorrection = False
self._aTGainFactor = 0.01521
self._aTThreshold = 20.0
self._editingResoSpecs = False
self._resoSpecsMode = ""
self._resoSpecsVal = 0
if not os.path.exists('calib_results/'):
os.makedirs('calib_results/')
if not os.path.exists('rcalibration/'):
os.makedirs('rcalibration/')
if not os.path.exists('angles_table/'):
os.makedirs('angles_table/')
self._listFilesRCal = [os.path.basename(x) for x in glob.glob('rcalibration/' + '*.pkl')]
self._listFilesCustomVAngles = [os.path.basename(x) for x in glob.glob('angles_table/' + '*.txt')]
self._vtabanglesCorrection = False
self._scalevangles = 1.0456
self._savingcalibinto = "calibration.pkl"
def _disconnect_device(self):
if self._dev is not None and self._connected:
self._dev.disconnect()
self._dev = None
if not self._overriden_specs:
self._specs = None
def _update_traces_roi(self):
if self._dev is not None and self._connected:
self._dev.set_traces_to_receive(int(self._tracesROI[0]), int(self._tracesROI[1]))
TracesMode = TracesMode
Q_ENUMS(TracesMode)
def _update_traces_mode(self):
if self._dev is None or not self._connected:
return
try:
self._update_traces_roi()
if self._tracesMode == TracesMode.PROCESSED:
def proc_trace_callback(new_traces):
if not self._locked:
self._received_traces_proc.append(new_traces)
self.makeDirty()
self._dev.set_data_mask(self._dev.get_data_mask() | leddar.data_masks["PDM_PROCESSED_TRACES"])
self._dev.set_callback_processed_trace(proc_trace_callback)
else:
self._dev.set_data_mask(self._dev.get_data_mask() & ~leddar.data_masks["PDM_PROCESSED_TRACES"])
if self._tracesMode == TracesMode.RAW:
def raw_trace_callback(new_traces):
if not self._locked:
self._received_traces_raw.append(new_traces)
self.makeDirty()
self._dev.set_data_mask(self._dev.get_data_mask() | leddar.data_masks["PDM_RAW_TRACES"])
self._dev.set_callback_raw_trace(raw_trace_callback)
else:
self._dev.set_data_mask(self._dev.get_data_mask() & ~leddar.data_masks["PDM_RAW_TRACES"])
except:
traceback.print_exc()
Product.InputProperty(vars(), int, 'tracesMode', _update_traces_mode)
Product.InputProperty(vars(), QVariant, 'tracesROI', _update_traces_roi)
Product.InputProperty(vars(), QVariant, 'rejectedFlags')
Product.InputProperty(vars(), str, 'ip', _disconnect_device)
def _update_data_thread(self):
if self._dev is None or not self._connected:
return
self._dev.set_data_thread_delay(self._delay)
if self._running:
self._dev.start_data_thread()
else:
self._dev.stop_data_thread()
Product.InputProperty(vars(), bool, 'running', _update_data_thread)
Product.InputProperty(vars(), int, 'delay', _update_data_thread)
Product.ROProperty(vars(), int, 'nDroppedPackages')
Product.ROProperty(vars(), bool, 'connected')
Product.ROProperty(vars(), float, 'fps')
Product.ConstProperty(vars(), list, 'properties')
Product.ROProperty(vars(), QVariant, 'states')
Product.ROProperty(vars(), bool, 'calibrating')
Product.ROProperty(vars(), float, 'calibrationProgress')
Product.InputProperty(vars(), bool, 'interpCal')
Product.InputProperty(vars(), bool, 'dTCorrection')
Product.InputProperty(vars(), float, 'dTFix')
Product.InputProperty(vars(), float, 'dTGainFactor')
Product.InputProperty(vars(), bool, 'aTCorrection')
Product.InputProperty(vars(), float, 'aTGainFactor')
Product.InputProperty(vars(), float, 'aTThreshold')
Product.ROProperty(vars(), bool, 'editingResoSpecs')
Product.ROProperty(vars(), float, 'resoSpecsVal')
Product.ROProperty(vars(), bool, 'restoringCal')
Product.ROProperty(vars(),list, 'listFilesRCal')
Product.ROProperty(vars(),list, 'listFilesCustomVAngles')
Product.InputProperty(vars(), bool, 'vtabanglesCorrection')
Product.InputProperty(vars(), float, 'scalevangles')
Product.InputProperty(vars(), str, 'savingcalibinto')
@Slot(int, int, int, result = bool)
def writeMemory(self, zone, address, value):
self._dev.write_memory(zone, address, np.array([value], np.uint16))
@Slot(int, int, int, result = str)
def readMemory(self, zone, address, n_bytes):
return str(np.array(self._dev.read_memory(zone, address, n_bytes), np.uint8).tolist())
@Slot(float, int, result = bool)
def calibrate(self, wall_distance, n_samples):
if self._dev and self._connected:
self._dev.set_calib_values(leddar.calib_types['ID_TIMEBASE_DELAY'], [0.0] * self._nChannels)
self._calibration_wall_distance = wall_distance
self._n_calibration_samples = n_samples
self._calibration_samples = []
self._calibration_states_samples = []
self._received_echoes = []
self.set_calibrating(self, True)
self.set_calibrationProgress(self, 0.0)
self.set_restoringCal(self, False)
return True
return False
@Slot(str, result = bool)
def restoreCalibration(self, spath_to = "rcalibration/calibration.pkl"):
if self._dev and self._connected:
self.set_restoringCal(self, True)
self._dev.set_calib_values(leddar.calib_types['ID_TIMEBASE_DELAY'],[0.0] * self._nChannels)
data = pickle.load(open(spath_to, 'rb'))
self._specs['v'] = data['v']
self._specs['h'] = data['h']
self._specs['v_fov'] = data['v_fov']
self._specs['h_fov'] = data['h_fov']
if 'angles' in data:
self._specs['angles'] = data['data']
self.scalevangles = data['scaling_angles']
self.dTGainFactor = data['dT_slope']
self.dTFix = data['dT_ref']
self.aTGainFactor = data['aT_slope']
self.aTThreshold = data['aT_threshold']
self._dev.set_calib_values(leddar.calib_types['ID_TIMEBASE_DELAY'], data['offsets'])
super(LiveCapture, self)._handle_specs_changed()
return True
return False
@Slot(str, result = bool)
def editResoSpecs(self, mode):
if mode == "NONE":
self.set_editingResoSpecs(self, False)
self.set_resoSpecsVal(self,0)
else:
self.set_editingResoSpecs(self, True)
self._resoSpecsMode = mode
self.set_resoSpecsVal(self,self._specs[mode])
return True
@Slot(float, result = bool)
def changeResoSpecs(self, new_value):
if self._resoSpecsMode == "h_fov" or self._resoSpecsMode == "v_fov":
self._specs[self._resoSpecsMode] = new_value
elif self._resoSpecsMode == "h" or self._resoSpecsMode == "v":
self._specs[self._resoSpecsMode] = int(new_value)
super(LiveCapture, self)._handle_specs_changed()
return True
@Slot(str, result = bool)
def changeAnglesSpecs(self, filename):
c_filename = 'angles_table/' + filename
self._specs['angles'] = clouds.custom_v_angles(self._specs, factor=self._scalevangles, filename=c_filename, dtype = np.float32)
self._specs['v_fov'] = np.rad2deg(np.max(self._specs['angles'][:,0]) - np.min(self._specs['angles'][:,0]))
super(LiveCapture, self)._handle_specs_changed()
return True
@Slot(str, result = bool)
def changeIP(self, ip_string):
if self._dev and self._connected:
try:
rv = self._dev.set_IP_config(False, ip_string)
print(f"ip changed to {self._dev.get_IP_config()}, you need to reboot your device!")
return rv
except:
traceback.print_exc()
return False
@Slot(str, QVariant, result = bool)
def setLeddarProperty(self, name, value):
if self._dev and self._connected:
try:
rv = self._dev.set_property_value(name, str(value))
self.connectedChanged.emit()
return rv
except:
traceback.print_exc()
return False
@Slot(str, result = QVariant)
def getLeddarProperty(self, name):
if self._dev and self._connected:
try:
return self._dev.get_property_value(name)
except:
traceback.print_exc()
return False
return None
@Slot(str, result = QVariant)
def getPropertyAvailableValues(self, name):
if self._dev and self._connected:
try:
rv = self._dev.get_property_available_values(name)
string_value = self._dev.get_property_value(name)
if isinstance(rv, dict):
if rv['type'] in ['list', 'bitfield']:
rv['current'] = string_value
return rv
else:
return {'type': 'list', 'data': [current], 'current': current}
except:
pass
return None
def _update(self):
if not self._running:
return
if self._dev is None or not self._connected:
self._dev = leddar.Device()
if not self._dev.connect(self._ip):
self._connected = False
self.connectedChanged.emit()
self._dev = None
return
self._connected = True
self.connectedChanged.emit()
self._emitConnectedChanged = True
if self._specs is None:
self._specs = utils.get_specs(self._dev)
else:
self._overriden_specs = True
self._handle_specs_changed()
def echo_callback(new_echoes):
if not self._locked:
self._received_echoes.append(new_echoes)
self.makeDirty()
self._dev.set_callback_echo(echo_callback)
self._dev.set_data_mask(leddar.data_masks["PDM_ECHOES"])
def state_callback(new_state):
if not self._locked:
self._received_states.append(new_state)
self.makeDirty()
self._dev.set_callback_state(state_callback)
self._dev.set_data_mask(self._dev.get_data_mask() | leddar.data_masks["PDM_STATES"])
self._update_traces_mode()
self._update_data_thread()
self._n_received = 0
if self._received_echoes:
self._n_received += 1
self._locked = True
self._nDroppedPackages = len(self._received_echoes) - 1
last_received = self._received_echoes[-1]
last_received['data'] = last_received['data'][np.isin(last_received['data']['flags'], self._rejectedFalgs, invert = True)]
if self._calibrating:
self._calibration_samples.append(last_received)
else:
if (self._dTCorrection) and (self._states is not None):
T = self._states['system_temp']
last_received = calibration.distance_correction_temperature(last_received, T, self._dTGainFactor, self._dTFix)
if (self._aTCorrection) and (self._states is not None):
T = self._states['system_temp']
last_received = calibration.amplitude_correction_temperature(last_received, T, self._aTGainFactor, self._aTThreshold)
self._received_echoes = []
self._locked = False
self.nDroppedPackagesChanged.emit()
if self._ts_prev is None:
self._ts_prev = time.time()
ts = time.time()#last_received['timestamp']
if self._n_received%10 == 0:
self._fps = 10 /(ts - self._ts_prev)
self.fpsChanged.emit()
self._ts_prev = ts
if self._emitConnectedChanged: #important for those watching properties. properties seem only valid after the first echo
self._emitConnectedChanged = False
self.connectedChanged.emit()
self._update_with_last_received(last_received)
if self._received_states:
last_received = self._received_states[-1]
self.set_states(self, last_received)
self._received_states = []
if self._calibrating:
self._calibration_states_samples.append(last_received)
if self._calibrating:
self.set_calibrationProgress(self, len(self._calibration_samples)/(self._n_calibration_samples+self._n_drop_calib_samples))
if self._calibrationProgress >= 1.0:
self._calibration_samples = self._calibration_samples[-self._n_calibration_samples::]
self._calibration_states_samples = self._calibration_states_samples[-self._n_calibration_samples::]
offsets = calibration.calibrate(self._calibration_samples, self._calibration_wall_distance, self._specs, self._interpCal)
if self._dTCorrection:
offsets = calibration.set_offset_to_base_temperature(self._calibration_states_samples, offsets, self._dTGainFactor, self._dTFix)
self._dev.set_calib_values(leddar.calib_types['ID_TIMEBASE_DELAY'], offsets.tolist())
self.saveCalibration(offsets = offsets.tolist())
self.set_calibrating(self, False)
if self._received_traces_raw:
self._update_traces_with_last_recieved(self._received_traces_raw[-1])
self._received_traces_raw = []
def saveCalibration(self, offsets, spath = 'calib_results/' ):
if not os.path.exists(spath):
os.makedirs(spath)
dico = { 'v': self._specs['v'],
'h': self._specs['h'],
'v_fov': self._specs['v_fov'],
'h_fov': self._specs['h_fov'],
'dT_slope': self._dTGainFactor,
'dT_ref': self._dTFix,
'aT_slope': self._aTGainFactor,
'aT_threshold': self._aTThreshold,
'offsets': offsets,
'data': self._calibration_samples
}
if 'angles' in self._specs:
dico['angles'] = self._specs['angles']
dico['scaling_angles'] = self._scalevangles
fname = spath + self._savingcalibinto
with open(fname, 'wb') as f:
pickle.dump(dico, f)
return True
class EchoesMPLFigureProvider(Product.Product):
def __init__(self, parent=None):
super(EchoesMPLFigureProvider, self).__init__(parent)
self._packages = None
self._absoluteMax = 2**20 #np.finfo('f4').max
self._absoluteMin = 0 #np.finfo('f4').min
self._max = self._absoluteMax
self._min = self._absoluteMin
self._dynamicMax = self._absoluteMax
self._dynamicMin = self._absoluteMin
self._dynamicRange = True
self._logScale = False
self._colorMap = 'viridis'
self._flipud = False
self._rot90 = False
self._options = 'amplitudes:max_amplitudes'
self.shape = None
self.fig = None
self.ax = None
self.ax_img = None
clicked = Signal(int,
int,
float,
int,
arguments=['v', 'h', 'value', 'button'])
Product.InputProperty(vars(), Product.VariantProduct, 'packages')
Product.InputProperty(vars(), float, 'absoluteMax')
Product.InputProperty(vars(), float, 'absoluteMin')
Product.InputProperty(vars(), float, 'min')
Product.InputProperty(vars(), float, 'max')
Product.InputProperty(vars(), bool, 'dynamicRange')
Product.InputProperty(vars(), bool, 'logScale')
Product.InputProperty(vars(), str, 'colorMap')
Product.InputProperty(vars(), str, 'options')
Product.ROProperty(vars(), float, 'dynamicMax')
Product.ROProperty(vars(), float, 'dynamicMin')
def handle_rotated_changed(self):
self.ax_img = None
Product.InputProperty(vars(), bool, 'flipud', handle_rotated_changed)
Product.InputProperty(vars(), bool, 'rot90', handle_rotated_changed)
def setCanvas(self, canvas):
self.canvas = canvas
self.ax = None
self.ax_img = None
self.makeDirty()
def _update(self):
if self._packages is not None:
p = self._packages.variant
if p is not None:
if self._options == 'amplitudes:max_amplitudes':
self.img = leddar_utils.images.extrema_image(
p['v'],
p['h'],
p['data'],
sort_field='amplitudes',
sort_direction=-1,
dtype='f4')
elif self._options == 'distances:max_amplitudes':
_, others = leddar_utils.images.extrema_image(
p['v'],
p['h'],
p['data'],
sort_field='amplitudes',
sort_direction=-1,
other_fields=['distances'],
dtype='f4')
self.img = others['distances']
# the following is a hack around an hard to overcome architectural flaw
# We want to reuse EchoesMPLFigureProvider in cheddar_config and das.api,
# but we don't yet want them to know about each other, thus we 'hide' a
# das.api.samples.Sample instance inside its 'raw' packet so we can orient
# the image in a standard way. It is crucial to keep the standard orientation in one place
if 'das.sample' in p:
s = p['das.sample']
self.img = s.transform_image(self.img)
if 'amplitudes:' in self._options and 'extrema_amp' in s.datasource.sensor.config:
self.absoluteMax = s.datasource.sensor.config[
'extrema_amp']
if 'distances:' in self._options and 'extrema_dist' in s.datasource.sensor.config:
self.absoluteMax = s.datasource.sensor.config[
'extrema_dist']
if self.img is None:
return
self.set_dynamicMin(self, self.img.min())
self.set_dynamicMax(self, self.img.max())
vmax = self.dynamicMax if self._dynamicRange else self._max
vmin = self.dynamicMin if self._dynamicRange else self._min
if self._flipud:
self.img = np.flipud(self.img)
if self._rot90:
self.img = np.rot90(self.img)
color_img = leddar_utils.images.to_color_image(
self.img,
self._colorMap,
vmin=vmin,
vmax=vmax,
log_scale=self._logScale)
if self.ax is None:
self.ax = self.canvas.getFigure().add_subplot(111)
self.helper = backend_qtquick5.MPLImageHelper(
self.img, self.ax)
def on_click(event):
try:
col, row = self.helper.to_indices(
event.xdata, event.ydata)
value = self.img[row, col]
if self._rotated:
row, col = col, row
self.clicked.emit(
row, col, value,
backend_qtquick5.to_qt_button[event.button])
except:
pass
self.cid = self.canvas.mpl_connect('button_press_event',
on_click)
self.helper.img = self.img
if self.ax_img is None:
self.ax_img = self.ax.imshow(color_img)
self.canvas.draw()
else:
self.ax_img.set_data(color_img)
self.canvas.draw()
if self.shape != self.img.shape:
self.shape = self.img.shape
class Viewport( QQuickFramebufferObject ):
def __init__( self, parent=None ):
super(Viewport, self).__init__( parent )
self.setAcceptedMouseButtons(Qt.AllButtons)
self.setAcceptHoverEvents(True)
self.renderer = None
self._camera = Camera()
self._mouse_start = None
self._start_eye = None
self._start_up = None
self._actors = None
self._debug_actors = Actors()
self._debug = True
self.render_to_texture_attachment = -1
self._backgroundColor = QColor(qRgba(1,1,1,1))
Product.ConstProperty(vars(), Camera, 'camera')
Product.RWProperty(vars(), 'QVariant', 'backgroundColor')
Product.RWProperty(vars(), Actors, 'actors')
def harvest_updates(self):
self.update()
# TODO: to prevent clogging 1 CPU, we should do something like this
# if self.renderer is not None:
# for actor in self.renderer.sorted_actors:
# if actor.dirty:
# self.update()
# return
def aspect_ratio(self):
return self.width()/self.height()
def view_matrix(self):
return self._camera.view_matrix()
def perspective_matrix(self):
if self._camera.perspective_override is None:
p = QMatrix4x4()
p.perspective(self._camera.vfov, self.aspect_ratio(), self._camera.near, self._camera.far)
return p
else:
return self._camera.perspective_override
def orthographic_matrix(self):
p = QMatrix4x4()
p.ortho(0, self.width(), 0, self.height(), self._camera.near, self._camera.far)
return p
def createRenderer( self ):
self.renderer = InFboRenderer.InFboRenderer()
self.timer = QTimer()
self.timer.timeout.connect(self.harvest_updates)
self.timer.start(0) #will be called after each event loop
return self.renderer
def set_render_to_texture_attachment(self, attachment):
if "QSG_RENDER_LOOP" in os.environ:
if os.environ['QSG_RENDER_LOOP'] != "basic":
warnings.warn("Error: multithreaded rendering enabled, please set os.environ['QSG_RENDER_LOOP'] = 'basic' before any Qt call")
if self.render_to_texture_attachment != attachment:
self.render_to_texture_attachment = attachment
self.update()
def get_render_to_texture_array(self):
if self.renderer is not None:
return self.renderer.render_to_texture_array
return None
def pick(self, clicked_x, clicked_y, modifiers = None):
# http://schabby.de/picking-opengl-ray-tracing/
aspect_ratio = self.aspect_ratio()
cam_origin = self._camera.eye
cam_direction = (self._camera.center - cam_origin).normalized()
# The coordinate system we chose has x pointing right, y pointing down, z pointing into the screen
# in screen coordinates, the vertical axis points down, this coincides with our 'y' axis.
v = -self._camera._up # our y axis points down
# in screen coordinates, the horizontal axis points right, this coincides with our x axis
h = QVector3D.crossProduct(cam_direction, self._camera._up).normalized() # cam_direction points into the screen
# in InFobRenderer::render(), we use Viewport::perspective_matrix(), where self._camera.fov is used
# as QMatrix4x4::perspective()'s verticalAngle parameter, so near clipping plane's vertical scale is given by:
v_scale = math.tan( math.radians(self._camera.vfov) / 2 ) * self._camera.near
h_scale = v_scale * aspect_ratio
# translate mouse coordinates so that the origin lies in the center
# of the viewport (screen coordinates origin is top, left)
x = clicked_x - self.width() / 2
y = clicked_y - self.height() / 2
# scale mouse coordinates so that half the view port width and height
# becomes 1 (to be coherent with v_scale, which takes half of fov)
x /= (self.width() / 2)
y /= (self.height() / 2)
# the picking ray origin: corresponds to the intersection of picking ray with
# near plane (we don't want to pick actors behind the near plane)
world_origin = cam_origin + cam_direction * self._camera.near + h * h_scale * x + v * v_scale * y
# the picking ray direction
world_direction = (world_origin - cam_origin).normalized()
if self._debug and modifiers is not None and (modifiers & Qt.ShiftModifier):
np_origin = utils.to_numpy(world_origin)
np_v = utils.to_numpy(v)
np_h = utils.to_numpy(h)
self._debug_actors.clearActors()
self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + np_h, 0.01, QColor('red')))
self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + np_v, 0.01, QColor('green')))
self._debug_actors.addActor(CustomActors.arrow(np_origin, np_origin + utils.to_numpy(world_direction) * 100, 0.01, QColor('magenta')))
min_t = float("inf")
min_result = None
for actor in self.renderer.sorted_actors:
if actor._geometry:
if actor._geometry.indices is not None\
and actor._geometry.attribs.vertices is not None:
bvh = actor._geometry.goc_bvh()
if bvh is None:
continue
bvh.update()
if bvh.bvh is None:
continue
# bring back the actor at the origin
m = actor.transform.worldTransform() if actor.transform else QMatrix4x4()
m_inv = m.inverted()[0]
# bring the ray in the actor's referential
local_origin, local_direction = m_inv.map(world_origin), m_inv.mapVector(world_direction)
local_origin_np, local_direction_np = utils.to_numpy(local_origin), utils.to_numpy(local_direction)
# try to intersect the actor's geometry!
if bvh.primitiveType == BVH.PrimitiveType.TRIANGLES or bvh.primitiveType == BVH.PrimitiveType.LINES:
ids, tuvs = bvh.bvh.intersect_ray(local_origin_np, local_direction_np, True)
if ids.size > 0:
actor_min_t = tuvs[:,0].min()
if actor_min_t < min_t:
min_t = actor_min_t
min_result = (actor, ids, tuvs, world_origin, world_direction, local_origin, local_direction)
elif bvh.primitiveType == BVH.PrimitiveType.POINTS:
object_id, distance, t = bvh.bvh.ray_distance(local_origin_np, local_direction_np)
real_distance = math.sqrt(t**2 + distance**2)
if real_distance < min_t:
min_t = real_distance
min_result = (actor, bvh.indices.ndarray[object_id, None], np.array([[t, distance, real_distance]]), world_origin, world_direction, local_origin, local_direction)
if min_result is not None:
return min_result
raise NothingToPickException()
def mouseDoubleClickEvent(self, event):
btns = event.buttons()
if btns & Qt.MidButton or (btns & Qt.LeftButton and event.modifiers() & Qt.ShiftModifier) :
# centers camera on clicked point
try:
_, _, tuvs, world_origin, world_direction, _, _ = self.pick(event.localPos().x(), event.localPos().y())
p = world_origin + world_direction * tuvs[0,0]
self._camera.center = p
self._camera.eye = world_origin
except NothingToPickException:
pass
except:
traceback.print_exc()
self.update()
def signal_helper(self, signal, event, ids, tuvs, world_origin, world_direction, local_origin, local_direction):
if ids.size > 0:
signal.emit(ids[0], QVector3D(tuvs[0,0], tuvs[0,1], tuvs[0,2]), world_origin, world_direction, local_origin, local_direction, Product.convert_to_dict(event), self)
def mousePressEvent(self, event):
"""
Called by the Qt libraries whenever the window receives a mouse click.
"""
self._mouse_start = (event.localPos().x(), event.localPos().y())
self._start_eye = self._camera.eye
self._start_up = self._camera.up
self._start_center = self.camera.center
if event.buttons() & Qt.RightButton:
try:
actor, ids, tuvs, world_origin, world_direction, local_origin, local_direction = self.pick(event.localPos().x(), event.localPos().y())
self.signal_helper(actor.clicked, event, ids, tuvs, world_origin, world_direction, local_origin, local_direction)
except NothingToPickException:
pass
except:
traceback.print_exc()
event.setAccepted(True)
def wheelEvent(self, event):
"""
Called by the Qt libraries whenever the window receives a mouse wheel change.
"""
delta = event.angleDelta().y()
# move in look direction of camera
# note: this will only do something for non-orthographic projection
front = self._camera.center - self._camera.eye
if event.modifiers() & Qt.ShiftModifier:
factor = (5*120)
else:
factor = (5*12)
d = front.normalized() * delta/factor
self._camera.eye -= d
self._camera.center -= d
# re-paint at the next timer tick
self.update()
def mouseReleaseEvent(self, event):
# nothing to be done here.
pass
def hoverMoveEvent(self, event):
try:
actor, ids, tuvs, world_origin, world_direction, local_origin, local_direction = self.pick(event.pos().x(), event.pos().y(), event.modifiers())
self.signal_helper(actor.hovered, event, ids, tuvs, world_origin, world_direction, local_origin, local_direction)
except NothingToPickException:
pass
except:
traceback.print_exc()
def mouseMoveEvent(self, event):
"""
Called by the Qt libraries whenever the window receives a mouse
move/drag event.
"""
btns = event.buttons()
x, y = event.localPos().x(), event.localPos().y()
x_0, y_0 = self._mouse_start
dx, dy = (x - x_0, y - y_0)
h_width = self.width()/2
h_height = self.height()/2
if btns & Qt.LeftButton:
# we want half a screen movement rotates the camera 90deg:
self._camera.pan_tilt(self._start_eye, self._start_up, 90.0 * dx/h_width, 90.0 * dy/h_height)
elif btns & Qt.MidButton:
self._camera.translate(self._start_eye, self._start_center, -dx/h_width, dy/h_height)
elif btns & (Qt.RightButton):
self._camera.roll(self._start_eye, self._start_up, -90.0 * dy/h_width)
# re-draw at next timer tick
self.update()
def signal_helper(self, signal, event, ids, tuvs, world_origin, world_direction, local_origin, local_direction):
if ids.size > 0:
signal.emit(ids[0], QVector3D(tuvs[0,0], tuvs[0,1], tuvs[0,2]), world_origin, world_direction, local_origin, local_direction, Product.convert_to_dict(event), self)
def set_local_transform(self, matrix4x4):
if matrix4x4 is None:
matrix4x4 = QMatrix4x4()
Product.assign_input(self, "localTransform", matrix4x4)
class ROSStereoCalibratorFilter(ROSCalibratorFilter):
def __init__(self, parent=None):
super(ROSStereoCalibratorFilter, self).__init__(parent=parent)
self._imageArrayRight = None
self._images_right = []
self._leftCalibPath = ""
self._rightCalibPath = ""
self._rightImageResult = Array.Array()
self._rightImageResult.set_producer(self)
Product.InputProperty(vars(), Array.Array, "imageArrayRight")
Product.InputProperty(vars(), str, "leftCalibPath")
Product.InputProperty(vars(), str, "rightCalibPath")
Product.ConstProperty(vars(), Array.Array,
'rightImageResult') #leftImageResult is self
@Slot()
def calibrate(self):
if self._images:
proc = intrinsics.ChessboardFinderProc(4)
corners_list_left = proc(self._images, self._pattern())
proc = intrinsics.ChessboardFinderProc(4)
corners_list_right = proc(self._images_right, self._pattern())
with open(self._leftCalibPath, 'rb') as f:
left_calib = pickle.load(f)
with open(self._rightCalibPath, 'rb') as f:
right_calib = pickle.load(f)
self._calibration = intrinsics.calibrate_camera_stereo(
corners_list_left, corners_list_right,
intrinsics.make_object_points(self._pattern(),
self._patternSpecs['size']),
self._image_size(), left_calib['matrix'],
left_calib['distortion'], right_calib['matrix'],
right_calib['distortion'])
self._update_outputs(None)
def _update(self):
if self._left is None or self._imageArray is None:
return
if self._left._imageArray.ndarray.size > 0 and self._imageArray.ndarray.size > 0:
self.ndarray = self._imageArray.ndarray
self._rightImageResult.ndarray = self._imageArray.ndarray
if abs(self._imageArray.timestamp -
self._left._imageArray.timestamp) < 1e-4: #100us appart
left = self._imageArray.ndarray
right = self._imageArrayRight.ndarray
gray_left = cv2.cvtColor(left, cv2.COLOR_RGB2GRAY)
gray_right = cv2.cvtColor(right, cv2.COLOR_RGB2GRAY)
rv_right = self._handle_new_image(gray_right)
if rv_right != gray_right:
#chessboard found in right image, let's look in left image:
self._rightImageResult.ndarray = rv_right
rv_left = self._handle_new_image(gray_left)
if rv_left != gray_left:
#left image was added to the database, let's add right image too
self.ndarray = rv_left
self._images_right.append(right)
return super()._update()
class Synchronized(Product.Product):
def __init__(self,
parent=None,
platform=None,
params={
'sync_labels': ['*'],
'interp_labels': [],
'tolerance_us': -1e3
}):
super(Synchronized, self).__init__(parent)
self._platform = platform
self._parameters = params
self._nIndices = 0
self._synchronized = None
def cb_parameters(self):
self._dataset = None
Product.InputProperty(vars(), QVariant, 'parameters', cb_parameters)
def cb_platform(self):
self._synchronized = None
Product.InputProperty(vars(), Platform, 'platform', cb_platform)
sensorNamesChanged = Signal()
@Property(QVariant, notify=sensorNamesChanged)
def sensorNames(self):
if self._synchronized is None:
return QVariant([])
names = set()
for k in self._synchronized.keys():
sensor_type, position, _ = platform.parse_datasource_name(k)
names.add('{}_{}'.format(sensor_type, position))
return QVariant(list(names))
@Slot(list, result=list)
def expandWildcards(self, labels):
if self._synchronized is not None:
return self._synchronized.expand_wildcards(labels)
datasourcesChanged = Signal()
@Property(QVariant, notify=datasourcesChanged)
def datasources(self):
if self._synchronized is None:
return QVariant([])
return QVariant(self._synchronized.keys())
nIndicesChanged = Signal()
@Property(int, notify=nIndicesChanged)
def nIndices(self):
return self._nIndices
def __getitem__(self, key):
return self._synchronized[key]
def _update(self):
if self._platform is not None:
self._synchronized = self._platform._platform.synchronized(
**self._parameters)
self._nIndices = len(self._synchronized)
self.nIndicesChanged.emit()
self.datasourcesChanged.emit()
self.sensorNamesChanged.emit()
class Array(Product.Product):
arrayChanged = Signal()
def __init__(self, parent=None, ndarray=None):
super(Array, self).__init__(parent)
self._input = None
self.ndarray = None
self._timestamp = 0
self.productClean.connect(self.arrayChanged)
self.set_ndarray(ndarray)
def set_ndarray(self, ndarray):
if (
ndarray is not None or self.ndarray is not None
): # if both array are None, we know they're the same, otherwise, we assume they're not
self.ndarray = ndarray
self.makeDirty()
def input_cb(self):
if self._input:
if isinstance(self._input, list):
if len(self._input) > 0 and isinstance(self._input[0],
QVector3D):
self._input = [[v.x(), v.y(), v.z()] for v in self._input]
a = np.array(self._input, self.ndarray.dtype)
elif isinstance(self._input, np.array):
a = self._input.as_type(self.ndarray.type)
if len(a.shape) == 1 and len(self.ndarray.shape) == 2:
a = np.expand_dims(a, axis=1) # the most frequent case
assert len(a.shape) == len(self.ndarray.shape)
for i in range(1, len(a.shape)):
assert a.shape[i] == self.ndarray.shape[i]
self.ndarray = a
Product.InputProperty(vars(), 'QVariant', 'input', input_cb)
@Slot(result='QVariant')
def values(self):
return self.ndarray.tolist()
@staticmethod
def recurse_value(a, indices):
if len(indices) == 1:
return a[int(indices[0])]
return Array.recurse_value(a[int(indices[0])], indices[1:])
@Slot(list, result=float)
def value(self, indices):
assert len(indices) == len(self.ndarray.shape)
return float(
self.ndarray.dtype.type(Array.recurse_value(self.ndarray,
indices)))
@Property(int, notify=arrayChanged)
def size(self):
return self.ndarray.size
@Property(list, notify=arrayChanged)
def shape(self):
return list(self.ndarray.shape)
@Slot()
def resize(self, new_size):
'''
Resizes dim 0 only
'''
s = list(self.ndarray.shape)
s[0] = new_size
self.ndarray.resize(tuple(s), refcheck=False)
class ROSCalibratorFilter(Image.ImageFilter):
def __init__(self, parent=None):
super(ROSCalibratorFilter, self).__init__(parent)
self._patternSpecs = {'nx': 13, 'ny': 10, 'size': 0.145}
self._camSpecs = {
'h': 1440,
'v': 1080,
'f': 3.1e-3,
'pixel_size': 3.45e-6
}
self._xCoverage = 0
self._yCoverage = 0
self._sizeCoverage = 0
self._skewCoverage = 0
self._nImages = 0
self._matrix = ''
self._distortion = ''
self._calibrated = False
self._name = "camera_name"
self._intrinsics_hints = None
self._calibration = None
self._init()
def _init(self):
self._images = []
self._params = []
self._intrinsics_hints = None
self._calibration = None
self._intrinsics_hints = np.eye(3, dtype=np.float64)
self._intrinsics_hints[0, 0] = self._intrinsics_hints[
1, 1] = self._camSpecs['f'] / self._camSpecs['pixel_size']
self._intrinsics_hints[0, 2] = self._camSpecs['h'] / 2
self._intrinsics_hints[1, 2] = self._camSpecs['v'] / 2
#inputs:
Product.InputProperty(vars(), str, 'name')
Product.InputProperty(vars(), QVariant, 'patternSpecs')
Product.InputProperty(vars(), QVariant, 'camSpecs')
#outputs:
Product.ROProperty(vars(), int, 'nImages')
Product.ROProperty(vars(), float, 'sizeCoverage')
Product.ROProperty(vars(), float, 'skewCoverage')
Product.ROProperty(vars(), float, 'xCoverage')
Product.ROProperty(vars(), float, 'yCoverage')
Product.ROProperty(vars(), float, 'skewCoverage')
Product.ROProperty(vars(), bool, 'calibrated')
Product.ROProperty(vars(), str, 'matrix')
Product.ROProperty(vars(), str, 'distortion')
def _pattern(self):
return (self._patternSpecs['nx'], self._patternSpecs['ny'])
def _image_size(self):
if self._images:
return tuple(reversed(self._images[0].shape[:2]))
else:
return (self._camSpecs['h'], self._camSpecs['v'])
@Slot()
def calibrate(self):
if self._images:
proc = intrinsics.ChessboardFinderProc(4)
corners_list = proc(self._images, self._pattern())
self._calibration = intrinsics.calibrate_camera(
corners_list,
intrinsics.make_object_points(self._pattern(),
self._patternSpecs['size']),
self._image_size())
self._update_outputs(None)
@Slot(str, str, str)
def save(self, name, pos, path):
intrinsics.save_as_datasource(self._images, path, name, pos,
self._calibration, self._camSpecs,
self._patternSpecs)
@Slot(str)
def load(self, path):
self._init()
with tarfile.open(path) as tar:
for filename in tar.getmembers():
if filename.name[-4:] == ".png":
byte_arr = bytearray(tar.extractfile(filename).read())
arr = np.asarray(byte_arr, dtype=np.uint8)
self._handle_new_image(
cv2.imdecode(arr, cv2.IMREAD_GRAYSCALE))
def _update_outputs(self, params):
if params is not None:
x, y, size, skew = ([float(p[3]) for p in params])
self.set_yCoverage(self, y)
self.set_xCoverage(self, x)
self.set_sizeCoverage(self, size)
self.set_skewCoverage(self, skew)
self.set_nImages(self, len(self._images))
if self._calibration is not None:
self.set_calibrated(self, True)
for label in ['matrix', 'distortion']:
getattr(self, f'set_{label}')(self,
str(self._calibration[label]))
def _handle_new_image(self, gray, try_add_to_db=True):
scrib_mono, corners, downsampled_corners, _ = intrinsics.downsample_and_detect(
gray, self._pattern())
scrib = cv2.cvtColor(scrib_mono, cv2.COLOR_GRAY2BGR)
if corners is not None:
# Draw (potentially downsampled) corners onto display image
cv2.drawChessboardCorners(scrib, self._pattern(),
downsampled_corners, True)
if try_add_to_db:
# Add sample to database only if it's sufficiently different from any previous sample.
params = intrinsics.get_parameters(corners, self._pattern(),
reversed(gray.shape[:2]))
if intrinsics.is_good_sample(params, self._params):
self._images.append(gray)
self._params.append(params)
self._update_outputs(
intrinsics.compute_progress(self._params))
return scrib # only return scrip if sample was added
else:
return scrib
return gray
def _update(self):
if self._imageArray is not None and self._imageArray.ndarray.size > 0:
image = self._imageArray.ndarray
now = datetime.utcnow()
now = time.mktime(now.timetuple()) + float(now.microsecond) / 1e6
delta_t = time.time() - self._imageArray.timestamp
#if delta_t < 0.5:
self.ndarray = self._handle_new_image(
cv2.cvtColor(image, cv2.COLOR_RGB2GRAY))