Add is_homogeneous_transform

SimoxUtility/CMakeLists.txt

@@ -180,6 +180,7 @@ SET(INCLUDES
 
     math/pose/align_box_orientation.h
     math/pose/check_rotation_matrix.h
+    math/pose/is_homogeneous_transform.h
     math/pose/invert.h
     math/pose/orthogonalize.h
     math/pose/pose.h

SimoxUtility/math/pose.h

@@ -5,6 +5,7 @@
 #include "pose/align_box_orientation.h"
 #include "pose/check_rotation_matrix.h"
 #include "pose/invert.h"
+#include "pose/is_homogeneous_transform.h"
 #include "pose/orthogonalize.h"
 #include "pose/pose.h"
 #include "pose/transform.h"

SimoxUtility/math/pose/is_homogeneous_transform.h

+#pragma once
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/math/pose/check_rotation_matrix.h>
+#include <SimoxUtility/math/is_finite.h>
+
+
+namespace simox::math
+{
+    inline bool is_homogeneous_transform(
+            const Eigen::Matrix4f& mat, 
+            float precision = 1e-6f)
+    {
+        return is_rotation_matrix(mat.template top_left_corner<3,3>(), precision) &&
+                is_finite(mat.template topRightCornery<3,1>()) &&
+                std::abs(mat(3,0)) < precision &&
+                std::abs(mat(3,1)) < precision &&
+                std::abs(mat(3,2)) < precision &&
+                std::abs(mat(3,3) -1) < precision;
+    }
+}
+