preparing to merge to tof vga

Author: FlorinBuica-luxonis

src/DatatypeBindings.cpp

@@ -21,6 +21,7 @@ void bind_spatiallocationcalculatordata(pybind11::module& m, void* pCallstack);
 void bind_stereodepthconfig(pybind11::module& m, void* pCallstack);
 void bind_systeminformation(pybind11::module& m, void* pCallstack);
 void bind_trackedfeatures(pybind11::module& m, void* pCallstack);
+void bind_tofconfig(pybind11::module& m, void* pCallstack);
 void bind_tracklets(pybind11::module& m, void* pCallstack);
 
 void DatatypeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
@@ -46,6 +47,7 @@ void DatatypeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
     callstack.push_front(bind_stereodepthconfig);
     callstack.push_front(bind_systeminformation);
     callstack.push_front(bind_trackedfeatures);
+    callstack.push_front(bind_tofconfig);
     callstack.push_front(bind_tracklets);
 }
 

src/pipeline/datatype/ToFConfigBindings.cpp

@@ -58,8 +58,11 @@ void bind_tofconfig(pybind11::module& m, void* pCallstack){
     toFConfig
         .def(py::init<>())
         .def(py::init<std::shared_ptr<RawToFConfig>>())
-
+        
         .def("setDepthParams", static_cast<ToFConfig&(ToFConfig::*)(dai::ToFConfig::DepthParams)>(&ToFConfig::setDepthParams), py::arg("config"), DOC(dai, ToFConfig, setDepthParams, 2))
+        .def("setFreqModUsed", static_cast<ToFConfig&(ToFConfig::*)(dai::ToFConfig::DepthParams::TypeFMod)>(&ToFConfig::setFreqModUsed), DOC(dai, ToFConfig, setDepthParams, 2))
+        .def("setAvgPhaseShuffle", &ToFConfig::setAvgPhaseShuffle, DOC(dai, node, ToFConfig, setAvgPhaseShuffle))
+        .def("setMinAmplitude", &ToFConfig::setMinAmplitude, DOC(dai, node, ToFConfig, setMinAmplitude))
 
         .def("set", &ToFConfig::set, py::arg("config"), DOC(dai, ToFConfig, set))
         .def("get", &ToFConfig::get, DOC(dai, ToFConfig, get))

src/pipeline/node/NodeBindings.cpp

@@ -114,6 +114,7 @@ void bind_edgedetector(pybind11::module& m, void* pCallstack);
 void bind_featuretracker(pybind11::module& m, void* pCallstack);
 void bind_apriltag(pybind11::module& m, void* pCallstack);
 void bind_detectionparser(pybind11::module& m, void* pCallstack);
+void bind_tof(pybind11::module& m, void* pCallstack);
 
 void NodeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
     // Bind Node et al
@@ -143,6 +144,7 @@ void NodeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
     callstack.push_front(bind_featuretracker);
     callstack.push_front(bind_apriltag);
     callstack.push_front(bind_detectionparser);
+    callstack.push_front(bind_tof);
 }
 
 void NodeBindings::bind(pybind11::module& m, void* pCallstack){

src/pipeline/node/ToFBindings.cpp

@@ -29,13 +29,13 @@ void bind_tof(pybind11::module& m, void* pCallstack){
 
     // Properties
     tofProperties
-        .def_readwrite("initialConfig", &ToFProperties::initialConfig)
+        .def_readwrite("initialConfig", &ToFProperties::initialConfig, DOC(dai, ToFProperties, initialConfig))
         ;
 
     // Node
     tof
-        .def_readonly("input", &ToF::input, DOC(dai, node, ToF, input), DOC(dai, node, ToF, input))
         .def_readonly("inputConfig", &ToF::inputConfig, DOC(dai, node, ToF, inputConfig), DOC(dai, node, ToF, inputConfig))
+        .def_readonly("input", &ToF::input, DOC(dai, node, ToF, input), DOC(dai, node, ToF, input))
         .def_readonly("depth", &ToF::depth, DOC(dai, node, ToF, depth), DOC(dai, node, ToF, depth))
         .def_readonly("amplitude", &ToF::amplitude, DOC(dai, node, ToF, amplitude), DOC(dai, node, ToF, amplitude))
         .def_readonly("error", &ToF::error, DOC(dai, node, ToF, error), DOC(dai, node, ToF, error))

utilities/cam_test.py

@@ -189,6 +189,7 @@ def get(self):
 xout = {}
 xout_raw = {}
 streams = []
+tofConfig = {}
 for c in cam_list:
     tofEnableRaw = False
     xout[c] = pipeline.createXLinkOut()
@@ -204,9 +205,10 @@ def get(self):
             tof[c].depth.link(xout[c].input)
             xinTofConfig.out.link(tof[c].inputConfig)
             tofConfig = tof[c].initialConfig.get()
-            tofConfig.setFreqModUsed(dai.ToFConfig.DepthParams.TypeFMod.F_MOD_MIN)
-            tofConfig.SetAvgPhaseShuffle(True)
-            tofConfig.setMinAmplitude(20.0)
+            tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN
+            tofConfig.depthParams.avgPhaseShuffle = False
+            tofConfig.depthParams.minimumAmplitude = 3.0
+            tof[c].initialConfig.set(tofConfig)
     elif cam_type_color[c]:
         cam[c] = pipeline.createColorCamera()
         cam[c].setResolution(color_res_opts[args.color_resolution])
@@ -287,6 +289,7 @@ def exit_cleanly(signum, frame):
         fps_capt[c] = FPS()
 
     controlQueue = device.getInputQueue('control')
+    tofCfgQueue = device.getInputQueue('tofConfig')
 
     # Manual exposure/focus set step
     EXP_STEP = 500  # us
@@ -331,6 +334,7 @@ def exit_cleanly(signum, frame):
     chroma_denoise = 0
     control = 'none'
     show = False
+    tof_amp_min = tofConfig.depthParams.minimumAmplitude
 
     print("Cam:", *['     ' + c.ljust(8)
           for c in cam_list], "[host | capture timestamp]")
@@ -419,6 +423,11 @@ def exit_cleanly(signum, frame):
         elif key == ord('c'):
             capture_list = streams.copy()
             capture_time = time.strftime('%Y%m%d_%H%M%S')
+        elif key == ord('g'):
+            f_mod = dai.RawToFConfig.DepthParams.TypeFMod.MAX if tofConfig.depthParams.freqModUsed  == dai.RawToFConfig.DepthParams.TypeFMod.MIN else dai.RawToFConfig.DepthParams.TypeFMod.MIN
+            print("ToF toggling f_mod value to:", f_mod)
+            tofConfig.depthParams.freqModUsed = f_mod
+            tofCfgQueue.send(tofConfig)
         elif key == ord('t'):
             print("Autofocus trigger (and disable continuous)")
             ctrl = dai.CameraControl()
@@ -493,7 +502,7 @@ def exit_cleanly(signum, frame):
             if floodIntensity < 0:
                 floodIntensity = 0
             device.setIrFloodLightBrightness(floodIntensity)
-        elif key >= 0 and chr(key) in '34567890[]':
+        elif key >= 0 and chr(key) in '34567890[]p':
             if key == ord('3'):
                 control = 'awb_mode'
             elif key == ord('4'):
@@ -514,6 +523,8 @@ def exit_cleanly(signum, frame):
                 control = 'luma_denoise'
             elif key == ord(']'):
                 control = 'chroma_denoise'
+            elif key == ord('p'):
+                control = 'tof_amplitude_min'
             print("Selected control:", control)
         elif key in [ord('-'), ord('_'), ord('+'), ord('=')]:
             change = 0
@@ -564,4 +575,9 @@ def exit_cleanly(signum, frame):
                 chroma_denoise = clamp(chroma_denoise + change, 0, 4)
                 print("Chroma denoise:", chroma_denoise)
                 ctrl.setChromaDenoise(chroma_denoise)
+            elif control == 'tof_amplitude_min':
+                amp_min = clamp(tofConfig.depthParams.minimumAmplitude + change, 0, 50)
+                print("Setting min amplitude(confidence) to:", amp_min)
+                tofConfig.depthParams.minimumAmplitude = amp_min
+                tofCfgQueue.send(tofConfig)
             controlQueue.send(ctrl)