From 3a4e66b9875f78b5dc6e9e8bc216c53fb5e50d98 Mon Sep 17 00:00:00 2001 From: John Ingve Olsen Date: Wed, 12 Jun 2024 12:15:26 +0200 Subject: [PATCH] Add marker detection and hand-eye API wrappers This commit adds API wrappers for the SDK 2.13 fiducial marker detection and hand-eye calibration. Added to the `zivid.calibration` module are the classes `MarkerShape`, `MarkerDictionary`, and `DetectionResultFiducialMarkers`, as well as the function `detect_markers`. The `DetectionResultFiducialMarkers` class can be used to construct a `HandEyeInput`, just like `DetectionResult`. The hand-eye test data was updated with some newer ZDFs that were downsampled to save on Git LFS bandwidth. Co-authored-by: Eskil Aursand --- modules/zivid/_calibration/detector.py | 243 +++++++++++++++++- modules/zivid/_calibration/hand_eye.py | 27 +- modules/zivid/calibration.py | 5 + src/Calibration/Calibration.cpp | 9 + src/Calibration/Detector.cpp | 58 +++++ src/Calibration/HandEye.cpp | 1 + .../ZividPython/Calibration/Detector.h | 3 + src/include/ZividPython/Matrix.h | 6 + test/calibration/test_hand_eye.py | 78 +++--- test/calibration/test_marker_detection.py | 126 +++++++++ test/conftest.py | 40 ++- test/test_data/handeye/eth/eth_transform.csv | 8 +- .../handeye/eth/eth_transform_marker.csv | 4 + test/test_data/handeye/eth/img01.zdf | 4 +- test/test_data/handeye/eth/img02.zdf | 4 +- test/test_data/handeye/eth/img03.zdf | 4 +- test/test_data/handeye/eth/img04.zdf | 4 +- test/test_data/handeye/eth/img05.zdf | 4 +- test/test_data/handeye/eth/img06.zdf | 3 + test/test_data/handeye/eth/pos01.csv | 4 - test/test_data/handeye/eth/pos02.csv | 4 - test/test_data/handeye/eth/pos03.csv | 4 - test/test_data/handeye/eth/pos04.csv | 4 - test/test_data/handeye/eth/pos05.csv | 4 - test/test_data/handeye/eth/pose01.csv | 4 + test/test_data/handeye/eth/pose02.csv | 4 + test/test_data/handeye/eth/pose03.csv | 4 + test/test_data/handeye/eth/pose04.csv | 4 + test/test_data/handeye/eth/pose05.csv | 4 + test/test_data/handeye/eth/pose06.csv | 4 + .../expected_2d_corners_1.csv | 4 + .../expected_2d_corners_2.csv | 4 + .../expected_2d_corners_3.csv | 4 + .../expected_2d_corners_4.csv | 4 + .../expected_3d_corners_1.csv | 4 + .../expected_3d_corners_2.csv | 4 + .../expected_3d_corners_3.csv | 4 + .../expected_3d_corners_4.csv | 4 + .../marker_detection/expected_poses_1.csv | 4 + .../marker_detection/expected_poses_2.csv | 4 + .../marker_detection/expected_poses_3.csv | 4 + .../marker_detection/expected_poses_4.csv | 4 + 42 files changed, 645 insertions(+), 78 deletions(-) create mode 100644 test/calibration/test_marker_detection.py create mode 100644 test/test_data/handeye/eth/eth_transform_marker.csv create mode 100644 test/test_data/handeye/eth/img06.zdf delete mode 100644 test/test_data/handeye/eth/pos01.csv delete mode 100644 test/test_data/handeye/eth/pos02.csv delete mode 100644 test/test_data/handeye/eth/pos03.csv delete mode 100644 test/test_data/handeye/eth/pos04.csv delete mode 100644 test/test_data/handeye/eth/pos05.csv create mode 100644 test/test_data/handeye/eth/pose01.csv create mode 100644 test/test_data/handeye/eth/pose02.csv create mode 100644 test/test_data/handeye/eth/pose03.csv create mode 100644 test/test_data/handeye/eth/pose04.csv create mode 100644 test/test_data/handeye/eth/pose05.csv create mode 100644 test/test_data/handeye/eth/pose06.csv create mode 100644 test/test_data/marker_detection/expected_2d_corners_1.csv create mode 100644 test/test_data/marker_detection/expected_2d_corners_2.csv create mode 100644 test/test_data/marker_detection/expected_2d_corners_3.csv create mode 100644 test/test_data/marker_detection/expected_2d_corners_4.csv create mode 100644 test/test_data/marker_detection/expected_3d_corners_1.csv create mode 100644 test/test_data/marker_detection/expected_3d_corners_2.csv create mode 100644 test/test_data/marker_detection/expected_3d_corners_3.csv create mode 100644 test/test_data/marker_detection/expected_3d_corners_4.csv create mode 100644 test/test_data/marker_detection/expected_poses_1.csv create mode 100644 test/test_data/marker_detection/expected_poses_2.csv create mode 100644 test/test_data/marker_detection/expected_poses_3.csv create mode 100644 test/test_data/marker_detection/expected_poses_4.csv diff --git a/modules/zivid/_calibration/detector.py b/modules/zivid/_calibration/detector.py index 45be11f0..f972ca0d 100644 --- a/modules/zivid/_calibration/detector.py +++ b/modules/zivid/_calibration/detector.py @@ -3,6 +3,7 @@ This module should not be imported directly by end-user, but rather accessed through the zivid.calibration module. """ + import _zivid from zivid.camera import Camera from zivid.frame import Frame @@ -10,7 +11,7 @@ class DetectionResult: - """Class representing detected feature points.""" + """Class representing detected feature points from a calibration board.""" def __init__(self, impl): """Initialize DetectionResult wrapper. @@ -68,6 +69,203 @@ def __str__(self): return str(self.__impl) +class MarkerShape: + """Holds physical (3D) and image (2D) properties of a detected fiducial marker.""" + + def __init__(self, impl): + """Initialize MarkerShape wrapper. + + This constructor is only used internally, and should not be called by the end-user. + + Args: + impl: Reference to internal/back-end instance. + + Raises: + TypeError: If argument does not match the expected internal class. + """ + if not isinstance(impl, _zivid.calibration.MarkerShape): + raise TypeError( + "Unsupported type for argument impl. Got {}, expected {}".format( + type(impl), _zivid.calibration.MarkerShape + ) + ) + + self.__impl = impl + + @property + def corners_in_pixel_coordinates(self): + """Get 2D image coordinates of the corners of the detected marker. + + Returns: + List of four numpy ndarrays + """ + return self.__impl.corners_in_pixel_coordinates() + + @property + def corners_in_camera_coordinates(self): + """Get 3D spatial coordinates of the corners of the detected marker. + + Returns: + List of four numpy ndarrays + """ + return self.__impl.corners_in_camera_coordinates() + + @property + def identifier(self): + """Get the id of the detected marker. + + Returns: + Id as int + """ + return self.__impl.id_() + + @property + def pose(self): + """Get 3D pose of the marker. + + Returns: + The Pose of the marker center (4x4 transformation matrix) + """ + return Pose(self.__impl.pose().to_matrix()) + + +class MarkerDictionary: + """Holds information about fiducial markers such as ArUco or AprilTag for detection. + + This class's properties describe the different dictionaries available, for example + aruco4x4_50 describes the ArUco dictionary with 50 markers of size 4x4. + + For more information on ArUco markers see the OpenCV documentation on ArUco markers: + https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html, + + To get more information about fiducial markers in general, refer to the wikipedia page: + https://en.wikipedia.org/wiki/Fiducial_marker + """ + + aruco4x4_50 = "aruco4x4_50" + aruco4x4_100 = "aruco4x4_100" + aruco4x4_250 = "aruco4x4_250" + aruco4x4_1000 = "aruco4x4_1000" + aruco5x5_50 = "aruco5x5_50" + aruco5x5_100 = "aruco5x5_100" + aruco5x5_250 = "aruco5x5_250" + aruco5x5_1000 = "aruco5x5_1000" + aruco6x6_50 = "aruco6x6_50" + aruco6x6_100 = "aruco6x6_100" + aruco6x6_250 = "aruco6x6_250" + aruco6x6_1000 = "aruco6x6_1000" + aruco7x7_50 = "aruco7x7_50" + aruco7x7_100 = "aruco7x7_100" + aruco7x7_250 = "aruco7x7_250" + aruco7x7_1000 = "aruco7x7_1000" + + _valid_values = { + "aruco4x4_50": _zivid.calibration.MarkerDictionary.aruco4x4_50, + "aruco4x4_100": _zivid.calibration.MarkerDictionary.aruco4x4_100, + "aruco4x4_250": _zivid.calibration.MarkerDictionary.aruco4x4_250, + "aruco4x4_1000": _zivid.calibration.MarkerDictionary.aruco4x4_1000, + "aruco5x5_50": _zivid.calibration.MarkerDictionary.aruco5x5_50, + "aruco5x5_100": _zivid.calibration.MarkerDictionary.aruco5x5_100, + "aruco5x5_250": _zivid.calibration.MarkerDictionary.aruco5x5_250, + "aruco5x5_1000": _zivid.calibration.MarkerDictionary.aruco5x5_1000, + "aruco6x6_50": _zivid.calibration.MarkerDictionary.aruco6x6_50, + "aruco6x6_100": _zivid.calibration.MarkerDictionary.aruco6x6_100, + "aruco6x6_250": _zivid.calibration.MarkerDictionary.aruco6x6_250, + "aruco6x6_1000": _zivid.calibration.MarkerDictionary.aruco6x6_1000, + "aruco7x7_50": _zivid.calibration.MarkerDictionary.aruco7x7_50, + "aruco7x7_100": _zivid.calibration.MarkerDictionary.aruco7x7_100, + "aruco7x7_250": _zivid.calibration.MarkerDictionary.aruco7x7_250, + "aruco7x7_1000": _zivid.calibration.MarkerDictionary.aruco7x7_1000, + } + + @classmethod + def valid_values(cls): + """Get valid values for MarkerDictionary. + + Returns: + A list of strings representing valid values for MarkerDictionary. + """ + return list(cls._valid_values.keys()) + + @classmethod + def marker_count(cls, dictionary_name): + """Get the number of markers in a dictionary. + + Args: + dictionary_name: Name of the dictionary, e.g. "aruco4x4_50". Must be one of the values returned by + valid_values(). + + Returns: + Number of markers in the dictionary. + + Raises: + ValueError: If the dictionary name is not one of the valid values returned by + valid_values(). + """ + if dictionary_name not in cls._valid_values: + raise ValueError( + "Invalid dictionary name '{}'. Valid values are {}".format( + dictionary_name, cls.valid_values() + ) + ) + + return cls._valid_values[dictionary_name].marker_count() + + +class DetectionResultFiducialMarkers: + """Class representing detected fiducial markers.""" + + def __init__(self, impl): + """Initialize DetectionResultFiducialMarkers wrapper. + + This constructor is only used internally, and should not be called by the end-user. + + Args: + impl: Reference to internal/back-end instance. + + Raises: + TypeError: If argument does not match the expected internal class. + """ + if not isinstance(impl, _zivid.calibration.DetectionResultFiducialMarkers): + raise TypeError( + "Unsupported type for argument impl. Got {}, expected {}".format( + type(impl), _zivid.calibration.DetectionResultFiducialMarkers + ) + ) + + self.__impl = impl + + def valid(self): + """Check validity of DetectionResult. + + Returns: + True if DetectionResult is valid + """ + return self.__impl.valid() + + def allowed_marker_ids(self): + """Get the allowed marker ids this detection result was made with. + + Returns: + A list of integers, equal to what was passed to the detection function. + """ + return self.__impl.allowed_marker_ids() + + def detected_markers(self): + """Get all detected markers. + + Returns: + A list of MarkerShape instances + """ + return [MarkerShape(impl) for impl in self.__impl.detected_markers()] + + def __bool__(self): + return bool(self.__impl) + + def __str__(self): + return str(self.__impl) + + def detect_feature_points(point_cloud): """Detect feature points from a calibration object in a point cloud. @@ -153,3 +351,46 @@ def capture_calibration_board(camera): camera._Camera__impl # pylint: disable=protected-access ) ) + + +def detect_markers(frame, allowed_marker_ids, marker_dictionary): + """Detect fiducial markers such as ArUco markers in a frame. + + Only markers with integer IDs are supported. To get more information about fiducial markers, refer to the + wikipedia page: https://en.wikipedia.org/wiki/Fiducial_marker + + For more information on ArUco markers specifically, see the OpenCV documentation on ArUco markers: + https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html, + + Frame need not contain all markers listed in allowedMarkerIds for a successful detection. + + Args: + frame: A frame containing an image of one or several fiducial markers + allowed_marker_ids: List of the IDs of markers to be detected + marker_dictionary: The name of the marker dictionary to use. The name must be one of the values returned by + MarkerDictionary.valid_values() + + Raises: + ValueError: If marker_dictionary is not one of the valid values returned by MarkerDictionary.valid_values() + + Returns: + A DetectionResultFiducialMarkers instance + """ + + if marker_dictionary not in MarkerDictionary.valid_values(): + raise ValueError( + "Invalid marker dictionary '{}'. Valid values are {}".format( + marker_dictionary, MarkerDictionary.valid_values() + ) + ) + dictionary = MarkerDictionary._valid_values.get( # pylint: disable=protected-access + marker_dictionary + ) + + return DetectionResultFiducialMarkers( + _zivid.calibration.detect_markers( + frame._Frame__impl, # pylint: disable=protected-access + allowed_marker_ids, + dictionary, + ) + ) diff --git a/modules/zivid/_calibration/hand_eye.py b/modules/zivid/_calibration/hand_eye.py index 33ee2ddf..2162a344 100644 --- a/modules/zivid/_calibration/hand_eye.py +++ b/modules/zivid/_calibration/hand_eye.py @@ -6,7 +6,7 @@ import _zivid from zivid._calibration.pose import Pose -from zivid._calibration.detector import DetectionResult +from zivid._calibration.detector import DetectionResult, DetectionResultFiducialMarkers class HandEyeInput: @@ -17,7 +17,7 @@ def __init__(self, robot_pose, detection_result): Args: robot_pose: The robot Pose at the time of capture - detection_result: The DetectionResult captured when in the above pose + detection_result: The DetectionResult or DetectionResultFiducialMarkers captured when in the above pose Raises: TypeError: If one of the input arguments are of the wrong type @@ -28,16 +28,23 @@ def __init__(self, robot_pose, detection_result): type(robot_pose) ) ) - if not isinstance(detection_result, DetectionResult): + if isinstance(detection_result, DetectionResult): + self.__impl = _zivid.calibration.HandEyeInput( + robot_pose._Pose__impl, # pylint: disable=protected-access + detection_result._DetectionResult__impl, # pylint: disable=protected-access + ) + elif isinstance(detection_result, DetectionResultFiducialMarkers): + self.__impl = _zivid.calibration.HandEyeInput( + robot_pose._Pose__impl, # pylint: disable=protected-access + detection_result._DetectionResultFiducialMarkers__impl, # pylint: disable=protected-access + ) + else: raise TypeError( - "Unsupported type for argument detection_result. Expected zivid.calibration.DetectionResult but got {}".format( - type(detection_result) - ) + ( + "Unsupported type for argument detection_result." + "Expected zivid.calibration.DetectionResult or zivid.calibration.DetectionResultFiducialMarkers but got {}" + ).format(type(detection_result)) ) - self.__impl = _zivid.calibration.HandEyeInput( - robot_pose._Pose__impl, # pylint: disable=protected-access - detection_result._DetectionResult__impl, # pylint: disable=protected-access - ) def robot_pose(self): """Get the contained robot pose. diff --git a/modules/zivid/calibration.py b/modules/zivid/calibration.py index a48f86a7..9b683274 100644 --- a/modules/zivid/calibration.py +++ b/modules/zivid/calibration.py @@ -1,10 +1,15 @@ """Module for calibration features, such as HandEye and MultiCamera.""" + # pylint: disable=unused-import from zivid._calibration.detector import ( DetectionResult, + DetectionResultFiducialMarkers, + MarkerShape, + MarkerDictionary, detect_feature_points, detect_calibration_board, capture_calibration_board, + detect_markers, ) from zivid._calibration.hand_eye import ( HandEyeInput, diff --git a/src/Calibration/Calibration.cpp b/src/Calibration/Calibration.cpp index 21820996..18adb26e 100644 --- a/src/Calibration/Calibration.cpp +++ b/src/Calibration/Calibration.cpp @@ -30,6 +30,9 @@ namespace ZividPython::Calibration ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeOutput); ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeInput); ZIVID_PYTHON_WRAP_CLASS(dest, DetectionResult); + ZIVID_PYTHON_WRAP_CLASS(dest, MarkerShape); + ZIVID_PYTHON_WRAP_CLASS(dest, MarkerDictionary); + ZIVID_PYTHON_WRAP_CLASS(dest, DetectionResultFiducialMarkers); ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeResidual); ZIVID_PYTHON_WRAP_CLASS(dest, MultiCameraResidual); @@ -51,6 +54,12 @@ namespace ZividPython::Calibration [](ReleasableCamera &releasableCamera) { return ReleasableFrame{ Zivid::Calibration::captureCalibrationBoard(releasableCamera.impl()) }; }) + .def("detect_markers", + [](const ReleasableFrame &releasableFrame, + const std::vector &allowedMarkerIds, + const MarkerDictionary &markerDictionary) { + return detectMarkers(releasableFrame.impl(), allowedMarkerIds, markerDictionary); + }) .def("calibrate_eye_in_hand", &Zivid::Calibration::calibrateEyeInHand) .def("calibrate_eye_to_hand", &Zivid::Calibration::calibrateEyeToHand) .def("calibrate_multi_camera", &Zivid::Calibration::calibrateMultiCamera) diff --git a/src/Calibration/Detector.cpp b/src/Calibration/Detector.cpp index 2bf9d6f2..9de12a16 100644 --- a/src/Calibration/Detector.cpp +++ b/src/Calibration/Detector.cpp @@ -4,6 +4,7 @@ #include #include +#include namespace py = pybind11; @@ -18,4 +19,61 @@ namespace ZividPython }) .def("pose", &Zivid::Calibration::DetectionResult::pose); } + + void wrapClass(pybind11::class_ pyClass) + { + pyClass.def("id_", &Zivid::Calibration::MarkerShape::id) + .def("pose", &Zivid::Calibration::MarkerShape::pose) + .def("corners_in_pixel_coordinates", + [](const Zivid::Calibration::MarkerShape &markerShape) { + const auto nativeCorners = markerShape.cornersInPixelCoordinates(); + auto corners = std::array{}; + for(int i = 0; i < 4; i++) + { + corners[i] = Conversion::toPyVector(nativeCorners[i]); + } + return corners; + }) + .def("corners_in_camera_coordinates", [](const Zivid::Calibration::MarkerShape &markerShape) { + const auto nativeCorners = markerShape.cornersInCameraCoordinates(); + auto corners = std::array{}; + for(int i = 0; i < 4; i++) + { + corners[i] = Conversion::toPyVector(nativeCorners[i]); + } + return corners; + }); + } + +#define ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, name) \ + pyClass.def_property_readonly_static(#name, [](py::object) { return Zivid::Calibration::MarkerDictionary::name; }); + + void wrapClass(pybind11::class_ pyClass) + { + pyClass.def("marker_count", &Zivid::Calibration::MarkerDictionary::markerCount); + + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco4x4_50); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco4x4_100); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco4x4_250); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco4x4_1000); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco5x5_50); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco5x5_100); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco5x5_250); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco5x5_1000); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco6x6_50); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco6x6_100); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco6x6_250); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco6x6_1000); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco7x7_50); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco7x7_100); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco7x7_250); + ZIVID_PYTHON_WRAP_MARKER_DICTIONARY_NAME(pyClass, aruco7x7_1000); + } + + void wrapClass(pybind11::class_ pyClass) + { + pyClass.def("valid", &Zivid::Calibration::DetectionResultFiducialMarkers::valid) + .def("allowed_marker_ids", &Zivid::Calibration::DetectionResultFiducialMarkers::allowedMarkerIds) + .def("detected_markers", &Zivid::Calibration::DetectionResultFiducialMarkers::detectedMarkers); + } } // namespace ZividPython diff --git a/src/Calibration/HandEye.cpp b/src/Calibration/HandEye.cpp index f5e95fda..dd83f680 100644 --- a/src/Calibration/HandEye.cpp +++ b/src/Calibration/HandEye.cpp @@ -29,6 +29,7 @@ namespace ZividPython void wrapClass(pybind11::class_ pyClass) { pyClass.def(py::init()) + .def(py::init()) .def("robot_pose", [](const Zivid::Calibration::HandEyeInput &handEyeInput) { return Conversion::toPy(handEyeInput.robotPose().toMatrix()); diff --git a/src/include/ZividPython/Calibration/Detector.h b/src/include/ZividPython/Calibration/Detector.h index 11f4f497..5af3c8c7 100644 --- a/src/include/ZividPython/Calibration/Detector.h +++ b/src/include/ZividPython/Calibration/Detector.h @@ -7,4 +7,7 @@ namespace ZividPython { void wrapClass(pybind11::class_ pyClass); + void wrapClass(pybind11::class_ pyClass); + void wrapClass(pybind11::class_ pyClass); + void wrapClass(pybind11::class_ pyClass); } // namespace ZividPython diff --git a/src/include/ZividPython/Matrix.h b/src/include/ZividPython/Matrix.h index e3c83c73..aaa24fbf 100644 --- a/src/include/ZividPython/Matrix.h +++ b/src/include/ZividPython/Matrix.h @@ -22,4 +22,10 @@ namespace ZividPython::Conversion { return Eigen::Vector3f{ source.x, source.y, source.z }; } + + inline auto toPyVector(const Zivid::PointXY &source) + { + return Eigen::Vector2f{ source.x, source.y }; + } + } // namespace ZividPython::Conversion diff --git a/test/calibration/test_hand_eye.py b/test/calibration/test_hand_eye.py index b0c55384..98742191 100644 --- a/test/calibration/test_hand_eye.py +++ b/test/calibration/test_hand_eye.py @@ -1,7 +1,12 @@ -def test_handeye_input_init_failure(checkerboard_frames, transform): - import pytest - import zivid.calibration +import tempfile +from pathlib import Path + +import zivid +import numpy as np +import pytest + +def test_handeye_input_init_failure(checkerboard_frames, transform): frame = checkerboard_frames[0] detection_result = zivid.calibration.detect_feature_points(frame.point_cloud()) @@ -11,9 +16,6 @@ def test_handeye_input_init_failure(checkerboard_frames, transform): def test_handeye_input(checkerboard_frames, transform): - import numpy as np - import zivid.calibration - point_cloud = checkerboard_frames[0].point_cloud() detection_result = zivid.calibration.detect_feature_points(point_cloud) pose = zivid.calibration.Pose(transform) @@ -37,24 +39,7 @@ def test_handeye_input(checkerboard_frames, transform): assert detection_result_returned.valid() == detection_result.valid() -def test_eyetohand_calibration( - handeye_eth_frames, handeye_eth_poses, handeye_eth_transform -): - import numpy as np - import zivid.calibration - - # Assemble input - inputs = [] - for frame, pose_matrix in zip(handeye_eth_frames, handeye_eth_poses): - inputs.append( - zivid.calibration.HandEyeInput( - zivid.calibration.Pose(pose_matrix), - zivid.calibration.detect_feature_points(frame.point_cloud()), - ) - ) - - # Perform eye-to-hand calibration - handeye_output = zivid.calibration.calibrate_eye_to_hand(inputs) +def _check_handeye_output(inputs, handeye_output, expected_transform): assert isinstance(handeye_output, zivid.calibration.HandEyeOutput) assert handeye_output.valid() assert bool(handeye_output) @@ -64,7 +49,7 @@ def test_eyetohand_calibration( transform_returned = handeye_output.transform() assert isinstance(transform_returned, np.ndarray) assert transform_returned.shape == (4, 4) - np.testing.assert_allclose(transform_returned, handeye_eth_transform, rtol=1e-5) + np.testing.assert_allclose(transform_returned, expected_transform, rtol=1e-5) # Check returned residuals residuals_returned = handeye_output.residuals() @@ -79,12 +64,45 @@ def test_eyetohand_calibration( assert residual.rotation() >= 0.0 -def test_eyetohand_calibration_save_load(handeye_eth_frames, handeye_eth_poses): - from pathlib import Path - import tempfile - import zivid - import numpy as np +def test_eyetohand_calibration( + handeye_eth_frames, handeye_eth_poses, handeye_eth_transform +): + # Assemble input + inputs = [] + for frame, pose_matrix in zip(handeye_eth_frames, handeye_eth_poses): + inputs.append( + zivid.calibration.HandEyeInput( + zivid.calibration.Pose(pose_matrix), + zivid.calibration.detect_feature_points(frame.point_cloud()), + ) + ) + + # Perform eye-to-hand calibration + handeye_output = zivid.calibration.calibrate_eye_to_hand(inputs) + _check_handeye_output(inputs, handeye_output, handeye_eth_transform) + + +def test_marker_eyetohand_calibration( + handeye_eth_frames, handeye_eth_poses, handeye_marker_eth_transform +): + # Assemble input + inputs = [] + for frame, pose_matrix in zip(handeye_eth_frames, handeye_eth_poses): + inputs.append( + zivid.calibration.HandEyeInput( + zivid.calibration.Pose(pose_matrix), + zivid.calibration.detect_markers( + frame, [1, 2, 3, 4], zivid.calibration.MarkerDictionary.aruco4x4_50 + ), + ) + ) + + # Perform eye-to-hand calibration + handeye_output = zivid.calibration.calibrate_eye_to_hand(inputs) + _check_handeye_output(inputs, handeye_output, handeye_marker_eth_transform) + +def test_eyetohand_calibration_save_load(handeye_eth_frames, handeye_eth_poses): handeye_output = zivid.calibration.calibrate_eye_to_hand( [ zivid.calibration.HandEyeInput( diff --git a/test/calibration/test_marker_detection.py b/test/calibration/test_marker_detection.py new file mode 100644 index 00000000..05c7179c --- /dev/null +++ b/test/calibration/test_marker_detection.py @@ -0,0 +1,126 @@ +import zivid +import numpy as np + + +def _check_markers( + markers, + markers_expected_2d_corners, + markers_expected_3d_corners, + markers_expected_poses, +): + for marker in markers: + detected_2d_corners = marker.corners_in_pixel_coordinates + expected_2d_corners = markers_expected_2d_corners[marker.identifier] + np.testing.assert_allclose(detected_2d_corners, expected_2d_corners, rtol=1e-4) + + detected_3d_corners = marker.corners_in_camera_coordinates + expected_3d_corners = markers_expected_3d_corners[marker.identifier] + np.testing.assert_allclose(detected_3d_corners, expected_3d_corners, rtol=1e-4) + + detected_pose = marker.pose.to_matrix() + expected_pose = markers_expected_poses[marker.identifier] + np.testing.assert_allclose(detected_pose, expected_pose, rtol=1e-3) + + +def test_detect_all_markers( + calibration_board_and_aruco_markers_frame, + markers_2d_corners, + markers_3d_corners, + markers_poses, +): + allowed_marker_ids = [1, 2, 3, 4] + + detection_result = zivid.calibration.detect_markers( + calibration_board_and_aruco_markers_frame, + allowed_marker_ids, + zivid.calibration.MarkerDictionary.aruco4x4_50, + ) + assert detection_result + assert detection_result.valid() + assert detection_result.allowed_marker_ids() == allowed_marker_ids + assert [ + m.identifier for m in detection_result.detected_markers() + ] == allowed_marker_ids + _check_markers( + detection_result.detected_markers(), + markers_2d_corners, + markers_3d_corners, + markers_poses, + ) + + +def test_detect_filtered_markers( + calibration_board_and_aruco_markers_frame, + markers_2d_corners, + markers_3d_corners, + markers_poses, +): + allowed_marker_ids = [1, 3] + + detection_result = zivid.calibration.detect_markers( + calibration_board_and_aruco_markers_frame, + allowed_marker_ids, + zivid.calibration.MarkerDictionary.aruco4x4_50, + ) + assert detection_result + assert detection_result.valid() + assert detection_result.allowed_marker_ids() == allowed_marker_ids + assert [ + m.identifier for m in detection_result.detected_markers() + ] == allowed_marker_ids + _check_markers( + detection_result.detected_markers(), + markers_2d_corners, + markers_3d_corners, + markers_poses, + ) + + +def test_detect_with_some_markers_not_present( + calibration_board_and_aruco_markers_frame, + markers_2d_corners, + markers_3d_corners, + markers_poses, +): + allowed_marker_ids = [1, 2, 4, 5, 6, 7] + + detection_result = zivid.calibration.detect_markers( + calibration_board_and_aruco_markers_frame, + allowed_marker_ids, + zivid.calibration.MarkerDictionary.aruco4x4_50, + ) + assert detection_result + assert detection_result.valid() + assert detection_result.allowed_marker_ids() == allowed_marker_ids + assert [m.identifier for m in detection_result.detected_markers()] == [1, 2, 4] + _check_markers( + detection_result.detected_markers(), + markers_2d_corners, + markers_3d_corners, + markers_poses, + ) + + +def test_detect_specifying_different_dictionary( + calibration_board_and_aruco_markers_frame, +): + allowed_marker_ids = [1, 2, 3, 4, 7, 9, 42] + + detection_result = zivid.calibration.detect_markers( + calibration_board_and_aruco_markers_frame, + allowed_marker_ids, + zivid.calibration.MarkerDictionary.aruco6x6_250, + ) + assert not detection_result + assert not detection_result.valid() + assert detection_result.allowed_marker_ids() == allowed_marker_ids + assert len(detection_result.detected_markers()) == 0 + + +def test_marker_dictionary(): + from zivid.calibration import MarkerDictionary + + assert MarkerDictionary.marker_count(MarkerDictionary.aruco4x4_50) == 50 + assert MarkerDictionary.marker_count(MarkerDictionary.aruco6x6_250) == 250 + + assert MarkerDictionary.aruco5x5_1000 in MarkerDictionary.valid_values() diff --git a/test/conftest.py b/test/conftest.py index 33526d27..5d2c77d8 100644 --- a/test/conftest.py +++ b/test/conftest.py @@ -94,9 +94,7 @@ def calibration_board_frame_fixture(application): @pytest.fixture(name="calibration_board_and_aruco_markers_frame", scope="module") def calibration_board_and_aruco_markers_frame_fixture(application): - with zivid.Frame( - _testdata_dir() / "calibration_board_and_aruco_markers.zdf" - ) as frame: + with zivid.Frame(_testdata_dir() / "handeye" / "eth" / "img01.zdf") as frame: yield frame @@ -151,6 +149,42 @@ def handeye_eth_transform_fixture(): return np.loadtxt(str(path), delimiter=",") +@pytest.fixture(name="handeye_marker_eth_transform", scope="function") +def handeye_marker_eth_transform_fixture(): + path = _testdata_dir() / "handeye" / "eth" / "eth_transform_marker.csv" + return np.loadtxt(str(path), delimiter=",") + + +@pytest.fixture(name="markers_2d_corners", scope="function") +def markers_2d_corners_fixture(): + path = _testdata_dir() / "marker_detection" + corners = { + int(file.stem.split("_")[-1]): np.loadtxt(file, delimiter=",") + for file in sorted(path.glob("expected_2d_corners_*.csv")) + } + return corners + + +@pytest.fixture(name="markers_3d_corners", scope="function") +def markers_3d_corners_fixture(): + path = _testdata_dir() / "marker_detection" + corners = { + int(file.stem.split("_")[-1]): np.loadtxt(file, delimiter=",") + for file in sorted(path.glob("expected_3d_corners_*.csv")) + } + return corners + + +@pytest.fixture(name="markers_poses", scope="function") +def markers_poses_fixture(): + path = _testdata_dir() / "marker_detection" + poses = { + int(file.stem.split("_")[-1]): np.loadtxt(file, delimiter=",") + for file in sorted(path.glob("expected_poses_*.csv")) + } + return poses + + @pytest.fixture(name="image_2d_rgba", scope="function") def image_2d_rgba_fixture(frame_2d): with frame_2d.image_rgba() as image_2d_rgb: diff --git a/test/test_data/handeye/eth/eth_transform.csv b/test/test_data/handeye/eth/eth_transform.csv index 7dbd1c36..b976e45c 100644 --- a/test/test_data/handeye/eth/eth_transform.csv +++ b/test/test_data/handeye/eth/eth_transform.csv @@ -1,4 +1,4 @@ --1.0561400e-01, 9.0025759e-01, -4.2235285e-01, 1.0759812e+03 - 9.8841107e-01, 1.4160982e-01, 5.4682411e-02, 2.1299239e+02 - 1.0903757e-01, -4.1168302e-01, -9.0478057e-01, 1.1420225e+03 - 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00 \ No newline at end of file +0.983483,-0.125840,0.130096,-155.676971 +-0.165186,-0.917843,0.360939,-1041.600952 +0.073988,-0.376468,-0.923471,1078.035522 +0.000000,0.000000,0.000000,1.000000 diff --git a/test/test_data/handeye/eth/eth_transform_marker.csv b/test/test_data/handeye/eth/eth_transform_marker.csv new file mode 100644 index 00000000..5898c191 --- /dev/null +++ b/test/test_data/handeye/eth/eth_transform_marker.csv @@ -0,0 +1,4 @@ +0.983431,-0.127023,0.129339,-155.261749 +-0.165822,-0.918623,0.358657,-1039.164429 +0.073256,-0.374161,-0.924466,1078.982788 +0.000000,0.000000,0.000000,1.000000 diff --git a/test/test_data/handeye/eth/img01.zdf b/test/test_data/handeye/eth/img01.zdf index 240addeb..ffa52a95 100644 --- a/test/test_data/handeye/eth/img01.zdf +++ b/test/test_data/handeye/eth/img01.zdf @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8da081d97135c29b82780a82a24a8d2176382ded746366bdabfcede0d6be7990 -size 15782253 +oid sha256:15e2b024b1e596b8895764b9ce3f180bb6b8e0db8e8aa13aa3d6e460917a2d78 +size 12535069 diff --git a/test/test_data/handeye/eth/img02.zdf b/test/test_data/handeye/eth/img02.zdf index 6c03ab15..44827924 100644 --- a/test/test_data/handeye/eth/img02.zdf +++ b/test/test_data/handeye/eth/img02.zdf @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2853c8daf7216b8985ba240def1829fb955b99cb4f22003de8fe42ca881cda40 -size 15339920 +oid sha256:95e3fe64f13cd517a94972173844321ec2c64353f83c39def2206eba796dd4b1 +size 12468371 diff --git a/test/test_data/handeye/eth/img03.zdf b/test/test_data/handeye/eth/img03.zdf index 994ed3c5..c96ec08d 100644 --- a/test/test_data/handeye/eth/img03.zdf +++ b/test/test_data/handeye/eth/img03.zdf @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:627c9d5799ddf9d631970e0f51aebaab9f4d935a7520df5548bc7eadc5e2551e -size 15680240 +oid sha256:dcfcf2039b7cf37d83edc838de56992f8248cb34b7085e78b5d6f18f677e4587 +size 12525080 diff --git a/test/test_data/handeye/eth/img04.zdf b/test/test_data/handeye/eth/img04.zdf index 8653bc6c..fee8b117 100644 --- a/test/test_data/handeye/eth/img04.zdf +++ b/test/test_data/handeye/eth/img04.zdf @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:603c712360c12619d517f98cd64a5f3b595d3d036fd44eab5105755404f9381b -size 15520072 +oid sha256:d56048dc7a1de95ff0875228824f32319e9cea6a18de75eb2550f67b947372e4 +size 12523416 diff --git a/test/test_data/handeye/eth/img05.zdf b/test/test_data/handeye/eth/img05.zdf index 31f53f4f..0ccd0157 100644 --- a/test/test_data/handeye/eth/img05.zdf +++ b/test/test_data/handeye/eth/img05.zdf @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a6132489acb10fe0277335f8ee74f4fcdc5cf401c90863b0e39db5fd9502f009 -size 15505966 +oid sha256:82ef1dd100f632d45dff03d9009eac850facf6bea71eb87de84029ee061cc502 +size 12553551 diff --git a/test/test_data/handeye/eth/img06.zdf b/test/test_data/handeye/eth/img06.zdf new file mode 100644 index 00000000..10c820a1 --- /dev/null +++ b/test/test_data/handeye/eth/img06.zdf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1179d3daaa4a48e40ecf7ba2c565ac99819eb573421334e8bf5d168343857dd6 +size 12575484 diff --git a/test/test_data/handeye/eth/pos01.csv b/test/test_data/handeye/eth/pos01.csv deleted file mode 100644 index 6e78beab..00000000 --- a/test/test_data/handeye/eth/pos01.csv +++ /dev/null @@ -1,4 +0,0 @@ --4.14496056e-02, 8.28186082e-01, -5.58918369e-01, 7.40688715e+02 - 9.99042184e-01, 4.22053275e-02, -1.15509529e-02, 3.11641567e+02 - 1.40229944e-02, -5.58861811e-01, -8.29142227e-01, 5.27534304e+02 - 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 \ No newline at end of file diff --git a/test/test_data/handeye/eth/pos02.csv b/test/test_data/handeye/eth/pos02.csv deleted file mode 100644 index 90542d0f..00000000 --- a/test/test_data/handeye/eth/pos02.csv +++ /dev/null @@ -1,4 +0,0 @@ - 6.14571154e-01, 2.51843950e-01, -7.47580712e-01, 7.88611338e+02 - 4.09656013e-01, -9.11759142e-01, 2.96178651e-02, 3.11662646e+02 --6.74154469e-01, -3.24453219e-01, -6.63510256e-01, 5.70821996e+02 - 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 \ No newline at end of file diff --git a/test/test_data/handeye/eth/pos03.csv b/test/test_data/handeye/eth/pos03.csv deleted file mode 100644 index 32c4cfbf..00000000 --- a/test/test_data/handeye/eth/pos03.csv +++ /dev/null @@ -1,4 +0,0 @@ - 8.77709825e-01, 3.85044162e-01, -2.85248061e-01, 7.88639977e+02 - 3.41973082e-01, -9.20297660e-01, -1.90017441e-01, 1.99916604e+02 --3.35678230e-01, 6.92330166e-02, -9.39429037e-01, 5.70800721e+02 - 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 \ No newline at end of file diff --git a/test/test_data/handeye/eth/pos04.csv b/test/test_data/handeye/eth/pos04.csv deleted file mode 100644 index f4c0f076..00000000 --- a/test/test_data/handeye/eth/pos04.csv +++ /dev/null @@ -1,4 +0,0 @@ - 9.30982511e-01, 2.20494439e-01, -2.90953204e-01, 7.88643889e+02 - 1.31712371e-01, -9.46187013e-01, -2.95604444e-01, 2.51841706e+02 --3.40475279e-01, 2.36880431e-01, -9.09925407e-01, 5.70786686e+02 - 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 \ No newline at end of file diff --git a/test/test_data/handeye/eth/pos05.csv b/test/test_data/handeye/eth/pos05.csv deleted file mode 100644 index 700595e9..00000000 --- a/test/test_data/handeye/eth/pos05.csv +++ /dev/null @@ -1,4 +0,0 @@ - 9.31002203e-01, 2.20432893e-01, -2.90936828e-01, 7.88638763e+02 - 2.33894359e-01, -9.72189548e-01, 1.18706489e-02, 2.92714985e+02 --2.80229061e-01, -7.91000831e-02, -9.56668621e-01, 5.70813804e+02 - 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 \ No newline at end of file diff --git a/test/test_data/handeye/eth/pose01.csv b/test/test_data/handeye/eth/pose01.csv new file mode 100644 index 00000000..a59a7e1e --- /dev/null +++ b/test/test_data/handeye/eth/pose01.csv @@ -0,0 +1,4 @@ +-0.9917778,0.1265608,0.01895083,-6.84404 +0.1276576,0.9680644,0.2157656,-625.175 +0.008961841,0.2164108,-0.9762613,168.7004 +0.0,0.0,0.0,1.0 diff --git a/test/test_data/handeye/eth/pose02.csv b/test/test_data/handeye/eth/pose02.csv new file mode 100644 index 00000000..39d193bd --- /dev/null +++ b/test/test_data/handeye/eth/pose02.csv @@ -0,0 +1,4 @@ +-0.9235696,0.3829628,0.01893981,-43.22397 +0.3777829,0.9004077,0.2157454,-508.0394 +0.06556889,0.206411,-0.976266,241.9057 +0.0,0.0,0.0,1.0 diff --git a/test/test_data/handeye/eth/pose03.csv b/test/test_data/handeye/eth/pose03.csv new file mode 100644 index 00000000..c009f38b --- /dev/null +++ b/test/test_data/handeye/eth/pose03.csv @@ -0,0 +1,4 @@ +-0.8344679,0.5184761,-0.1866701,-121.2015 +0.4561117,0.8399507,0.294015,-474.976 +0.3092335,0.1602037,-0.937395,241.9134 +0.0,0.0,0.0,1.0 diff --git a/test/test_data/handeye/eth/pose04.csv b/test/test_data/handeye/eth/pose04.csv new file mode 100644 index 00000000..2433cb4e --- /dev/null +++ b/test/test_data/handeye/eth/pose04.csv @@ -0,0 +1,4 @@ +-0.9593575,-0.0703332,0.2732882,143.5639 +-0.02235286,0.9843394,0.1748607,-591.7223 +-0.2813068,0.1616452,-0.9459056,345.9045 +0.0,0.0,0.0,1.0 diff --git a/test/test_data/handeye/eth/pose05.csv b/test/test_data/handeye/eth/pose05.csv new file mode 100644 index 00000000..5cee86f1 --- /dev/null +++ b/test/test_data/handeye/eth/pose05.csv @@ -0,0 +1,4 @@ +-0.9315816,0.2972482,0.2092828,135.5491 +0.334492,0.9263411,0.1732264,-556.3668 +-0.142376,0.2313779,-0.9623894,261.6708 +0.0,0.0,0.0,1.0 diff --git a/test/test_data/handeye/eth/pose06.csv b/test/test_data/handeye/eth/pose06.csv new file mode 100644 index 00000000..4cb16ffd --- /dev/null +++ b/test/test_data/handeye/eth/pose06.csv @@ -0,0 +1,4 @@ +-0.9516793,0.3037019,0.04551592,82.73807 +0.3070299,0.9439936,0.1208666,-374.4935 +-0.006259312,0.129001,-0.9916247,187.4054 +0.0,0.0,0.0,1.0 diff --git a/test/test_data/marker_detection/expected_2d_corners_1.csv b/test/test_data/marker_detection/expected_2d_corners_1.csv new file mode 100644 index 00000000..f41e01c9 --- /dev/null +++ b/test/test_data/marker_detection/expected_2d_corners_1.csv @@ -0,0 +1,4 @@ +574,265 +546,265 +546,238 +573,238 diff --git a/test/test_data/marker_detection/expected_2d_corners_2.csv b/test/test_data/marker_detection/expected_2d_corners_2.csv new file mode 100644 index 00000000..994797d1 --- /dev/null +++ b/test/test_data/marker_detection/expected_2d_corners_2.csv @@ -0,0 +1,4 @@ +648,438 +643,470 +614,464 +619,434 diff --git a/test/test_data/marker_detection/expected_2d_corners_3.csv b/test/test_data/marker_detection/expected_2d_corners_3.csv new file mode 100644 index 00000000..a8000f17 --- /dev/null +++ b/test/test_data/marker_detection/expected_2d_corners_3.csv @@ -0,0 +1,4 @@ +377,494 +346,492 +350,466 +381,469 diff --git a/test/test_data/marker_detection/expected_2d_corners_4.csv b/test/test_data/marker_detection/expected_2d_corners_4.csv new file mode 100644 index 00000000..54d80616 --- /dev/null +++ b/test/test_data/marker_detection/expected_2d_corners_4.csv @@ -0,0 +1,4 @@ +326,290 +326,260 +351,263 +352,292 diff --git a/test/test_data/marker_detection/expected_3d_corners_1.csv b/test/test_data/marker_detection/expected_3d_corners_1.csv new file mode 100644 index 00000000..4a6320c3 --- /dev/null +++ b/test/test_data/marker_detection/expected_3d_corners_1.csv @@ -0,0 +1,4 @@ +115.167229,-32.816845,1096.996826 +80.216850,-32.664619,1093.883789 +80.573372,-66.225739,1099.040283 +114.447258,-66.529823,1102.028320 diff --git a/test/test_data/marker_detection/expected_3d_corners_2.csv b/test/test_data/marker_detection/expected_3d_corners_2.csv new file mode 100644 index 00000000..bf5f63d4 --- /dev/null +++ b/test/test_data/marker_detection/expected_3d_corners_2.csv @@ -0,0 +1,4 @@ +191.902725,167.360886,1015.674377 +185.083374,203.152802,1009.118958 +154.819336,200.024261,1029.515015 +161.452209,165.958099,1035.810181 diff --git a/test/test_data/marker_detection/expected_3d_corners_3.csv b/test/test_data/marker_detection/expected_3d_corners_3.csv new file mode 100644 index 00000000..9439afc5 --- /dev/null +++ b/test/test_data/marker_detection/expected_3d_corners_3.csv @@ -0,0 +1,4 @@ +-113.256432,221.360397,969.682190 +-147.155945,219.147308,968.943970 +-146.583313,195.679031,995.853638 +-111.610542,198.904343,995.521179 diff --git a/test/test_data/marker_detection/expected_3d_corners_4.csv b/test/test_data/marker_detection/expected_3d_corners_4.csv new file mode 100644 index 00000000..f4da2b5f --- /dev/null +++ b/test/test_data/marker_detection/expected_3d_corners_4.csv @@ -0,0 +1,4 @@ +-174.314056,-1.904519,1002.705933 +-175.409363,-36.024792,1009.367432 +-150.255753,-33.322426,1032.565796 +-148.270966,0.355447,1026.870361 diff --git a/test/test_data/marker_detection/expected_poses_1.csv b/test/test_data/marker_detection/expected_poses_1.csv new file mode 100644 index 00000000..08c4358a --- /dev/null +++ b/test/test_data/marker_detection/expected_poses_1.csv @@ -0,0 +1,4 @@ +-0.996188,0.012585,0.086322,97.554245 +-0.000475,0.988742,-0.149630,-49.818485 +-0.087234,-0.149100,-0.984967,1097.975586 +0.000000,0.000000,0.000000,1.000000 diff --git a/test/test_data/marker_detection/expected_poses_2.csv b/test/test_data/marker_detection/expected_poses_2.csv new file mode 100644 index 00000000..8f564063 --- /dev/null +++ b/test/test_data/marker_detection/expected_poses_2.csv @@ -0,0 +1,4 @@ +-0.187712,0.830468,-0.524488,172.941940 +0.966205,0.060077,-0.250677,184.050980 +-0.176669,-0.553818,-0.813679,1022.716431 +0.000000,0.000000,0.000000,1.000000 diff --git a/test/test_data/marker_detection/expected_poses_3.csv b/test/test_data/marker_detection/expected_poses_3.csv new file mode 100644 index 00000000..9c6dd405 --- /dev/null +++ b/test/test_data/marker_detection/expected_poses_3.csv @@ -0,0 +1,4 @@ +-0.997221,-0.039449,0.063202,-129.307327 +-0.073562,0.655752,-0.751384,208.977020 +-0.011803,-0.753945,-0.656832,982.244934 +0.000000,0.000000,0.000000,1.000000 diff --git a/test/test_data/marker_detection/expected_poses_4.csv b/test/test_data/marker_detection/expected_poses_4.csv new file mode 100644 index 00000000..029d3624 --- /dev/null +++ b/test/test_data/marker_detection/expected_poses_4.csv @@ -0,0 +1,4 @@ +-0.051621,-0.732723,0.678566,-161.979538 +-0.983619,-0.080204,-0.161432,-18.018311 +0.172708,-0.675784,-0.716581,1018.047058 +0.000000,0.000000,0.000000,1.000000