Camera undistort for RVC4 #1363

asahtik · 2025-06-23T11:59:17Z

Purpose

Implements undistortion on RVC4 (Camera / ImageManip)

Specification

None / not applicable

Dependencies & Potential Impact

None / not applicable

Deployment Plan

None / not applicable

Testing & Validation

None / not applicable

…e/camera_undistort_rvc4

moratom

Thanks!

Left some comments/questions.
We should also move the camera_undistort.py example from the RVC2 folder before merging.

moratom · 2025-06-30T16:18:00Z

tests/src/ondevice_tests/pipeline/node/image_manip_test.cpp

+    }
+
+    // Scale down crop
+    {
+        auto cfg = std::make_shared<dai::ImageManipConfig>(*config);
+        cfg->setOutputSize(600, 400, dai::ImageManipConfig::ResizeMode::CENTER_CROP);
+        configQueue->send(cfg);
+        auto outFrame = outputQueue->get<dai::ImgFrame>();
+        REQUIRE(outFrame != nullptr);
+        REQUIRE(outFrame->getWidth() == 600);
+        REQUIRE(outFrame->getHeight() == 400);
+    }
+
+    // Scale down letterbox
+    {
+        auto cfg = std::make_shared<dai::ImageManipConfig>(*config);
+        cfg->setOutputSize(600, 400, dai::ImageManipConfig::ResizeMode::LETTERBOX);
+        configQueue->send(cfg);
+        auto outFrame = outputQueue->get<dai::ImgFrame>();
+        REQUIRE(outFrame != nullptr);
+        REQUIRE(outFrame->getWidth() == 600);
+        REQUIRE(outFrame->getHeight() == 400);
+    }
+
+    // Scale down letterbox bg
+    {
+        auto cfg = std::make_shared<dai::ImageManipConfig>(*config);
+        cfg->setOutputSize(600, 400, dai::ImageManipConfig::ResizeMode::LETTERBOX);
+        cfg->setBackgroundColor(100, 0, 0);
+        configQueue->send(cfg);
+        auto outFrame = outputQueue->get<dai::ImgFrame>();
+        REQUIRE(outFrame != nullptr);
+        REQUIRE(outFrame->getWidth() == 600);
+        REQUIRE(outFrame->getHeight() == 400);
+    }
+
+    // Crop
+    {
+        auto cfg = std::make_shared<dai::ImageManipConfig>(*config);
+        cfg->addCrop(100, 200, 600, 400);
+        configQueue->send(cfg);
+        auto outFrame = outputQueue->get<dai::ImgFrame>();
+        REQUIRE(outFrame != nullptr);
+        REQUIRE(outFrame->getWidth() == 600);
+        REQUIRE(outFrame->getHeight() == 400);
+    }
+
+    // Affine
+    {
+        auto cfg = std::make_shared<dai::ImageManipConfig>(*config);
+        cfg->addCropRotatedRect(dai::RotatedRect(dai::Point2f(350, 250), dai::Size2f(600, 400), 20));
+        configQueue->send(cfg);
+        auto outFrame = outputQueue->get<dai::ImgFrame>();
+        REQUIRE(outFrame != nullptr);
+        REQUIRE(outFrame->getWidth() == 600);
+        REQUIRE(outFrame->getHeight() == 400);
+    }
+
+    // Scale down small
+    {
+        auto cfg = std::make_shared<dai::ImageManipConfig>(*config);
+        cfg->addCrop(100, 100, 199, 199);
+        cfg->setOutputSize(100, 100);
+        configQueue->send(cfg);
+        auto outFrame = outputQueue->get<dai::ImgFrame>();
+        REQUIRE(outFrame != nullptr);
+        REQUIRE(outFrame->getWidth() == 100);
+        REQUIRE(outFrame->getHeight() == 100);
+    }
+
+    p.stop();
+} 
+
+TEST_CASE("ImageManip NV12") {
+    runManipTests(dai::ImgFrame::Type::NV12, false);
+}
+
+TEST_CASE("ImageManip GRAY8") {
+    runManipTests(dai::ImgFrame::Type::GRAY8, false);
+}
+
+TEST_CASE("ImageManip RGB888i") {
+    runManipTests(dai::ImgFrame::Type::RGB888i, false);
+}
+
+TEST_CASE("ImageManip NV12 undistort no coefficients") {
+    runManipTests(dai::ImgFrame::Type::NV12, true);
+}
+
+TEST_CASE("ImageManip NV12 undistort") {
+    runManipTests(dai::ImgFrame::Type::NV12, true, {-7.56764030456543, 18.97133445739746, 0.0006435539107769728, -5.642612813971937e-05, 6.156050682067871, -7.587080001831055, 19.094820022583008, 5.732314109802246, 0.0, 0.0, 0.0, 0.0, -0.0009434317471459508, 0.002672438742592931});
+}
+
+TEST_CASE("ImageManip GRAY8 undistort") {
+    runManipTests(dai::ImgFrame::Type::GRAY8, true, {-7.56764030456543, 18.97133445739746, 0.0006435539107769728, -5.642612813971937e-05, 6.156050682067871, -7.587080001831055, 19.094820022583008, 5.732314109802246, 0.0, 0.0, 0.0, 0.0, -0.0009434317471459508, 0.002672438742592931});
+}
+
+TEST_CASE("ImageManip RGB888i undistort") {
+    runManipTests(dai::ImgFrame::Type::RGB888i, true, {-7.56764030456543, 18.97133445739746, 0.0006435539107769728, -5.642612813971937e-05, 6.156050682067871, -7.587080001831055, 19.094820022583008, 5.732314109802246, 0.0, 0.0, 0.0, 0.0, -0.0009434317471459508, 0.002672438742592931});
+}


This doesn't actually test if undistortion is working? I think we should add a distorted and an undistorted image and then compare byte by byte.

moratom · 2025-06-30T16:35:04Z

src/utility/ImageManipImpl.cpp

+#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT
+void dai::impl::UndistortOpenCvImpl::undistort(cv::Mat& src, cv::Mat& dst) {
+    if(dst.size().width == (int)width && dst.size().height == (int)height) {
+        cv::remap(src, dst, undistortMap1, undistortMap2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
+    } else if(dst.size().width == (int)width / 2 && dst.size().height == (int)height / 2) {
+        if(undistortMap1Half.empty() || undistortMap2Half.empty()) {
+            cv::Mat cvCameraMatrix(3, 3, CV_32F, this->cameraMatrix.data());
+            cvCameraMatrix.at<float>(0, 2) /= 2;
+            cvCameraMatrix.at<float>(1, 2) /= 2;
+            cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cvCameraMatrix, distCoeffs, cv::Size(width / 2, height / 2), 1);
+            cv::initUndistortRectifyMap(
+                cvCameraMatrix, distCoeffs, cv::Mat(), newCameraMatrix, cv::Size(width / 2, height / 2), CV_16SC2, undistortMap1Half, undistortMap2Half);
+        }
+        cv::remap(src, dst, undistortMap1Half, undistortMap2Half, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(127, 127));
+    } else {
+        throw std::runtime_error(fmt::format("UndistortImpl: Output size does not match the expected size (got {}x{}, expected {}x{} or {}x{})",
+                                             dst.size().width,
+                                             dst.size().height,
+                                             width,
+                                             height,
+                                             width / 2,
+                                             height / 2));
+    }
+}
+#endif


I think we should get the camera matrix for initUndistortRectifyMap from the ImageManip itself (and alpha should not be used) to keep the behavior predictable.

So right now however ImageManip node is configured the ImgTransformations and internally as well we should be able to tell what are the target image intrinsics and then use those.

…e/camera_undistort_rvc4

asahtik added 13 commits June 18, 2025 16:20

Implement undistortion in imagemanip

303b3f2

Fix undistort

aa0c922

Merge branch 'develop' of github.com:luxonis/depthai-core into featur…

a1689ae

…e/camera_undistort_rvc4

Fix manip undistort

62a175d

Add manip undistort tests

f7e5031

RVC4 FW: manip undistort

5f25fea

Manip undistort bugfixes

cf58fa7

RVC4 FW: Manip undistort fixes

9d60793

Style

eec590d

Set distortionCoefficients to 0 when undistorted

9dd490a

RVC4 FW: Set distortionCoefficients to 0 when undistorted

f17c4dc

Merge branch 'develop' of github.com:luxonis/depthai-core into featur…

35e9a57

…e/camera_undistort_rvc4

RVC4 FW: ImageManip bugfix

206fd0d

moratom reviewed Jun 30, 2025

View reviewed changes

asahtik added 8 commits July 1, 2025 10:51

Merge branch 'develop' of github.com:luxonis/depthai-core into featur…

3151cb1

…e/camera_undistort_rvc4

Merge branch 'develop' of github.com:luxonis/depthai-core into featur…

b10248b

…e/camera_undistort_rvc4

Merge branch 'develop' of github.com:luxonis/depthai-core into featur…

f1ab08b

…e/camera_undistort_rvc4

Move example, check if distortion is working in tests

f906b53

Merge branch 'develop' of github.com:luxonis/depthai-core into featur…

eac0a89

…e/camera_undistort_rvc4

Bugfixes, expand tests

986e36a

Do not resize when undistorting

a8a684d

RVC4 FW: image manip bugfixes

d77863b

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Camera undistort for RVC4 #1363

Camera undistort for RVC4 #1363

Uh oh!

asahtik commented Jun 23, 2025

Uh oh!

moratom left a comment

Uh oh!

moratom Jun 30, 2025

Uh oh!

moratom Jun 30, 2025

Uh oh!

Uh oh!

Camera undistort for RVC4 #1363

Are you sure you want to change the base?

Camera undistort for RVC4 #1363

Uh oh!

Conversation

asahtik commented Jun 23, 2025

Purpose

Specification

Dependencies & Potential Impact

Deployment Plan

Testing & Validation

Uh oh!

moratom left a comment

Choose a reason for hiding this comment

Uh oh!

moratom Jun 30, 2025

Choose a reason for hiding this comment

Uh oh!

moratom Jun 30, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!