From 3a057a10ac812d677a64adea72fe822f318670fe Mon Sep 17 00:00:00 2001 From: Galoshi Date: Thu, 25 Jul 2024 15:18:30 +0200 Subject: [PATCH 1/4] Added unit vector matrix to image client --- o3d3xx/pcic/image_client.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/o3d3xx/pcic/image_client.py b/o3d3xx/pcic/image_client.py index be3fa5d..39f62f4 100644 --- a/o3d3xx/pcic/image_client.py +++ b/o3d3xx/pcic/image_client.py @@ -12,7 +12,7 @@ def __init__(self, address, port): # disable all result output self.sendCommand("p0") # format string for all images - pcicConfig = "{ \"layouter\": \"flexible\", \"format\": { \"dataencoding\": \"ascii\" }, \"elements\": [ { \"type\": \"string\", \"value\": \"star\", \"id\": \"start_string\" }, { \"type\": \"blob\", \"id\": \"normalized_amplitude_image\" }, { \"type\": \"blob\", \"id\": \"distance_image\" }, { \"type\": \"blob\", \"id\": \"x_image\" }, { \"type\": \"blob\", \"id\": \"y_image\" }, { \"type\": \"blob\", \"id\": \"z_image\" }, { \"type\": \"blob\", \"id\": \"confidence_image\" }, { \"type\": \"blob\", \"id\": \"diagnostic_data\" }, { \"type\": \"blob\", \"id\": \"extrinsic_calibration\" }, { \"type\": \"blob\", \"id\": \"intrinsic_calibration\" },{ \"type\": \"blob\", \"id\": \"inverse_intrinsic_calibration\" },{ \"type\": \"string\", \"value\": \"stop\", \"id\": \"end_string\" } ] }" + pcicConfig = "{ \"layouter\": \"flexible\", \"format\": { \"dataencoding\": \"ascii\" }, \"elements\": [ { \"type\": \"string\", \"value\": \"star\", \"id\": \"start_string\" }, { \"type\": \"blob\", \"id\": \"normalized_amplitude_image\" }, { \"type\": \"blob\", \"id\": \"distance_image\" }, { \"type\": \"blob\", \"id\": \"x_image\" }, { \"type\": \"blob\", \"id\": \"y_image\" }, { \"type\": \"blob\", \"id\": \"z_image\" }, { \"type\": \"blob\", \"id\": \"confidence_image\" }, { \"type\": \"blob\", \"id\": \"diagnostic_data\" }, { \"type\": \"blob\", \"id\": \"extrinsic_calibration\" }, { \"type\": \"blob\", \"id\": \"intrinsic_calibration\" },{ \"type\": \"blob\", \"id\": \"inverse_intrinsic_calibration\" },{ \"id\": \"all_unit_vector_matrices\", \"type\": \"blob\" },{ \"type\": \"string\", \"value\": \"stop\", \"id\": \"end_string\" } ] }" answer = self.sendCommand("c%09d%s" % (len(pcicConfig), pcicConfig)) if str(answer, 'utf-8') != "*": raise @@ -102,6 +102,8 @@ def readNextFrame(self): image = array.array('f', bytes(data)) elif pixelFormat == 8: image = array.array('d', bytes(data)) + elif pixelFormat == 10: + image = array.array('i', bytes(data)) else: image = None @@ -133,6 +135,10 @@ def readNextFrame(self): elif chunkType == 202: result['z'] = image + # unit vector matrix + elif chunkType == 223: + result['unitVectorMatrix'] = image + # confidence image elif chunkType == 300: result['confidence'] = image From fee244d0475d0fc179bbc327f473f16ec82a927e Mon Sep 17 00:00:00 2001 From: Galoshi Date: Thu, 25 Jul 2024 15:41:30 +0200 Subject: [PATCH 2/4] Added an example script for setting extrinsic calibration parameters in new application --- examples/extrinsic_calibration.py | 60 +++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 examples/extrinsic_calibration.py diff --git a/examples/extrinsic_calibration.py b/examples/extrinsic_calibration.py new file mode 100644 index 0000000..04fc6a0 --- /dev/null +++ b/examples/extrinsic_calibration.py @@ -0,0 +1,60 @@ +import o3d3xx +import sys +import time + +if len(sys.argv) > 1: + address = sys.argv[1] +else: + address = '192.168.0.99' + +# create device +device = o3d3xx.Device(address) + +# open a session and create an application for editing +session = device.requestSession() +session.startEdit() +applicationIndex = session.edit.createApplication() +application = session.edit.editApplication(applicationIndex) + +# configure the application to +# - double exposure +application.imagerConfig.changeType("under5m_moderate") +# - process interface trigger +application.setParameter("TriggerMode", "2") +# and perform an auto-exposure run to determine +# exposure times +application.imagerConfig.startCalculateExposureTime() +# wait until the auto-exposure process has finished +while application.imagerConfig.getProgressCalculateExposureTime() < 1.0: + time.sleep(1) +# name and save the application and stop editing +application.setParameter("Name", "o3d3xx-python example extrinsic calibration") + +application.save() +session.edit.stopEditingApplication() + +# set extrinsic calibration parameters +session.edit.device.setParameter("ExtrinsicCalibTransX", 0.0) # mm +session.edit.device.setParameter("ExtrinsicCalibTransY", 0.0) # mm +session.edit.device.setParameter("ExtrinsicCalibTransZ", 2530.0) # mm +session.edit.device.setParameter("ExtrinsicCalibRotX", 6.32) # degree +session.edit.device.setParameter("ExtrinsicCalibRotY", 5.11) # degree +session.edit.device.setParameter("ExtrinsicCalibRotZ", 25.75) # degree + +# set the new application as active and save the change +session.edit.device.setParameter("ActiveApplication", str(applicationIndex)) +session.edit.device.save() + +# finish the session +session.cancelSession() + +# create image client for retrieving the unit vector matrix +device = o3d3xx.ImageClient(address=address, port=50010) + +# trigger device +device.sendCommand(cmd="t") +# read frames +frames = device.readNextFrame() +# print unit vector matrix +unit_vector_matrix = frames["unitVectorMatrix"] +print(unit_vector_matrix) From 08b26352849b805149c95a39bc5714cfd7f149fa Mon Sep 17 00:00:00 2001 From: Galoshi Date: Fri, 26 Jul 2024 11:19:40 +0200 Subject: [PATCH 3/4] Fixed wrong array-protocol type string for unit vector matrix --- examples/extrinsic_calibration.py | 71 ++++++++++++++++++++++++------- o3d3xx/pcic/image_client.py | 7 +-- 2 files changed, 60 insertions(+), 18 deletions(-) diff --git a/examples/extrinsic_calibration.py b/examples/extrinsic_calibration.py index 04fc6a0..d8d62ff 100644 --- a/examples/extrinsic_calibration.py +++ b/examples/extrinsic_calibration.py @@ -7,14 +7,27 @@ else: address = '192.168.0.99' +ifm_app_name = "o3d3xx-python example extrinsic calibration" # create device -device = o3d3xx.Device(address) +ifm_device = o3d3xx.Device(address) + +# Try to find our ifm application on device; if it's not there, we're going to install it. +ifm_app_index = None +ifm_apps = ifm_device.rpc.getApplicationList() +for i, ifm_app in enumerate(ifm_apps, start=1): + if ifm_app['Name'] == ifm_app_name: + print(f'ifm application found on camera: {ifm_app_name}') + ifm_app_index = i + break # open a session and create an application for editing -session = device.requestSession() +session = ifm_device.requestSession() session.startEdit() -applicationIndex = session.edit.createApplication() -application = session.edit.editApplication(applicationIndex) + +if not ifm_app_index: + ifm_app_index = session.edit.createApplication() + +application = session.edit.editApplication(ifm_app_index) # configure the application to # - double exposure @@ -33,16 +46,17 @@ application.save() session.edit.stopEditingApplication() -# set extrinsic calibration parameters -session.edit.device.setParameter("ExtrinsicCalibTransX", 0.0) # mm -session.edit.device.setParameter("ExtrinsicCalibTransY", 0.0) # mm -session.edit.device.setParameter("ExtrinsicCalibTransZ", 2530.0) # mm -session.edit.device.setParameter("ExtrinsicCalibRotX", 6.32) # degree -session.edit.device.setParameter("ExtrinsicCalibRotY", 5.11) # degree -session.edit.device.setParameter("ExtrinsicCalibRotZ", 25.75) # degree +# translation in [mm] +session.edit.device.setParameter('ExtrinsicCalibTransX', '42.1') +session.edit.device.setParameter('ExtrinsicCalibTransY', '44.2') +session.edit.device.setParameter('ExtrinsicCalibTransZ', '2246.3') +# rotation in [degree] +session.edit.device.setParameter('ExtrinsicCalibRotX', '5.6') +session.edit.device.setParameter('ExtrinsicCalibRotY', '7.8') +session.edit.device.setParameter('ExtrinsicCalibRotZ', '9.1') # set the new application as active and save the change -session.edit.device.setParameter("ActiveApplication", str(applicationIndex)) +session.edit.device.setParameter("ActiveApplication", str(ifm_app_index)) session.edit.device.save() # finish the session @@ -55,6 +69,33 @@ device.sendCommand(cmd="t") # read frames frames = device.readNextFrame() -# print unit vector matrix -unit_vector_matrix = frames["unitVectorMatrix"] -print(unit_vector_matrix) + +test = frames['extrinsicCalibration'] +tX = frames['extrinsicCalibration'].transX +tY = frames['extrinsicCalibration'].transY +tZ = frames['extrinsicCalibration'].transZ + +# calculate the X,Y,Z coordinates and compare them to the sensor X,Y,Z +dist = frames['distance'] +unitVec = frames['unitVectors'] +sensorX = frames['x'] +sensorY = frames['y'] +sensorZ = frames['z'] + +for pixIdx in range(len(dist)): + x = 0 + y = 0 + z = 0 + if dist[pixIdx] != 0: # only for valid values + x = round(unitVec[pixIdx*3+0] * dist[pixIdx] + tX) + y = round(unitVec[pixIdx*3+1] * dist[pixIdx] + tY) + z = round(unitVec[pixIdx*3+2] * dist[pixIdx] + tZ) + + # Test the cartesian values for similarity. + # We do not expect exact equality due to round off + # errors. + assert(abs(sensorX[pixIdx]-x) <= 1 and + abs(sensorY[pixIdx]-y) <= 1 and + abs(sensorZ[pixIdx]-z) <= 1) + +print("Test passed") diff --git a/o3d3xx/pcic/image_client.py b/o3d3xx/pcic/image_client.py index 39f62f4..68ce9ca 100644 --- a/o3d3xx/pcic/image_client.py +++ b/o3d3xx/pcic/image_client.py @@ -102,8 +102,9 @@ def readNextFrame(self): image = array.array('f', bytes(data)) elif pixelFormat == 8: image = array.array('d', bytes(data)) - elif pixelFormat == 10: - image = array.array('i', bytes(data)) + elif pixelFormat == 10: # special format for 3*float32 + image = array.array('f', bytes(data)) + else: image = None @@ -137,7 +138,7 @@ def readNextFrame(self): # unit vector matrix elif chunkType == 223: - result['unitVectorMatrix'] = image + result['unitVectors'] = image # confidence image elif chunkType == 300: From 7f36175190b5dd1e18e4d857dd04a3171183356a Mon Sep 17 00:00:00 2001 From: Galoshi Date: Fri, 26 Jul 2024 11:20:39 +0200 Subject: [PATCH 4/4] Added similarity test for cartesian values to example script extrinsic_calibration.py --- examples/extrinsic_calibration.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/extrinsic_calibration.py b/examples/extrinsic_calibration.py index d8d62ff..243eb87 100644 --- a/examples/extrinsic_calibration.py +++ b/examples/extrinsic_calibration.py @@ -49,7 +49,7 @@ # translation in [mm] session.edit.device.setParameter('ExtrinsicCalibTransX', '42.1') session.edit.device.setParameter('ExtrinsicCalibTransY', '44.2') -session.edit.device.setParameter('ExtrinsicCalibTransZ', '2246.3') +session.edit.device.setParameter('ExtrinsicCalibTransZ', '46.3') # rotation in [degree] session.edit.device.setParameter('ExtrinsicCalibRotX', '5.6') session.edit.device.setParameter('ExtrinsicCalibRotY', '7.8')