Merge pull request #1190 from luxonis/v3_camera_undistortion_rvc2

RVC2: Add undistortion capability to Camera node
luxonis · Jan 21, 2025 · 261f4b5 · 261f4b5
2 parents 393378b + 72cc2a1
commit 261f4b5
Show file tree

Hide file tree

Showing 12 changed files with 77 additions and 7 deletions.
diff --git a/.gitignore b/.gitignore
@@ -41,3 +41,5 @@ _builds/
 
 *.swp
 /env
+
+.history/
diff --git a/bindings/python/src/capabilities/ImgFrameCapabilityBindings.cpp b/bindings/python/src/capabilities/ImgFrameCapabilityBindings.cpp
@@ -27,6 +27,7 @@ void ImgFrameCapabilityBindings::bind(pybind11::module& m, void* pCallstack) {
         .def_readwrite("fps", &ImgFrameCapability::fps)
         .def_readwrite("type", &ImgFrameCapability::type)
         .def_readwrite("resizeMode", &ImgFrameCapability::resizeMode)
+        .def_readwrite("enableUndistortion", &ImgFrameCapability::enableUndistortion)
 
         ;
 }
diff --git a/bindings/python/src/pipeline/datatype/ImageManipConfigV2Bindings.cpp b/bindings/python/src/pipeline/datatype/ImageManipConfigV2Bindings.cpp
@@ -96,6 +96,8 @@ void bind_imagemanipconfigv2(pybind11::module& m, void* pCallstack) {
         .def("setReusePreviousImage", &ImageManipConfigV2::setReusePreviousImage, py::arg("reuse"), DOC(dai, ImageManipConfigV2, setReusePreviousImage))
         .def("setSkipCurrentImage", &ImageManipConfigV2::setSkipCurrentImage, py::arg("skip"), DOC(dai, ImageManipConfigV2, setSkipCurrentImage))
         .def("setFrameType", &ImageManipConfigV2::setFrameType, py::arg("type"), DOC(dai, ImageManipConfigV2, setFrameType))
+        .def("setUndistort", &ImageManipConfigV2::setUndistort, py::arg("undistort"), DOC(dai, ImageManipConfigV2, setUndistort))
+        .def("getUndistort", &ImageManipConfigV2::getUndistort, DOC(dai, ImageManipConfigV2, getUndistort))
         .def("setColormap",
              static_cast<ImageManipConfigV2& (ImageManipConfigV2::*)(Colormap)>(&ImageManipConfigV2::setColormap),
              py::arg("colormap"),

diff --git a/bindings/python/src/pipeline/node/CameraBindings.cpp b/bindings/python/src/pipeline/node/CameraBindings.cpp
@@ -27,11 +27,12 @@ void bind_camera(pybind11::module& m, void* pCallstack) {
         // .def("setCamera", &Camera::setCamera, "name"_a, DOC(dai, node, Camera, setCamera))
         // .def("getCamera", &Camera::getCamera, DOC(dai, node, Camera, getCamera))
         .def("requestOutput",
-             py::overload_cast<std::pair<uint32_t, uint32_t>, std::optional<ImgFrame::Type>, ImgResizeMode, float>(&Camera::requestOutput),
+             py::overload_cast<std::pair<uint32_t, uint32_t>, std::optional<ImgFrame::Type>, ImgResizeMode, float, std::optional<bool>>(&Camera::requestOutput),
              "size"_a,
              "type"_a = std::nullopt,
              "resizeMode"_a = dai::ImgResizeMode::CROP,
              "fps"_a = 30,
+             "enableUndistortion"_a = std::nullopt,
              py::return_value_policy::reference_internal,
              DOC(dai, node, Camera, requestOutput))
         .def("requestOutput",

diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake
@@ -4,4 +4,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot")
 
 # "version if applicable"
 # set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+93f7b75a885aa32f44c5e9f53b74470c49d2b1af")
-set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+3cf298000129597b70c7f266bb70bc13647a356f")
+set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+a0274cd7dafa3622160fe0c7d1fccfc1937ec268")
diff --git a/cmake/Depthai/DepthaiDeviceSideConfig.cmake b/cmake/Depthai/DepthaiDeviceSideConfig.cmake
@@ -2,7 +2,7 @@
 set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot")
 
 # "full commit hash of device side binary"
-set(DEPTHAI_DEVICE_SIDE_COMMIT "3c7d7c93a4c4c55d235ff7e406ed8824b9c18fef")
+set(DEPTHAI_DEVICE_SIDE_COMMIT "976351fda3b70483e37882f989686bf0aa8c604d")
 
 # "version if applicable"
 set(DEPTHAI_DEVICE_SIDE_VERSION "")
diff --git a/examples/python/RVC2/Camera/camera_undistortion.py b/examples/python/RVC2/Camera/camera_undistortion.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+
+import cv2
+import depthai as dai
+
+# Create pipeline
+with dai.Pipeline() as pipeline:
+    # Define source and output
+    cam = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
+    croppedQueue = cam.requestOutput((300,300), resizeMode=dai.ImgResizeMode.CROP, enableUndistortion=True).createOutputQueue()
+    stretchedQueue = cam.requestOutput((300,300), resizeMode=dai.ImgResizeMode.STRETCH, enableUndistortion=True).createOutputQueue()
+    letterBoxedQueue = cam.requestOutput((300,300), resizeMode=dai.ImgResizeMode.LETTERBOX, enableUndistortion=True).createOutputQueue()
+
+    # Connect to device and start pipeline
+    pipeline.start()
+    while pipeline.isRunning():
+        croppedIn = croppedQueue.get()
+        assert isinstance(croppedIn, dai.ImgFrame)
+        cv2.imshow("cropped undistorted", croppedIn.getCvFrame())
+
+        stretchedIn = stretchedQueue.get()
+        assert isinstance(stretchedIn, dai.ImgFrame)
+        cv2.imshow("stretched undistorted", stretchedIn.getCvFrame())
+
+        letterBoxedIn = letterBoxedQueue.get()
+        assert isinstance(letterBoxedIn, dai.ImgFrame)
+        cv2.imshow("letterboxed undistorted", letterBoxedIn.getCvFrame())
+
+        if cv2.waitKey(1) == ord("q"):
+            break
diff --git a/include/depthai/capabilities/ImgFrameCapability.hpp b/include/depthai/capabilities/ImgFrameCapability.hpp
@@ -46,9 +46,10 @@ class ImgFrameCapability : public CapabilityCRTP<Capability, ImgFrameCapability>
     CapabilityRange<float> fps;
     std::optional<ImgFrame::Type> type;
     ImgResizeMode resizeMode{ImgResizeMode::CROP};
+    std::optional<bool> enableUndistortion;
     // TODO(jakgra) add optional CapabilityRange fov / max-min horiz. / vertical crop;
 
-    DEPTHAI_SERIALIZE(ImgFrameCapability, size, fps, type, resizeMode);
+    DEPTHAI_SERIALIZE(ImgFrameCapability, size, fps, type, resizeMode, enableUndistortion);
 
    private:
     class Impl;

diff --git a/include/depthai/pipeline/datatype/ImageManipConfigV2.hpp b/include/depthai/pipeline/datatype/ImageManipConfigV2.hpp
@@ -225,6 +225,7 @@ class ImageManipOpsBase : public ImageManipOpsEnums {
     uint8_t backgroundG = 0;
     uint8_t backgroundB = 0;
     Colormap colormap = Colormap::NONE;
+    bool undistort = false;
 
     C operations{};
 
@@ -242,6 +243,7 @@ class ImageManipOpsBase : public ImageManipOpsEnums {
         to.backgroundG = backgroundG;
         to.backgroundB = backgroundB;
         to.colormap = colormap;
+        to.undistort = undistort;
 
         to.operations.clear();
         to.operations.insert(to.operations.end(), operations.begin(), operations.end());
@@ -373,6 +375,15 @@ class ImageManipOpsBase : public ImageManipOpsEnums {
         return *this;
     }
 
+    ImageManipOpsBase& setUndistort(bool undistort) {
+        this->undistort = undistort;
+        return *this;
+    }
+
+    bool getUndistort() const {
+        return undistort;
+    }
+
     const C& getOperations() const {
         return this->operations;
     }
@@ -383,7 +394,7 @@ class ImageManipOpsBase : public ImageManipOpsEnums {
     }
 
     DEPTHAI_SERIALIZE(
-        ImageManipOpsBase, operations, outputWidth, outputHeight, center, resizeMode, background, backgroundR, backgroundG, backgroundB, colormap);
+        ImageManipOpsBase, operations, outputWidth, outputHeight, center, resizeMode, background, backgroundR, backgroundG, backgroundB, colormap, undistort);
 };
 
 /**
@@ -518,6 +529,17 @@ class ImageManipConfigV2 : public Buffer {
      */
     ImageManipConfigV2& setFrameType(ImgFrame::Type frameType);
 
+    /**
+     * Sets the undistort flag
+     */
+    ImageManipConfigV2& setUndistort(bool undistort);
+
+    /**
+     * Gets the undistort flag
+     * @returns True if undistort is enabled, false otherwise
+     */
+    bool getUndistort() const;
+
     /**
      * Instruct ImageManip to not remove current image from its queue and use the same for next message.
      * @param reuse True to enable reuse, false otherwise

diff --git a/include/depthai/pipeline/node/Camera.hpp b/include/depthai/pipeline/node/Camera.hpp
@@ -23,7 +23,8 @@ class Camera : public DeviceNodeCRTP<DeviceNode, Camera, CameraProperties>, publ
     Node::Output* requestOutput(std::pair<uint32_t, uint32_t> size,
                                 std::optional<ImgFrame::Type> type = std::nullopt,
                                 ImgResizeMode resizeMode = ImgResizeMode::CROP,
-                                float fps = 30);
+                                float fps = 30,
+                                std::optional<bool> enableUndistortion = std::nullopt);
     /**
      * Request output with advanced controls. Mainly to be used by custom node writers.
      */

diff --git a/src/pipeline/datatype/ImageManipConfigV2.cpp b/src/pipeline/datatype/ImageManipConfigV2.cpp
@@ -86,6 +86,15 @@ ImageManipConfigV2& ImageManipConfigV2::setFrameType(ImgFrame::Type frameType) {
     return *this;
 }
 
+ImageManipConfigV2& ImageManipConfigV2::setUndistort(bool undistort) {
+    base.setUndistort(undistort);
+    return *this;
+}
+
+bool ImageManipConfigV2::getUndistort() const {
+    return base.getUndistort();
+}
+
 ImageManipConfigV2& ImageManipConfigV2::setReusePreviousImage(bool reuse) {
     reusePreviousImage = reuse;
     return *this;

diff --git a/src/pipeline/node/Camera.cpp b/src/pipeline/node/Camera.cpp
@@ -148,12 +148,13 @@ Node::Output* Camera::requestFullResolutionOutput(ImgFrame::Type type, float fps
     return pimpl->requestOutput(*this, cap, false);
 }
 
-Node::Output* Camera::requestOutput(std::pair<uint32_t, uint32_t> size, std::optional<ImgFrame::Type> type, ImgResizeMode resizeMode, float fps) {
+Node::Output* Camera::requestOutput(std::pair<uint32_t, uint32_t> size, std::optional<ImgFrame::Type> type, ImgResizeMode resizeMode, float fps, std::optional<bool> enableUndistortion) {
     ImgFrameCapability cap;
     cap.size.fixed(size);
     cap.fps.fixed(fps);
     cap.type = type;
     cap.resizeMode = resizeMode;
+    cap.enableUndistortion = enableUndistortion;
     return pimpl->requestOutput(*this, cap, false);
 }