Updated ToF docs, added diagram, updated settings

docs/source/components/nodes/color_camera.rst

@@ -69,13 +69,12 @@ For IMX378 (12MP), the **post-processing** works like this:
     │ ISP ├────────────────►│  video  ├───────────────►│ preview  │
     └─────┘  max 3840x2160  └─────────┘  and cropping  └──────────┘
 
-If resolution was set to 12MP, and we were to use ``video``, we'd get a 4K frame (3840x2160) cropped from the center of the 12MP frame.
+If the resolution is set to 12MP and video mode is used, a 4K frame (3840x2160) will be cropped from the center of the 12MP frame.
 
 Full FOV
 ########
 
-Some sensors (let's take IXM378 for an example) will, by default, have 1080P resolution set, which is a crop from the full sensor resolution.
-You can print sensor features to see how FOV is affected by the selected sensor resolution:
+Some sensors, such as the IXM378, default to a 1080P resolution, which is a crop from the full sensor resolution. You can print sensor features to see how the field of view (FOV) is affected by the selected sensor resolution:
 
 .. code-block:: python
 
@@ -104,8 +103,8 @@ You can print sensor features to see how FOV is affected by the selected sensor
     #     {width: 1280, height: 800, minFps: 1.687, maxFps: 129.6, type: MONO, fov: {x:0, y: 0, width: 1280, height: 800}}
     #     {width: 640, height: 400, minFps: 1.687, maxFps: 255.7, type: MONO, fov: {x:0, y: 0, width: 1280, height: 800}}
 
-So for IMX378, if we select 4K or 1080P resolution, FOV will be cropped ~5% horizontally and ~29% vertically. So we can either select ``THE_12_MP``,
-``THE_1352X1012``, or ``THE_2024X1520`` resolution to get the full sensor FOV.
+For the IMX378 sensor, selecting a 4K or 1080P resolution results in approximately 5% horizontal and 29% vertical FOV cropping.
+To utilize the full sensor FOV, you should choose one of the following resolutions: ``THE_12_MP``, ``THE_1352X1012``, or ``THE_2024X1520``.
 
 Usage
 #####

docs/source/components/nodes/tof.rst

@@ -61,17 +61,40 @@ In :ref:`ToF depth` example we allow users to quickly configure ToF settings. Th
 
 And these settings are up to the user:
 
-- Optical Correction: It's a process that corrects the optical effect (On -> ToF returns distance represented by Green Line), so it matches :ref:`StereoDepth` depth reporting. It does rectification and distance to depth conversion (Z-map).
+(level+1)*1.5m + phaseUnwrapErrorThreshold/2
+- Optical Correction: It's a process that corrects the optical effect. When enabled, the ToF returns depth map (represented by Green Line on graph below) instead of distance, so it matches :ref:`StereoDepth` depth reporting. It does rectification and distance to depth conversion (Z-map).
 - Phase Unwrapping - Process that corrects the phase wrapping effect of the ToF sensor. You can set it to [0..5 are optimized]. The higher the number, the longer the ToF range, but it also increases the noise.
-    - `0` - Disabled.
-    - `1` - Up to 1.5 meters
-    - `2` - Up to 3 meters
-    - `3` - Up to 4.5 meters
-    - `4` - Up to 6 meters
-    - `5` - Up to 7.5 meters (not recommended for most applications)
+    - `0` - Disabled, up to ~1.5 meters
+    - `1` - Up to ~3 meters
+    - `2` - Up to ~4.5 meters
+    - `3` - Up to ~6 meters
+    - `4` - Up to ~7.5 meters
+- Burst mode: When enabled, ToF node won't reuse frames, as shown on the graph below. It's related to post-processing of the ToF frames, not the actual sensor/projector. It's disabled by default.
+- Phase shuffle Temporal filter: Averages shuffled and non-shuffled frames of the same modulation frequency to reduce noise. It's enabled by default. You can disable it to reduce :ref:`ToF motion blur` and system load.
 
 .. image:: /_static/images/components/tof-optical-correction.png
 
+.. image:: /_static/images/components/tof-diagram.png
+
+Phase unwrapping
+################
+
+If the time it takes for the light to travel from ToF sensor and back exceeds the period of the emitted wave (1.5m or 1.8m), the resulting measurement will "wrap" back to a lower value. This is called phase wrapping.
+It's similar to how a clock resets after 12 hours. Phase unwrapping is possible as our ToF has two different modulation frequencies (80Mhz and 100MHz).
+
+Phase unwrapping aims to correct this by allowing the sensor to interpret longer distances without confusion. It uses algorithms to keep track of how many cycles (round trips of the wave) have occurred,
+thus correcting the "wrapped" phases. The downside is that the more cycles the sensor has to keep track of, the more noise it introduces into the measurement.
+
+ToF motion blur
+###############
+
+To reduce motion blur, we recommend these settings:
+
+- Increase camera FPS. It goes up to 160 FPS, which causes frame capture to be the fastest (6.25ms between frames). This will reduce motion blur as ToF combines multiple frames to get the depth. Note that 160FPS will increase system load significantly (see :ref:`Debugging <Debugging DepthAI pipeline>`). Note also that higher FPS -> lower exposure times, which can increase noise.
+- Disable phase shuffle temporal filter. This will introduce more noise.
+- Disable phase unwrapping. This will reduce max distance to 1.5 meters, so about 1 cubic meter of space will be visible.
+- Enable burst mode. This is irrelevant if shuffle filter and phase unwrapping are disabled (see diagram above). When enabled, ToF node won't reuse frames (lower FPS).
+
 Max distance
 ############
 
@@ -84,10 +107,10 @@ Maximum ToF distance depends on the phase unwrapping level and modulation freque
   c & = 299792458.0 \quad \text{//! speed of light in m/s} \\
   MAX\_80MHZ\_MM & = \frac{c}{80000000 \times 2} \times 1000 \quad \text{//! convert speed of light to mm/160ns} \\
   MAX\_MM\_80MHZ & = \text{round}(MAX\_80MHZ\_MM) \quad \text{// round(1873.7) -> 1874} \\
-  MAX\_DIST\_80MHZ & = 1874 \times (\text{phaseUnwrappingLevel} + 1) \quad \text{//! in mm for 80 MHz} \\
+  MAX\_DIST\_80MHZ & = (\text{phaseUnwrappingLevel} + 1) \times 1.5 \times 1000 + \frac{\text{phaseUnwrapErrorThreshold}}{2} \quad \text{//! corrected formula in mm for 80 MHz} \\
   MAX\_100MHZ\_MM & = \frac{c}{100000000 \times 2} \times 1000 \quad \text{//! convert speed of light to mm/200ns} \\
   MAX\_MM\_100MHZ & = \text{round}(MAX\_100MHZ\_MM) \quad \text{// round(1498.9636) -> 1499} \\
-  MAX\_DIST\_100MHZ & = 1499 \times (\text{phaseUnwrappingLevel} + 1) \quad \text{//! in mm for 100 MHz}
+  MAX\_DIST\_100MHZ & = (\text{phaseUnwrappingLevel} + 1) \times 1.5 \times 1000 + \frac{\text{phaseUnwrapErrorThreshold}}{2} \quad \text{//! corrected formula in mm for 100 MHz} \\
   \end{align*}
 
 Usage
@@ -113,6 +136,14 @@ Usage
     # Median filter, kernel size 5x5
     tof.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_5x5)
 
+    tofConfig = tof.initialConfig.get()
+    # Temporal filter averages shuffle/non-shuffle frequencies
+    tofConfig.enablePhaseShuffleTemporalFilter = True
+    # Phase unwrapping, for longer range.
+    tofConfig.phaseUnwrappingLevel = 4 # Up to 7.5 meters
+    tofConfig.phaseUnwrapErrorThreshold = 300
+    tof.initialConfig.set(tofConfig)
+
     # ToF node converts raw sensor frames into depth
     tof_cam.raw.link(tof.input)
 

docs/source/samples/ToF/tof_depth.rst

@@ -22,12 +22,12 @@ With keyboard you can configure ToF settings:
 - Temperature Correction: Turn on/off with `t`. It's a process that corrects the temperature effect of the ToF sensor. Should be enabled.
 - Optical Correction: Turn on/off with `o`. It's a process that corrects the optical effect (On -> ToF returns distance represented by Green Line), so it matches stereo depth reporting.
 - Phase Unwrapping - Process that corrects the phase wrapping effect of the ToF sensor. The higher the number, the longer the ToF range, but it also increases the noise.
-    - `0` - Disabled.
-    - `1` - Up to 1.5 meters
-    - `2` - Up to 3 meters
-    - `3` - Up to 4.5 meters
-    - `4` - Up to 6 meters
-    - `5` - Up to 6 meters
+    - `0` - Disabled, up to ~1.5 meters
+    - `1` - Up to ~3 meters
+    - `2` - Up to ~4.5 meters
+    - `3` - Up to ~6 meters
+    - `4` - Up to ~7.5 meters
+    - `5` - Up to ~9 meters (increased noise - not recommended for most applications)
 
 .. image:: /_static/images/components/tof-optical-correction.png
 

examples/ToF/tof_depth.py

@@ -18,10 +18,12 @@ def create_pipeline():
 
     # Configure the ToF node
     tofConfig = tof.initialConfig.get()
+    # Disable for debugging:
     tofConfig.enableFPPNCorrection = True
-    tofConfig.enableOpticalCorrection = True
     tofConfig.enableWiggleCorrection = True
-    tofConfig.enableTemperatureCorrection = False
+    tofConfig.enableTemperatureCorrection = True
+    # Optional:
+    tofConfig.enableOpticalCorrection = True
     tofConfig.phaseUnwrappingLevel = 4
     tofConfig.phaseUnwrapErrorThreshold = 300
     xinTofConfig = pipeline.create(dai.node.XLinkIn)
@@ -32,7 +34,7 @@ def create_pipeline():
 
     cam_tof = pipeline.create(dai.node.Camera)
     cam_tof.properties.numFramesPoolRaw = 5
-    cam_tof.setFps(60)
+    cam_tof.setFps(60) # ToF node will produce depth frames at /2 of this rate
     cam_tof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG)
     cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_A)
     cam_tof.raw.link(tof.input)
@@ -41,7 +43,6 @@ def create_pipeline():
     xout.setStreamName("depth")
     tof.depth.link(xout.input)
 
-
     tofConfig = tof.initialConfig.get()
 
     return pipeline, tofConfig
@@ -76,7 +77,7 @@ def create_pipeline():
                 break
             elif key == ord('0'):
                 tofConfig.enablePhaseUnwrapping = False
-                tofConfig.phaseUnwrappingLevel = 4
+                tofConfig.phaseUnwrappingLevel = 0
                 tofConfigInQueue.send(tofConfig)
             elif key == ord('1'):
                 tofConfig.enablePhaseUnwrapping = True
@@ -114,15 +115,8 @@ def create_pipeline():
 
             imgFrame = qDepth.get()  # blocking call, will wait until a new data has arrived
             depth_map = imgFrame.getFrame()
-
-            depth_downscaled = depth_map[::4]
-            non_zero_depth = depth_downscaled[depth_downscaled != 0]  # Remove invalid depth values
-            if len(non_zero_depth) == 0:
-                min_depth, max_depth = 0, 0
-            else:
-                min_depth = 200
-                max_depth = 6000
-            depth_colorized = np.interp(depth_map, (min_depth, max_depth), (0, 255)).astype(np.uint8)
+            max_depth = (tofConfig.phaseUnwrappingLevel - 1) * 1874 # 80MHz modulation freq.
+            depth_colorized = np.interp(depth_map, (0, max_depth), (0, 255)).astype(np.uint8)
             depth_colorized = cv2.applyColorMap(depth_colorized, cvColorMap)
 
             cv2.imshow("Colorized depth", depth_colorized)