diff --git a/bindings/python/src/pipeline/CommonBindings.cpp b/bindings/python/src/pipeline/CommonBindings.cpp index c680e82bb1..ca7e412ed3 100644 --- a/bindings/python/src/pipeline/CommonBindings.cpp +++ b/bindings/python/src/pipeline/CommonBindings.cpp @@ -543,6 +543,7 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { .def_readwrite("translation", &Extrinsics::translation) .def_readwrite("specTranslation", &Extrinsics::specTranslation) .def_readwrite("toCameraSocket", &Extrinsics::toCameraSocket) + .def_readwrite("toDeviceId", &Extrinsics::toDeviceId) .def_readwrite("lengthUnit", &Extrinsics::lengthUnit) .def("getRotationMatrix", &Extrinsics::getRotationMatrix, DOC(dai, Extrinsics, getRotationMatrix)) .def("getInverseRotationMatrix", &Extrinsics::getInverseRotationMatrix, DOC(dai, Extrinsics, getInverseRotationMatrix)) @@ -573,6 +574,10 @@ void CommonBindings::bind(pybind11::module& m, void* pCallstack) { py::arg("unit") = LengthUnit::CENTIMETER, DOC(dai, Extrinsics, getTranslationVector)) .def("isEqualExtrinsics", &Extrinsics::isEqualExtrinsics, py::arg("other"), py::arg("epsilon") = 1e-6f, DOC(dai, Extrinsics, isEqualExtrinsics)) + .def("hasCompatibleCoordinateSystem", + &Extrinsics::hasCompatibleCoordinateSystem, + py::arg("other"), + DOC(dai, Extrinsics, hasCompatibleCoordinateSystem)) .def("getExtrinsicsTransformationTo", &Extrinsics::getExtrinsicsTransformationTo, py::arg("to"), diff --git a/include/depthai/common/Extrinsics.hpp b/include/depthai/common/Extrinsics.hpp index a5d975c6d5..707e530830 100644 --- a/include/depthai/common/Extrinsics.hpp +++ b/include/depthai/common/Extrinsics.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -45,6 +46,12 @@ struct Extrinsics { */ CameraBoardSocket toCameraSocket = CameraBoardSocket::AUTO; + /** + * The device containing the destination camera socket for which these extrinsics are defined. + * An empty value means that the device is unknown. + */ + std::string toDeviceId; + /** * The distance unit for the translation vector. */ @@ -140,6 +147,14 @@ struct Extrinsics { */ bool isEqualExtrinsics(const Extrinsics& other, float epsilon = 1e-6f) const; + /** + * Check whether these extrinsics can be expressed relative to the same target coordinate system as another Extrinsics object. + * Unknown device IDs and AUTO camera sockets are treated as compatible for backwards compatibility. + * @param other The other Extrinsics object to compare with + * @return true if no known part of the target coordinate system differs, false otherwise + */ + bool hasCompatibleCoordinateSystem(const Extrinsics& other) const; + /** * Get the extrinsic transformation matrix from this Extrinsics to the target Extrinsics. * @param to The target Extrinsics to get the transformation matrix to @@ -151,7 +166,7 @@ struct Extrinsics { bool useSpecTranslation = false, LengthUnit sourceUnit = LengthUnit::CENTIMETER) const; - DEPTHAI_SERIALIZE_OPTIONAL(Extrinsics, rotationMatrix, translation, specTranslation, toCameraSocket, lengthUnit); + DEPTHAI_SERIALIZE_OPTIONAL(Extrinsics, rotationMatrix, translation, specTranslation, toCameraSocket, lengthUnit, toDeviceId); }; -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/include/depthai/common/ImgTransformations.hpp b/include/depthai/common/ImgTransformations.hpp index cfafba4caf..91ff7cc7a7 100644 --- a/include/depthai/common/ImgTransformations.hpp +++ b/include/depthai/common/ImgTransformations.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "depthai/common/CameraModel.hpp" #include "depthai/common/Extrinsics.hpp" @@ -145,7 +146,8 @@ struct ImgTransformation { */ std::vector getDistortionCoefficients() const; /** - * Retrieve the extrinsics to the source sensor. + * Retrieve the pose of the source sensor or virtual camera relative to its target coordinate system. + * The target coordinate system is identified by Extrinsics::toDeviceId and Extrinsics::toCameraSocket. * @return Extrinsics */ Extrinsics getExtrinsics() const; @@ -357,7 +359,7 @@ struct ImgTransformation { * calibration. * @param sourceUnit The desired measurement unit in which to return the transformation matrix in. * @return 4x4 homogeneous transformation matrix representing the extrinsics from this transformation to the target transformation - * @note Both transformations must have a common toCameraSocket. Otherwise extrinsics cannot be calculated. + * @note Both transformations must have a compatible target device ID and a common target camera socket. Otherwise extrinsics cannot be calculated. */ std::array, 4> getExtrinsicsTransformationMatrixTo(const ImgTransformation& to, bool useSpecTranslation = false, diff --git a/protos/common.proto b/protos/common.proto index 9bbda54ec8..608b650f8e 100644 --- a/protos/common.proto +++ b/protos/common.proto @@ -33,6 +33,7 @@ message Extrinsics { Point3f specTranslation = 3; CameraBoardSocket toCameraSocket = 4; optional LengthUnit lengthUnit = 5; + string toDeviceId = 6; } message ImgTransformation { diff --git a/src/pipeline/datatype/Extrinsics.cpp b/src/pipeline/datatype/Extrinsics.cpp index d61482185b..4c442316ec 100644 --- a/src/pipeline/datatype/Extrinsics.cpp +++ b/src/pipeline/datatype/Extrinsics.cpp @@ -109,6 +109,9 @@ bool Extrinsics::isEqualExtrinsics(const Extrinsics& other, float epsilon) const if(this->toCameraSocket != other.toCameraSocket) { return false; } + if(this->toDeviceId != other.toDeviceId) { + return false; + } const auto thisTranslation = getTranslationVector(false, LengthUnit::CENTIMETER); const auto otherTranslation = other.getTranslationVector(false, LengthUnit::CENTIMETER); @@ -121,9 +124,19 @@ bool Extrinsics::isEqualExtrinsics(const Extrinsics& other, float epsilon) const return true; } +bool Extrinsics::hasCompatibleCoordinateSystem(const Extrinsics& other) const { + const bool differentKnownDevices = !toDeviceId.empty() && !other.toDeviceId.empty() && toDeviceId != other.toDeviceId; + const bool differentKnownSockets = + toCameraSocket != CameraBoardSocket::AUTO && other.toCameraSocket != CameraBoardSocket::AUTO && toCameraSocket != other.toCameraSocket; + return !differentKnownDevices && !differentKnownSockets; +} + std::array, 4> Extrinsics::getExtrinsicsTransformationTo(const Extrinsics& to, const bool useSpecTranslation, const LengthUnit sourceUnit) const { + if(!hasCompatibleCoordinateSystem(to)) { + throw std::runtime_error("Cannot get extrinsics between different target coordinate systems."); + } if(this->toCameraSocket == dai::CameraBoardSocket::AUTO || to.toCameraSocket == dai::CameraBoardSocket::AUTO) { throw std::runtime_error( "Cannot get extrinsics transformation to or from an extrinsics with AUTO camera socket. Please specify the camera socket for both extrinsics."); @@ -160,4 +173,4 @@ bool Extrinsics::validRotationMatrix() const { return rotationMatrix.size() == 3 && rotationMatrix[0].size() == 3 && rotationMatrix[1].size() == 3 && rotationMatrix[2].size() == 3; }; -} // namespace dai \ No newline at end of file +} // namespace dai diff --git a/src/pipeline/datatype/ImgTransformations.cpp b/src/pipeline/datatype/ImgTransformations.cpp index 1eed237c01..8b34b0e3a9 100644 --- a/src/pipeline/datatype/ImgTransformations.cpp +++ b/src/pipeline/datatype/ImgTransformations.cpp @@ -50,6 +50,9 @@ inline bool RRinRR(const dai::RotatedRect& in, const dai::RotatedRect& out) { } dai::Point2f interSourceFrameTransform(dai::Point2f sourcePt, const ImgTransformation& from, const ImgTransformation& to) { + if(!from.getExtrinsics().hasCompatibleCoordinateSystem(to.getExtrinsics())) { + throw std::runtime_error("Cannot remap ImgTransformations expressed relative to different target coordinate systems."); + } if(from.isEqualTransformation(to)) { return sourcePt; } @@ -119,7 +122,6 @@ bool ImgTransformation::isEqualTransformation(const ImgTransformation& other) co auto thisExtrinsics = getExtrinsics(); auto otherExtrinsics = other.getExtrinsics(); if(!thisExtrinsics.isEqualExtrinsics(otherExtrinsics)) return false; - if(getSize() != other.getSize()) return false; if(getSourceSize() != other.getSourceSize()) return false; return true; @@ -532,6 +534,7 @@ std::array, 4> ImgTransformation::getExtrinsicsTransformati } bool ImgTransformation::isAlignedTo(const ImgTransformation& to) const { + if(!extrinsics.hasCompatibleCoordinateSystem(to.extrinsics)) return false; if(width != to.width || height != to.height) return false; if(this->distortionModel != to.distortionModel) return false; auto approxEqual = [](float a, float b, float absTol = ROUND_UP_EPS, float relTol = 2 * ROUND_UP_EPS) { diff --git a/src/utility/ProtoSerialize.cpp b/src/utility/ProtoSerialize.cpp index c8eb26420b..13cbda3a41 100644 --- a/src/utility/ProtoSerialize.cpp +++ b/src/utility/ProtoSerialize.cpp @@ -113,6 +113,7 @@ void serializeImgTransformation(proto::common::ImgTransformation* imgTransformat protoExtrinsics->mutable_spectranslation()->set_z(extrinsics.specTranslation.z); protoExtrinsics->set_tocamerasocket(static_cast(extrinsics.toCameraSocket)); protoExtrinsics->set_lengthunit(static_cast(extrinsics.lengthUnit)); + protoExtrinsics->set_todeviceid(extrinsics.toDeviceId); for(const auto& crop : transformation.getSrcCrops()) { auto* protoCrop = imgTransformation->add_srccrops(); @@ -160,6 +161,7 @@ ImgTransformation deserializeImgTransformation(const proto::common::ImgTransform extrinsics.specTranslation = Point3f(t.x(), t.y(), t.z()); } extrinsics.toCameraSocket = static_cast(protoExtrinsics.tocamerasocket()); + extrinsics.toDeviceId = protoExtrinsics.todeviceid(); if(protoExtrinsics.has_lengthunit()) { extrinsics.lengthUnit = static_cast(protoExtrinsics.lengthunit()); } else { diff --git a/tests/src/ondevice_tests/img_transformation_test.cpp b/tests/src/ondevice_tests/img_transformation_test.cpp index e00aef4416..0a1eac1666 100644 --- a/tests/src/ondevice_tests/img_transformation_test.cpp +++ b/tests/src/ondevice_tests/img_transformation_test.cpp @@ -399,6 +399,7 @@ const std::filesystem::path& getTransformationTestDataFolder() { // ----------------------------------------------------------------------------- TEST_CASE("ImgTransformation in ImgFrame") { dai::Pipeline pipeline; + const auto expectedDeviceId = pipeline.getDefaultDevice()->getDeviceId(); auto cam = pipeline.create()->build(); auto camOut = cam->requestOutput({600, 400}, dai::ImgFrame::Type::NV12); auto q = camOut->createOutputQueue(); @@ -411,6 +412,8 @@ TEST_CASE("ImgTransformation in ImgFrame") { REQUIRE(!isIdentity(frame->transformation.getMatrixInv())); REQUIRE(!isIdentity(frame->transformation.getSourceIntrinsicMatrix())); REQUIRE(!isIdentity(frame->transformation.getSourceIntrinsicMatrixInv())); + REQUIRE_FALSE(expectedDeviceId.empty()); + REQUIRE(frame->transformation.getExtrinsics().toDeviceId == expectedDeviceId); } // ----------------------------------------------------------------------------- diff --git a/tests/src/onhost_tests/image_transformations_test.cpp b/tests/src/onhost_tests/image_transformations_test.cpp index 4bd54c6116..65a4854421 100644 --- a/tests/src/onhost_tests/image_transformations_test.cpp +++ b/tests/src/onhost_tests/image_transformations_test.cpp @@ -4,6 +4,7 @@ #include "depthai/common/Extrinsics.hpp" #include "depthai/common/ImgTransformations.hpp" #include "depthai/utility/ImageManipImpl.hpp" +#include "depthai/utility/Serialization.hpp" #define CATCH_CONFIG_MAIN #include @@ -231,6 +232,61 @@ TEST_CASE("identityTransformation") { REQUIRE_THAT(back.height, Catch::Matchers::WithinAbs(r.height, 1e-6)); } +TEST_CASE("ImgTransformation target coordinate system metadata") { + auto makeTransformation = [](const std::string& toDeviceId, dai::CameraBoardSocket toCameraSocket) { + dai::ImgTransformation transformation(640, 480); + auto extrinsics = transformation.getExtrinsics(); + extrinsics.toDeviceId = toDeviceId; + extrinsics.toCameraSocket = toCameraSocket; + transformation.setExtrinsics(extrinsics); + return transformation; + }; + + auto source = makeTransformation("mxid-a", dai::CameraBoardSocket::CAM_A); + REQUIRE(source.getExtrinsics().toDeviceId == "mxid-a"); + + auto sameTarget = makeTransformation("mxid-a", dai::CameraBoardSocket::CAM_A); + REQUIRE(source.getExtrinsics().hasCompatibleCoordinateSystem(sameTarget.getExtrinsics())); + REQUIRE(source.isEqualTransformation(sameTarget)); + REQUIRE(source.isAlignedTo(sameTarget)); + + auto otherDevice = makeTransformation("mxid-b", dai::CameraBoardSocket::CAM_A); + REQUIRE_FALSE(source.getExtrinsics().hasCompatibleCoordinateSystem(otherDevice.getExtrinsics())); + REQUIRE_FALSE(source.isEqualTransformation(otherDevice)); + REQUIRE_FALSE(source.isAlignedTo(otherDevice)); + REQUIRE_THROWS(source.getExtrinsicsTransformationMatrixTo(otherDevice)); + + auto otherSocket = makeTransformation("mxid-a", dai::CameraBoardSocket::CAM_B); + REQUIRE_FALSE(source.getExtrinsics().hasCompatibleCoordinateSystem(otherSocket.getExtrinsics())); + REQUIRE_FALSE(source.isAlignedTo(otherSocket)); + REQUIRE_THROWS(source.getExtrinsicsTransformationMatrixTo(otherSocket)); + + auto unknownDevice = makeTransformation("", dai::CameraBoardSocket::CAM_A); + REQUIRE(source.getExtrinsics().hasCompatibleCoordinateSystem(unknownDevice.getExtrinsics())); + + auto unknownSocket = makeTransformation("mxid-a", dai::CameraBoardSocket::AUTO); + REQUIRE(source.getExtrinsics().hasCompatibleCoordinateSystem(unknownSocket.getExtrinsics())); + + dai::Point2f point{10.0f, 20.0f}; + REQUIRE_THROWS(source.remapPointTo(otherDevice, point)); + REQUIRE_THROWS(source.remapPointTo(otherSocket, point)); + + dai::ImgTransformation replayTransformation(640, 480); + REQUIRE(source.getExtrinsics().hasCompatibleCoordinateSystem(replayTransformation.getExtrinsics())); + REQUIRE_FALSE(source.isEqualTransformation(replayTransformation)); + REQUIRE(source.isAlignedTo(replayTransformation)); + + const auto remappedReplayPoint = replayTransformation.remapPointTo(source, point); + REQUIRE_THAT(remappedReplayPoint.x, Catch::Matchers::WithinAbs(point.x, 1e-6)); + REQUIRE_THAT(remappedReplayPoint.y, Catch::Matchers::WithinAbs(point.y, 1e-6)); + + const auto serialized = dai::utility::serialize(source); + dai::ImgTransformation deserialized; + dai::utility::deserialize(serialized, deserialized); + REQUIRE(deserialized.getExtrinsics().toDeviceId == "mxid-a"); + REQUIRE(deserialized.isEqualTransformation(source)); +} + // ----------------------------------------------------------------------------- // invalidTransformations // Purpose: