diff --git a/modules/zivid/experimental/hand_eye_low_dof.py b/modules/zivid/experimental/hand_eye_low_dof.py new file mode 100644 index 00000000..246455ef --- /dev/null +++ b/modules/zivid/experimental/hand_eye_low_dof.py @@ -0,0 +1,269 @@ +"""Experimental implementation of hand-eye calibration for robots with low degrees-of-freedom. + +This API may change in the future. +""" + +import collections.abc +import _zivid +from zivid.calibration import Pose, HandEyeOutput, MarkerDictionary + + +class FixedPlacementOfFiducialMarker: + """Specifies the fixed placement of a fiducial marker for low degrees-of-freedom hand-eye calibration.""" + + def __init__(self, marker_id, position): + """Construct a FixedPlacementOfFiducialMarker. + + For eye-in-hand calibration, positions should be given in the robot's base frame. For eye-to-hand calibration, + positions should be given in the robot's end-effector frame. + + Note: the units of the input robot poses must be consistent with the units of the point clouds used to create + the detection result. Zivid point clouds are, by default, in millimeters. + + Args: + marker_id: The ID of the fiducial marker to specify a position for. + position: The position of the fiducial marker as a three-element list, specified at the center of the + marker. + + Raises: + TypeError: If one of the input arguments is of the wrong type. + """ + if not isinstance(marker_id, int): + raise TypeError( + "Unsupported type for argument marker_id. Expected int but got {}".format( + type(marker_id) + ) + ) + + if not isinstance( + position, (collections.abc.Iterable, _zivid.data_model.PointXYZ) + ): + raise TypeError( + "Unsupported type for argument position. Expected: (collections.abc.Iterable, _zivid.data_model.PointXYZ), got {value_type}".format( + value_type=type(position) + ) + ) + + self.__impl = _zivid.calibration.FixedPlacementOfFiducialMarker( + marker_id, # pylint: disable=protected-access + _zivid.data_model.PointXYZ(position), # pylint: disable=protected-access + ) + + @property + def id(self): + """Get ID of fiducial marker. + + Returns: + An integer representing the ID + """ + return self.__impl.id + + @property + def position(self): + """Get position of fiducial marker. + + Returns: + A three-element list of floats + """ + return self.__impl.position.to_array() + + def __str__(self): + return str(self.__impl) + + +class FixedPlacementOfFiducialMarkers: # pylint: disable=too-few-public-methods + """Specifies the fixed placement of a list of fiducial markers for low degrees-of-freedom hand-eye calibration.""" + + def __init__(self, marker_dictionary, markers): + """Construct a FixedPlacementOfFiducialMarkers instance. + + Args: + marker_dictionary: The dictionary that describes the appearance of the given markers. The name must be one + of the values returned by MarkerDictionary.valid_values() + markers: A list of FixedPlacementOfFiducialMarker describing the fixed placement of fiducial markers. + + Raises: + ValueError: If marker_dictionary is not one of the valid values returned by MarkerDictionary.valid_values() + TypeError: If one of the input arguments are of the wrong type + """ + + 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 + ) + ) + + if not ( + isinstance(markers, list) + and all( + isinstance(marker, FixedPlacementOfFiducialMarker) for marker in markers + ) + ): + raise TypeError( + "Unsupported type for argument position. Expected list of FixedPlacementOfFiducialMarker but got {}".format( + type(markers) + ) + ) + + self.__impl = _zivid.calibration.FixedPlacementOfFiducialMarkers( + dictionary, # pylint: disable=protected-access + [ + marker._FixedPlacementOfFiducialMarker__impl # pylint: disable=protected-access + for marker in markers + ], + ) + + def __str__(self): + return str(self.__impl) + + +class FixedPlacementOfCalibrationBoard: # pylint: disable=too-few-public-methods + """Specifies the fixed placement of a Zivid calibration board for low degrees-of-freedom hand-eye calibration.""" + + def __init__(self, position_or_pose): + """Construct a FixedPlacementOfCalibrationBoard instance. + + For eye-in-hand calibration, the position or pose should be given in the robot's base frame. For eye-to-hand + calibration, the position or pose should be given in the robot's end-effector frame. + + The origin is the top left inner corner of the calibration board. Using a pose instead of a position can improve + accuracy of the hand-eye calibration in some situations. + + Note: the units of the input robot poses must be consistent with the units of the point clouds used to create + the detection result. Zivid point clouds are, by default, in millimeters. + + Args: + position_or_pose: A position specifying the origin of the calibration board as a three-element list, or the + pose of the calibration board specified using the Pose type. + + Raises: + TypeError: If the input argument is of the wrong type. + """ + if isinstance(position_or_pose, Pose): + self.__impl = _zivid.calibration.FixedPlacementOfCalibrationBoard( + position_or_pose._Pose__impl # pylint: disable=protected-access + ) + elif isinstance( + position_or_pose, (collections.abc.Iterable, _zivid.data_model.PointXYZ) + ): + self.__impl = _zivid.calibration.FixedPlacementOfCalibrationBoard( + _zivid.data_model.PointXYZ( + position_or_pose + ), # pylint: disable=protected-access + ) + else: + raise TypeError( + "Unsupported type for argument id. Expected zivid.calibration.Pose " + "or a three-element list, but got {}".format(type(position_or_pose)) + ) + + def __str__(self): + return str(self.__impl) + + +class FixedPlacementOfCalibrationObjects: # pylint: disable=too-few-public-methods + """Specifies the fixed placement of calibration objects for low degrees-of-freedom hand-eye calibration.""" + + def __init__(self, fixed_objects): + """Construct a FixedPlacementOfCalibrationObjects instance from fiducial markers or a calibration board. + + Args: + fixed_objects: An instance of FixedPlacementOfFiducialMarkers or FixedPlacementOfCalibrationBoard. + + Raises: + TypeError: If the input argument is of the wrong type. + """ + if isinstance(fixed_objects, FixedPlacementOfFiducialMarkers): + self.__impl = _zivid.calibration.FixedPlacementOfCalibrationObjects( + fixed_objects._FixedPlacementOfFiducialMarkers__impl # pylint: disable=protected-access + ) + elif isinstance(fixed_objects, FixedPlacementOfCalibrationBoard): + self.__impl = _zivid.calibration.FixedPlacementOfCalibrationObjects( + fixed_objects._FixedPlacementOfCalibrationBoard__impl # pylint: disable=protected-access + ) + else: + raise TypeError( + "Unsupported type for argument fixed_objects. Got {}, expected {} or {}".format( + type(fixed_objects), + FixedPlacementOfFiducialMarkers, + FixedPlacementOfCalibrationBoard, + ) + ) + + def __str__(self): + return str(self.__impl) + + +def calibrate_eye_in_hand_low_dof(calibration_inputs, fixed_objects): + """Perform eye-in-hand calibration for low degrees-of-freedom robots. + + For robots with low degrees-of-freedom (DOF), that is, less than 6 DOF, the robot pose and capture inputs are not + alone sufficient to uniquely identify the solution to the hand-eye calibration. This procedure additionally takes + knowledge about the fixed placement of the calibration objects in the scene to provide a unique solution. For 6 DOF + robots, consider using the `calibrate_eye_in_hand` function instead. + + The procedure requires all robot poses to be different. At least 2 poses are required when using a calibration + board, or 6 poses when using fiducial markers. For fiducial markers, each marker must be detected across 2 poses at + minimum. An exception will be thrown if the preceding requirements are not fulfilled. + + Note: the units of the input robot poses must be consistent with the units of the point clouds used to create the + detection results. Zivid point clouds are, by default, in millimeters. + + Args: + calibration_inputs: List of HandEyeInput + fixed_objects: Specifies the fixed placement of calibration objects in the robot's base frame, using an instance + of FixedPlacementOfCalibrationObjects. + + Returns: + A HandEyeOutput instance containing the eye-in-hand transform (camera pose in robot end-effector frame) + """ + return HandEyeOutput( + _zivid.calibration.calibrate_eye_in_hand_low_dof( + [ + calibration_input._HandEyeInput__impl # pylint: disable=protected-access + for calibration_input in calibration_inputs + ], + fixed_objects._FixedPlacementOfCalibrationObjects__impl, # pylint: disable=protected-access + ) + ) + + +def calibrate_eye_to_hand_low_dof(calibration_inputs, fixed_objects): + """Perform eye-to-hand calibration for low degrees-of-freedom robots. + + For robots with low degrees-of-freedom (DOF), that is, less than 6 DOF, the robot pose and capture inputs are not + alone sufficient to uniquely identify the solution to the hand-eye calibration. This procedure additionally takes + knowledge about the fixed placement of the calibration objects in the scene to provide a unique solution. For 6 DOF + robots, consider using the `calibrate_eye_to_hand` function instead. + + The procedure requires all robot poses to be different. At least 2 poses are required when using a calibration + board, or 6 poses when using fiducial markers. For fiducial markers, each marker must be detected across 2 poses at + minimum. An exception will be thrown if the preceding requirements are not fulfilled. + + Note: the units of the input robot poses must be consistent with the units of the point clouds used to create the + detection results. Zivid point clouds are, by default, in millimeters. + + Args: + calibration_inputs: List of HandEyeInput + fixed_objects: Specifies the fixed placement of calibration objects in the robot's end-effector frame, using an + instance of FixedPlacementOfCalibrationObjects. + + Returns: + A HandEyeOutput instance containing the eye-to-hand transform (camera pose in robot base frame) + """ + return HandEyeOutput( + _zivid.calibration.calibrate_eye_to_hand_low_dof( + [ + calibration_input._HandEyeInput__impl # pylint: disable=protected-access + for calibration_input in calibration_inputs + ], + fixed_objects._FixedPlacementOfCalibrationObjects__impl, # pylint: disable=protected-access + ) + ) diff --git a/src/Calibration/Calibration.cpp b/src/Calibration/Calibration.cpp index 18adb26e..3fddb42e 100644 --- a/src/Calibration/Calibration.cpp +++ b/src/Calibration/Calibration.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -25,6 +26,7 @@ namespace ZividPython::Calibration void wrapAsSubmodule(py::module &dest) { using namespace Zivid::Calibration; + using namespace Zivid::Experimental::Calibration::HandEyeLowDOF; ZIVID_PYTHON_WRAP_CLASS(dest, Pose); ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeOutput); @@ -35,6 +37,11 @@ namespace ZividPython::Calibration ZIVID_PYTHON_WRAP_CLASS(dest, DetectionResultFiducialMarkers); ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeResidual); + ZIVID_PYTHON_WRAP_CLASS(dest, FixedPlacementOfFiducialMarker); + ZIVID_PYTHON_WRAP_CLASS(dest, FixedPlacementOfFiducialMarkers); + ZIVID_PYTHON_WRAP_CLASS(dest, FixedPlacementOfCalibrationBoard); + ZIVID_PYTHON_WRAP_CLASS(dest, FixedPlacementOfCalibrationObjects); + ZIVID_PYTHON_WRAP_CLASS(dest, MultiCameraResidual); ZIVID_PYTHON_WRAP_CLASS(dest, MultiCameraOutput); @@ -62,6 +69,20 @@ namespace ZividPython::Calibration }) .def("calibrate_eye_in_hand", &Zivid::Calibration::calibrateEyeInHand) .def("calibrate_eye_to_hand", &Zivid::Calibration::calibrateEyeToHand) + .def( + "calibrate_eye_in_hand_low_dof", + [](const std::vector &inputs, const FixedPlacementOfCalibrationObjects &fixedObjects) { + return Zivid::Experimental::Calibration::calibrateEyeInHandLowDOF(inputs, fixedObjects); + }, + py::arg("inputs"), + py::arg("fixed_objects")) + .def( + "calibrate_eye_to_hand_low_dof", + [](const std::vector &inputs, const FixedPlacementOfCalibrationObjects &fixedObjects) { + return Zivid::Experimental::Calibration::calibrateEyeToHandLowDOF(inputs, fixedObjects); + }, + py::arg("inputs"), + py::arg("fixed_objects")) .def("calibrate_multi_camera", &Zivid::Calibration::calibrateMultiCamera) .def( "intrinsics", @@ -95,4 +116,4 @@ namespace ZividPython::Calibration py::arg("camera"), py::arg("settings")); } -} // namespace ZividPython::Calibration \ No newline at end of file +} // namespace ZividPython::Calibration diff --git a/src/Calibration/HandEye.cpp b/src/Calibration/HandEye.cpp index dd83f680..3691ec2d 100644 --- a/src/Calibration/HandEye.cpp +++ b/src/Calibration/HandEye.cpp @@ -1,5 +1,6 @@ #include +#include #include #include @@ -36,4 +37,37 @@ namespace ZividPython }) .def("detection_result", &Zivid::Calibration::HandEyeInput::detectionResult); } + + void wrapClass( + pybind11::class_ pyClass) + { + using T = Zivid::Experimental::Calibration::HandEyeLowDOF::FixedPlacementOfFiducialMarker; + pyClass.def(py::init()) + .def_property_readonly("id", &T::id) + .def_property_readonly("position", &T::position); + } + + void wrapClass( + pybind11::class_ pyClass) + { + using T = Zivid::Experimental::Calibration::HandEyeLowDOF::FixedPlacementOfFiducialMarkers; + pyClass.def( + py::init + &>()); + } + + void wrapClass( + pybind11::class_ pyClass) + { + pyClass.def(py::init()).def(py::init()); + } + + void wrapClass( + pybind11::class_ pyClass) + { + pyClass + .def(py::init()) + .def(py::init()); + } } // namespace ZividPython diff --git a/src/PixelMapping.cpp b/src/PixelMapping.cpp index 65d235ab..b83e01b6 100644 --- a/src/PixelMapping.cpp +++ b/src/PixelMapping.cpp @@ -1,5 +1,3 @@ -#pragma once - #include #include diff --git a/src/include/ZividPython/Calibration/HandEye.h b/src/include/ZividPython/Calibration/HandEye.h index 11c084b7..6b2a7273 100644 --- a/src/include/ZividPython/Calibration/HandEye.h +++ b/src/include/ZividPython/Calibration/HandEye.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include @@ -9,4 +10,12 @@ namespace ZividPython void wrapClass(pybind11::class_ pyClass); void wrapClass(pybind11::class_ pyClass); void wrapClass(pybind11::class_ pyClass); + 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/test/calibration/test_hand_eye.py b/test/calibration/test_hand_eye.py index 98742191..709f41b4 100644 --- a/test/calibration/test_hand_eye.py +++ b/test/calibration/test_hand_eye.py @@ -39,31 +39,6 @@ def test_handeye_input(checkerboard_frames, transform): assert detection_result_returned.valid() == detection_result.valid() -def _check_handeye_output(inputs, handeye_output, expected_transform): - assert isinstance(handeye_output, zivid.calibration.HandEyeOutput) - assert handeye_output.valid() - assert bool(handeye_output) - assert str(handeye_output) - - # Check returned transform - transform_returned = handeye_output.transform() - assert isinstance(transform_returned, np.ndarray) - assert transform_returned.shape == (4, 4) - np.testing.assert_allclose(transform_returned, expected_transform, rtol=1e-5) - - # Check returned residuals - residuals_returned = handeye_output.residuals() - assert isinstance(residuals_returned, list) - assert len(residuals_returned) == len(inputs) - for residual in residuals_returned: - assert isinstance(residual, zivid.calibration.HandEyeResidual) - assert str(residual) - assert isinstance(residual.translation(), float) - assert residual.translation() >= 0.0 - assert isinstance(residual.rotation(), float) - assert residual.rotation() >= 0.0 - - def test_eyetohand_calibration( handeye_eth_frames, handeye_eth_poses, handeye_eth_transform ): @@ -79,7 +54,7 @@ def test_eyetohand_calibration( # Perform eye-to-hand calibration handeye_output = zivid.calibration.calibrate_eye_to_hand(inputs) - _check_handeye_output(inputs, handeye_output, handeye_eth_transform) + pytest.helpers.check_handeye_output(inputs, handeye_output, handeye_eth_transform) def test_marker_eyetohand_calibration( @@ -99,7 +74,9 @@ def test_marker_eyetohand_calibration( # Perform eye-to-hand calibration handeye_output = zivid.calibration.calibrate_eye_to_hand(inputs) - _check_handeye_output(inputs, handeye_output, handeye_marker_eth_transform) + pytest.helpers.check_handeye_output( + inputs, handeye_output, handeye_marker_eth_transform + ) def test_eyetohand_calibration_save_load(handeye_eth_frames, handeye_eth_poses): diff --git a/test/conftest.py b/test/conftest.py index 5d2c77d8..a4380eb1 100644 --- a/test/conftest.py +++ b/test/conftest.py @@ -155,6 +155,57 @@ def handeye_marker_eth_transform_fixture(): return np.loadtxt(str(path), delimiter=",") +@pytest.fixture(name="handeye_eth_low_dof_markers_transform", scope="function") +def handeye_eth_low_dof_markers_transform_fixture(): + path = ( + _testdata_dir() + / "handeye" + / "eth" + / "low_dof" + / "eth_low_dof_transform_marker.csv" + ) + return np.loadtxt(str(path), delimiter=",") + + +@pytest.fixture(name="handeye_eth_low_dof_transform", scope="function") +def handeye_eth_low_dof_transform_fixture(): + path = _testdata_dir() / "handeye" / "eth" / "low_dof" / "eth_low_dof_transform.csv" + return np.loadtxt(str(path), delimiter=",") + + +@pytest.fixture( + name="handeye_eth_low_dof_fixed_calibration_board_pose", scope="function" +) +def handeye_eth_low_dof_fixed_calibration_board_pose_fixture(): + path = ( + _testdata_dir() + / "handeye" + / "eth" + / "low_dof" + / "eth_low_dof_fixed_calibration_board_pose.csv" + ) + return np.loadtxt(str(path), delimiter=",") + + +@pytest.fixture( + name="handeye_eth_low_dof_fixed_markers_id_position_list", scope="function" +) +def handeye_eth_low_dof_fixed_markers_id_position_list_fixture(): + markers_id_position_list = [] + for i in range(1, 5): + path = ( + _testdata_dir() + / "handeye" + / "eth" + / "low_dof" + / f"eth_low_dof_fixed_marker_id_{i}.csv" + ) + pose = np.loadtxt(str(path), delimiter=",") + position = pose[:3, 3] + markers_id_position_list.append((i, position)) + return markers_id_position_list + + @pytest.fixture(name="markers_2d_corners", scope="function") def markers_2d_corners_fixture(): path = _testdata_dir() / "marker_detection" @@ -276,6 +327,32 @@ def equality_tester(settings_type, value_collection_1, value_collection_2): assert instance_3 != instance_2 +@pytest.helpers.register +def check_handeye_output(inputs, handeye_output, expected_transform): + assert isinstance(handeye_output, zivid.calibration.HandEyeOutput) + assert handeye_output.valid() + assert bool(handeye_output) + assert str(handeye_output) + + # Check returned transform + transform_returned = handeye_output.transform() + assert isinstance(transform_returned, np.ndarray) + assert transform_returned.shape == (4, 4) + np.testing.assert_allclose(transform_returned, expected_transform, rtol=1e-5) + + # Check returned residuals + residuals_returned = handeye_output.residuals() + assert isinstance(residuals_returned, list) + assert len(residuals_returned) == len(inputs) + for residual in residuals_returned: + assert isinstance(residual, zivid.calibration.HandEyeResidual) + assert str(residual) + assert isinstance(residual.translation(), float) + assert residual.translation() >= 0.0 + assert isinstance(residual.rotation(), float) + assert residual.rotation() >= 0.0 + + class Cd: def __init__(self, new_path): self.new_path = new_path diff --git a/test/experimental/test_hand_eye_low_dof.py b/test/experimental/test_hand_eye_low_dof.py new file mode 100644 index 00000000..07c3ddc8 --- /dev/null +++ b/test/experimental/test_hand_eye_low_dof.py @@ -0,0 +1,256 @@ +import zivid +import zivid.experimental.hand_eye_low_dof +import numpy as np +import pytest + + +def test_fixed_placement_of_fiducial_marker(): + marker = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 0, [1, 2, 3] + ) + assert str(marker) == "{ id: 0, position: { x: 1, y: 2, z: 3 } }" + assert marker.position == [1, 2, 3] + assert marker.id == 0 + + marker = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 1, [1.5, 2.5, 3.5] + ) + assert str(marker) == "{ id: 1, position: { x: 1.5, y: 2.5, z: 3.5 } }" + marker = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 2, (10, 10.5, 11) + ) + assert str(marker) == "{ id: 2, position: { x: 10, y: 10.5, z: 11 } }" + + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker(0, 1) + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker(0, []) + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker(0, [1]) + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker(0, [1, 2]) + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 0, [1, 2, 3, 4] + ) + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 0, [1, 2, "string"] + ) + + +def test_fixed_placement_of_fiducial_markers(): + marker1 = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 0, [1, 2, 3] + ) + marker2 = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 1, [4, 5, 6] + ) + markers = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarkers( + zivid.calibration.MarkerDictionary.aruco4x4_50, [marker1, marker2] + ) + + assert str(markers) == ( + "{ dictionary: aruco4x4_50, markers: { { id: 0, position: { x: 1, y: 2, z: 3 " + "} }, { id: 1, position: { x: 4, y: 5, z: 6 } } } }" + ) + + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarkers( + zivid.calibration.MarkerDictionary.aruco4x4_50, [1, 2, 3] + ) + with pytest.raises(ValueError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarkers( + "invalid_marker_dictionary", [marker1, marker2] + ) + + +def test_fixed_placement_of_calibration_board(): + calibration_board = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard([1, 2, 3]) + ) + assert str(calibration_board) == "point: { x: 1, y: 2, z: 3 }" + calibration_board = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard( + (1.5, 2, 3.5) + ) + ) + assert str(calibration_board) == "point: { x: 1.5, y: 2, z: 3.5 }" + + calibration_board = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard( + zivid.calibration.Pose(zivid.Matrix4x4.identity()) + ) + ) + assert str(calibration_board) == ( + "pose: [ [ 1.000000, 0.000000, 0.000000, 0.000000], \n" + " [ 0.000000, 1.000000, 0.000000, 0.000000], \n" + " [ 0.000000, 0.000000, 1.000000, 0.000000], \n" + " [ 0.000000, 0.000000, 0.000000, 1.000000] ]" + ) + + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard( + [1, 2, "string"] + ) + with pytest.raises(TypeError): + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard(1) + + +def test_fixed_placement_of_calibration_objects(): + calibration_board = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard([1, 2, 3]) + ) + calibration_objects = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationObjects( + calibration_board + ) + ) + assert str(calibration_objects) == "calibrationBoard: point: { x: 1, y: 2, z: 3 }" + + marker = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + 0, [1, 2, 3] + ) + markers = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarkers( + zivid.calibration.MarkerDictionary.aruco4x4_250, [marker] + ) + calibration_objects = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationObjects(markers) + ) + assert ( + str(calibration_objects) + == "markers: { dictionary: aruco4x4_250, markers: { { id: 0, position: { x: 1, y: 2, z: 3 } } } }" + ) + + +def test_eth_transform_low_dof_approximate_match( + handeye_eth_transform, + handeye_marker_eth_transform, + handeye_eth_low_dof_transform, + handeye_eth_low_dof_markers_transform, +): + # Ensure that the low DOF calibration transforms are approximately the same as their + # full 6-DOF calibration transform counterparts. + np.testing.assert_allclose( + handeye_eth_transform, handeye_eth_low_dof_transform, rtol=1e-2 + ) + np.testing.assert_allclose( + handeye_marker_eth_transform, handeye_eth_low_dof_markers_transform, rtol=1e-2 + ) + + +def test_eye_to_hand_low_dof_calibration_with_calibration_board( + handeye_eth_frames, + handeye_eth_poses, + handeye_eth_low_dof_fixed_calibration_board_pose, + handeye_eth_low_dof_transform, +): + 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_calibration_board(frame), + ) + ) + + calibration_board_pose = zivid.calibration.Pose( + handeye_eth_low_dof_fixed_calibration_board_pose + ) + + fixed_calibration_board = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard( + calibration_board_pose + ) + ) + fixed_objects = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationObjects( + fixed_calibration_board + ) + ) + + handeye_output = zivid.experimental.hand_eye_low_dof.calibrate_eye_to_hand_low_dof( + inputs, fixed_objects + ) + pytest.helpers.check_handeye_output( + inputs, handeye_output, handeye_eth_low_dof_transform + ) + + +def test_eye_to_hand_low_dof_calibration_with_markers( + handeye_eth_frames, + handeye_eth_poses, + handeye_eth_low_dof_markers_transform, + handeye_eth_low_dof_fixed_markers_id_position_list, +): + 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 + ), + ) + ) + + fixed_marker_list = [] + for marker_id, position in handeye_eth_low_dof_fixed_markers_id_position_list: + fixed_marker_list.append( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarker( + marker_id, position + ) + ) + + fixed_markers = zivid.experimental.hand_eye_low_dof.FixedPlacementOfFiducialMarkers( + zivid.calibration.MarkerDictionary.aruco4x4_50, fixed_marker_list + ) + fixed_objects = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationObjects( + fixed_markers + ) + ) + + handeye_output = zivid.experimental.hand_eye_low_dof.calibrate_eye_to_hand_low_dof( + inputs, fixed_objects + ) + pytest.helpers.check_handeye_output( + inputs, handeye_output, handeye_eth_low_dof_markers_transform + ) + + +def test_eye_in_hand_low_dof_calibration_with_eye_to_hand_data( + handeye_eth_frames, + handeye_eth_poses, + handeye_eth_low_dof_fixed_calibration_board_pose, + handeye_eth_low_dof_transform, +): + # This is a negative test, it won't calibrate correctly since we are using + # eye-to-hand calibration data with the eye-in-hand functionality. + 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_calibration_board(frame), + ) + ) + + calibration_board_pose = zivid.calibration.Pose( + np.linalg.inv(handeye_eth_low_dof_fixed_calibration_board_pose) + ) + fixed_calibration_board = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationBoard( + calibration_board_pose + ) + ) + fixed_objects = ( + zivid.experimental.hand_eye_low_dof.FixedPlacementOfCalibrationObjects( + fixed_calibration_board + ) + ) + + with pytest.raises(RuntimeError): + _ = zivid.experimental.hand_eye_low_dof.calibrate_eye_to_hand_low_dof( + inputs, fixed_objects + ) diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_calibration_board_pose.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_calibration_board_pose.csv new file mode 100644 index 00000000..12719d92 --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_calibration_board_pose.csv @@ -0,0 +1,4 @@ +9.99863382e-01, -1.64939625e-02, 1.09068176e-03, -8.62072851e01 +1.64698414e-02, 9.99678207e-01, 1.92916581e-02, -2.20662949e02 +-1.40852676e-03, -1.92709589e-02, 9.99813332e-01, 8.45384867e01 +0.00000000e00, 0.00000000e00, 0.00000000e00, 1.00000000e00 diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_1.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_1.csv new file mode 100644 index 00000000..91efc8d7 --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_1.csv @@ -0,0 +1,4 @@ +9.99976944e-01, -6.29740536e-03, -2.64507631e-03, -9.45798545e+01 +6.33697048e-03, 9.99864256e-01, 1.52088018e-02, 4.13624306e+00 +2.54818467e-03, -1.52256366e-02, 9.99881136e-01, 8.09407137e+01 +0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_2.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_2.csv new file mode 100644 index 00000000..9e5432c4 --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_2.csv @@ -0,0 +1,4 @@ + 2.08146456e-01, 7.76432507e-01, -5.94834512e-01, -1.61570942e+02 +-9.78095183e-01, 1.66592400e-01, -1.24808110e-01, -2.40366371e+02 + 2.19018750e-03, 6.07783067e-01, 7.94100061e-01, 3.88355722e+01 + 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_3.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_3.csv new file mode 100644 index 00000000..400dc85c --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_3.csv @@ -0,0 +1,4 @@ +9.93766937e-01, -1.11237450e-01, 7.34028068e-03, 1.43233429e+02 +9.07796691e-02, 7.69269774e-01, -6.32442132e-01, -2.65461067e+02 +6.47050871e-02, 6.29166385e-01, 7.74573241e-01, 2.99797612e+01 +0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_4.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_4.csv new file mode 100644 index 00000000..336b427e --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_fixed_marker_id_4.csv @@ -0,0 +1,4 @@ +3.04612807e-02, -7.90602379e-01, 6.11571315e-01, 1.71304551e+02 +9.99460206e-01, 1.65802892e-02, -2.83464806e-02, -3.50534130e+01 +1.22703284e-02, 6.12105127e-01, 7.90681594e-01, 3.06688180e+01 +0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_transform.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_transform.csv new file mode 100644 index 00000000..a26ebda6 --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_transform.csv @@ -0,0 +1,4 @@ + 9.8341876e-01, -1.2604053e-01, 1.3038915e-01, -1.5606738e+02 +-1.6551855e-01, -9.1761994e-01, 3.6135471e-01, -1.0418158e+03 + 7.4102350e-02, -3.7694484e-01, -9.2326671e-01, 1.0781750e+03 + 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00 diff --git a/test/test_data/handeye/eth/low_dof/eth_low_dof_transform_marker.csv b/test/test_data/handeye/eth/low_dof/eth_low_dof_transform_marker.csv new file mode 100644 index 00000000..cb942c23 --- /dev/null +++ b/test/test_data/handeye/eth/low_dof/eth_low_dof_transform_marker.csv @@ -0,0 +1,4 @@ + 9.8331505e-01, -1.2777601e-01, 1.2947875e-01, -1.5543134e+02 +-1.6660008e-01, -9.1837466e-01, 3.5893229e-01, -1.0392861e+03 + 7.3047072e-02, -3.7451470e-01, -9.2433918e-01, 1.0793580e+03 + 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00