Fix: Perspective distortion numerical stability (#1793)

* Fix numerical instability issue in undistortPerspective point

* Add single cam remapping example

* Add const to var declaration and use CamelCase in example

* Add cpp example

Author: aljazkonec1

examples/cpp/Remapping/CMakeLists.txt

@@ -5,4 +5,7 @@ cmake_minimum_required(VERSION 3.10)
 ## function: dai_set_example_test_labels(example_name ...)
 
 dai_add_example(point_remapping point_remapping.cpp ON OFF)
+dai_add_example(single_cam_remapping single_cam_remapping.cpp ON OFF)
+
 dai_set_example_test_labels(point_remapping ondevice rvc2_all rvc4 ci)
+dai_set_example_test_labels(single_cam_remapping ondevice rvc2_all rvc4 ci)

examples/cpp/Remapping/single_cam_remapping.cpp

@@ -0,0 +1,163 @@
+#include <atomic>
+#include <cmath>
+#include <csignal>
+#include <iomanip>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <optional>
+#include <sstream>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "depthai/depthai.hpp"
+
+const std::string DISTORTED_WINDOW = "CAM_A distorted 640x480";
+const std::string UNDISTORTED_WINDOW = "CAM_A undistorted 1000x400";
+const std::pair<uint32_t, uint32_t> DISTORTED_SIZE = {640, 480};
+const std::pair<uint32_t, uint32_t> UNDISTORTED_SIZE = {1000, 400};
+
+std::atomic<bool> quitEvent(false);
+std::optional<cv::Point> selectedPoint = std::nullopt;
+
+void signalHandler(int) {
+    quitEvent = true;
+}
+
+void onLeftClick(int event, int x, int y, int flags, void* param) {
+    (void)flags;
+    (void)param;
+
+    if(event == cv::EVENT_LBUTTONDOWN) {
+        selectedPoint = cv::Point(x, y);
+    }
+}
+
+cv::Mat toColorFrame(const cv::Mat& frame) {
+    if(frame.channels() == 3) {
+        return frame;
+    }
+
+    cv::Mat colorFrame;
+    cv::cvtColor(frame, colorFrame, cv::COLOR_GRAY2BGR);
+    return colorFrame;
+}
+
+void addStatusLines(cv::Mat& frame, const std::vector<std::string>& lines, const cv::Scalar& color = cv::Scalar(255, 0, 255)) {
+    for(size_t index = 0; index < lines.size(); ++index) {
+        cv::putText(frame,
+                    lines[index],
+                    cv::Point(10, 28 + static_cast<int>(index) * 24),
+                    cv::FONT_HERSHEY_SIMPLEX,
+                    0.65,
+                    color,
+                    2,
+                    cv::LINE_AA);
+    }
+}
+
+bool isInsideFrame(const dai::Point2f& point, const cv::Mat& frame) {
+    return 0 <= point.x && point.x < frame.cols && 0 <= point.y && point.y < frame.rows;
+}
+
+void drawPoint(cv::Mat& frame, const std::optional<dai::Point2f>& point, const cv::Scalar& color) {
+    if(!point.has_value() || !isInsideFrame(*point, frame)) {
+        return;
+    }
+
+    const cv::Point intPoint(static_cast<int>(std::lround(point->x)), static_cast<int>(std::lround(point->y)));
+    if(intPoint.x < 0 || intPoint.x >= frame.cols || intPoint.y < 0 || intPoint.y >= frame.rows) {
+        return;
+    }
+
+    cv::drawMarker(frame, intPoint, color, cv::MARKER_CROSS, 16, 2);
+    cv::circle(frame, intPoint, 6, color, 1);
+}
+
+std::string formatPointStatus(const std::string& prefix, const dai::Point2f& point) {
+    std::ostringstream stream;
+    stream << std::fixed << std::setprecision(1) << prefix << ": (" << point.x << ", " << point.y << ")";
+    return stream.str();
+}
+
+int main() {
+    signal(SIGTERM, signalHandler);
+    signal(SIGINT, signalHandler);
+
+    dai::Pipeline pipeline;
+
+    auto camera = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_A);
+    auto* distorted = camera->requestOutput(DISTORTED_SIZE, std::nullopt, dai::ImgResizeMode::CROP, std::nullopt, false);
+    auto* undistorted = camera->requestOutput(UNDISTORTED_SIZE, std::nullopt, dai::ImgResizeMode::CROP, std::nullopt, true);
+
+    auto distQueue = distorted->createOutputQueue();
+    auto undistQueue = undistorted->createOutputQueue();
+
+    cv::namedWindow(DISTORTED_WINDOW);
+    cv::namedWindow(UNDISTORTED_WINDOW);
+    cv::setMouseCallback(DISTORTED_WINDOW, onLeftClick);
+
+    std::cout << "Left click in the distorted CAM_A window to remap a point to the undistorted output." << std::endl;
+    std::cout << "Press 'c' to clear the point and 'q' to quit." << std::endl;
+
+    pipeline.start();
+    while(pipeline.isRunning() && !quitEvent) {
+        auto distortedFrame = distQueue->get<dai::ImgFrame>();
+        auto undistortedFrame = undistQueue->get<dai::ImgFrame>();
+
+        if(distortedFrame == nullptr || undistortedFrame == nullptr) {
+            continue;
+        }
+
+        if(!distortedFrame->validateTransformations() || !undistortedFrame->validateTransformations()) {
+            std::cerr << "Invalid transformations!" << std::endl;
+            continue;
+        }
+
+        auto& distortedTransform = distortedFrame->getTransformation();
+        auto& undistortedTransform = undistortedFrame->getTransformation();
+
+        cv::Mat distortedView = toColorFrame(distortedFrame->getCvFrame());
+        cv::Mat undistortedView = toColorFrame(undistortedFrame->getCvFrame());
+
+        std::optional<dai::Point2f> sourcePoint = std::nullopt;
+        std::optional<dai::Point2f> remappedPoint = std::nullopt;
+        std::string sourceStatus = "Click in this window to select a point";
+        std::string targetStatus = "Waiting for a selected point";
+
+        if(selectedPoint.has_value()) {
+            sourcePoint = dai::Point2f(static_cast<float>(selectedPoint->x), static_cast<float>(selectedPoint->y));
+            remappedPoint = distortedTransform.remapPointTo(undistortedTransform, *sourcePoint);
+            sourceStatus = formatPointStatus("Source", *sourcePoint);
+            targetStatus = formatPointStatus("Remapped", *remappedPoint);
+            if(!isInsideFrame(*remappedPoint, undistortedView)) {
+                targetStatus += " outside target frame";
+            }
+        }
+
+        drawPoint(distortedView, sourcePoint, cv::Scalar(0, 255, 0));
+        drawPoint(undistortedView, remappedPoint, cv::Scalar(0, 255, 255));
+
+        addStatusLines(distortedView, {"Distorted CAM_A output", sourceStatus, "Press c to clear, q to quit"});
+        addStatusLines(undistortedView,
+                       {"Undistorted CAM_A output",
+                        targetStatus,
+                        "Target size: " + std::to_string(UNDISTORTED_SIZE.first) + "x" + std::to_string(UNDISTORTED_SIZE.second)});
+
+        cv::imshow(DISTORTED_WINDOW, distortedView);
+        cv::imshow(UNDISTORTED_WINDOW, undistortedView);
+
+        const int key = cv::waitKey(1);
+        if(key == 'q') {
+            pipeline.stop();
+            break;
+        }
+        if(key == 'c') {
+            selectedPoint = std::nullopt;
+        }
+    }
+
+    pipeline.stop();
+    pipeline.wait();
+    return 0;
+}

examples/python/Remapping/single_cam_remapping.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+
+import cv2
+import depthai as dai
+
+DISTORTED_WINDOW = "CAM_A distorted 640x480"
+UNDISTORTED_WINDOW = "CAM_A undistorted 1000x400"
+DISTORTED_SIZE = (640, 480)
+UNDISTORTED_SIZE = (1000, 400)
+
+selectedPoint = None
+
+
+def onLeftClick(event, x, y, flags, param):
+    del flags, param
+    global selectedPoint
+    if event == cv2.EVENT_LBUTTONDOWN:
+        selectedPoint = (x, y)
+
+
+def toColorFrame(frame):
+    if len(frame.shape) == 3 and frame.shape[2] == 3:
+        return frame
+    return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
+
+
+def addStatusLines(frame, lines, color=(255, 0, 255)):
+    for index, line in enumerate(lines):
+        cv2.putText(
+            frame,
+            line,
+            (10, 28 + index * 24),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.65,
+            color,
+            2,
+            cv2.LINE_AA,
+        )
+
+
+def isInsideFrame(point, frame):
+    height, width = frame.shape[:2]
+    return 0 <= point.x < width and 0 <= point.y < height
+
+
+def drawPoint(frame, point, color):
+    if point is None or not isInsideFrame(point, frame):
+        return
+    intPoint = (int(round(point.x)), int(round(point.y)))
+    cv2.drawMarker(frame, intPoint, color, markerType=cv2.MARKER_CROSS, markerSize=16, thickness=2)
+    cv2.circle(frame, intPoint, 6, color, 1)
+
+
+if __name__ == "__main__":
+    with dai.Pipeline() as pipeline:
+        camera = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A)
+        distorted = camera.requestOutput(DISTORTED_SIZE, resizeMode=dai.ImgResizeMode.CROP, enableUndistortion=False)
+        undistorted = camera.requestOutput(UNDISTORTED_SIZE, resizeMode=dai.ImgResizeMode.CROP, enableUndistortion=True)
+
+        distQueue = distorted.createOutputQueue()
+        undistQueue = undistorted.createOutputQueue()
+
+        cv2.namedWindow(DISTORTED_WINDOW)
+        cv2.namedWindow(UNDISTORTED_WINDOW)
+        cv2.setMouseCallback(DISTORTED_WINDOW, onLeftClick)
+
+        print("Left click in the distorted CAM_A window to remap a point to the undistorted output.")
+        print("Press 'c' to clear the point and 'q' to quit.")
+
+        pipeline.start()
+        while pipeline.isRunning():
+            distortedFrame = distQueue.get()
+            undistortedFrame = undistQueue.get()
+            assert isinstance(distortedFrame, dai.ImgFrame)
+            assert isinstance(undistortedFrame, dai.ImgFrame)
+            assert distortedFrame.validateTransformations()
+            assert undistortedFrame.validateTransformations()
+
+            distortedTransform = distortedFrame.getTransformation()
+            undistortedTransform = undistortedFrame.getTransformation()
+
+            distortedView = toColorFrame(distortedFrame.getCvFrame())
+            undistortedView = toColorFrame(undistortedFrame.getCvFrame())
+
+            sourcePoint = None
+            remappedPoint = None
+            sourceStatus = "Click in this window to select a point"
+            targetStatus = "Waiting for a selected point"
+
+            if selectedPoint is not None:
+                sourcePoint = dai.Point2f(float(selectedPoint[0]), float(selectedPoint[1]))
+                remappedPoint = distortedTransform.remapPointTo(undistortedTransform, sourcePoint)
+                sourceStatus = f"Source: ({sourcePoint.x:.1f}, {sourcePoint.y:.1f})"
+                targetStatus = f"Remapped: ({remappedPoint.x:.1f}, {remappedPoint.y:.1f})"
+                if not isInsideFrame(remappedPoint, undistortedView):
+                    targetStatus += " outside target frame"
+
+
+            drawPoint(distortedView, sourcePoint, (0, 255, 0))
+            drawPoint(undistortedView, remappedPoint, (0, 255, 255))
+
+            addStatusLines(
+                distortedView,
+                [
+                    "Distorted CAM_A output",
+                    sourceStatus,
+                    "Press c to clear, q to quit",
+                ],
+            )
+            addStatusLines(
+                undistortedView,
+                [
+                    "Undistorted CAM_A output",
+                    targetStatus,
+                    f"Target size: {UNDISTORTED_SIZE[0]}x{UNDISTORTED_SIZE[1]}",
+                ],
+            )
+
+            cv2.imshow(DISTORTED_WINDOW, distortedView)
+            cv2.imshow(UNDISTORTED_WINDOW, undistortedView)
+
+            key = cv2.waitKey(1)
+            if key == ord("q"):
+                break
+            if key == ord("c"):
+                selectedPoint = None