From 9e42addeaefb86d61ccf974adef3093c5cf16a9c Mon Sep 17 00:00:00 2001
From: John Ingve Olsen <john.olsen@zivid.com>
Date: Thu, 21 Nov 2024 14:55:58 +0100
Subject: [PATCH] Implement Matrix4x4.identity()

---
 modules/zivid/matrix4x4.py | 10 ++++++++++
 src/Matrix4x4.cpp          |  1 +
 test/test_matrix4x4.py     |  6 ++++++
 3 files changed, 17 insertions(+)

diff --git a/modules/zivid/matrix4x4.py b/modules/zivid/matrix4x4.py
index 9c7abd6d..7a98dc63 100644
--- a/modules/zivid/matrix4x4.py
+++ b/modules/zivid/matrix4x4.py
@@ -57,6 +57,16 @@ def save(self, file_path):
         """
         super().save(str(file_path))
 
+    @staticmethod
+    def identity():
+        """
+        Return the identity matrix.
+
+        Returns:
+            A new matrix, holding the identity matrix.
+        """
+        return Matrix4x4(_zivid.Matrix4x4.identity())
+
     def __getitem__(self, indexes):
         """
         Access specified element with bounds checking.
diff --git a/src/Matrix4x4.cpp b/src/Matrix4x4.cpp
index f1c363e3..f3026818 100644
--- a/src/Matrix4x4.cpp
+++ b/src/Matrix4x4.cpp
@@ -69,6 +69,7 @@ void ZividPython::wrapClass(py::class_<Zivid::Matrix4x4> pyClass)
         .def(py::init<const Matrix4x4 &>())
         .def(py::init(&createMatrixFrom4x4Array))
         .def(py::init<const std::string &>())
+        .def_static("identity", &Matrix4x4::identity<Matrix4x4::ValueType>)
         .def("save", &Matrix4x4::save)
         .def("load", &Matrix4x4::load)
         .def("_getitem", &getItem)
diff --git a/test/test_matrix4x4.py b/test/test_matrix4x4.py
index e7ea2618..224bf148 100644
--- a/test/test_matrix4x4.py
+++ b/test/test_matrix4x4.py
@@ -1,6 +1,7 @@
 from pathlib import Path
 import tempfile
 import numpy
+import numpy as np
 import numpy.testing
 import pytest
 import zivid
@@ -312,3 +313,8 @@ def test_implicit_convert_to_numpy():
     pose2 = zivid.calibration.Pose(numpy.array(sample))
 
     assert_all_equal_2d(zivid.Matrix4x4(pose1.to_matrix()), pose2.to_matrix())
+
+
+def test_identity():
+    identity = zivid.Matrix4x4.identity()
+    assert np.array_equal(identity, np.identity(4, dtype=np.float32))