add sanity check for camera info

ipm_library/ipm_library/exceptions.py

@@ -13,6 +13,12 @@
 # limitations under the License.
 
 
+class InvalidCameraInfoException(Exception):
+    """Raised if a CameraInfo is invalid."""
+
+    pass
+
+
 class InvalidPlaneException(Exception):
     """Raised if a plane is invalid, i.e. a=b=c=0."""
 

ipm_library/ipm_library/ipm.py

@@ -16,7 +16,11 @@
 
 from ipm_interfaces.msg import PlaneStamped, Point2DStamped
 from ipm_library import utils
-from ipm_library.exceptions import InvalidPlaneException, NoIntersectionError
+from ipm_library.sanity_camera_info import sanity_check
+from ipm_library.exceptions import (
+    InvalidCameraInfoException,
+    InvalidPlaneException,
+    NoIntersectionError)
 import numpy as np
 from sensor_msgs.msg import CameraInfo
 from std_msgs.msg import Header
@@ -42,14 +46,20 @@ def __init__(
         """
         # TF needs a listener that is init in the node context, so we need a reference
         self._tf_buffer = tf_buffer
-        self.set_camera_info(camera_info)
+        if camera_info:
+            self.set_camera_info(camera_info)
 
     def set_camera_info(self, camera_info: CameraInfo) -> None:
         """
         Set a new `CameraInfo` message.
 
         :param camera_info: The updated camera info message.
+        :raise: InvalidCameraInfoException if the camera info is invalid
         """
+        if sanity_check(camera_info):
+            self._camera_info = camera_info
+        else:
+            raise InvalidCameraInfoException
         self._camera_info = camera_info
 
     def get_camera_info(self):

ipm_library/ipm_library/sanity_camera_info.py

@@ -0,0 +1,34 @@
+# Copyright (c) 2022 Kenji Brameld
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from sensor_msgs.msg import CameraInfo
+
+
+def sanity_check(camera_info: CameraInfo) -> bool:
+    """
+    Sanity check if the camera info is valid.
+
+    :param camera_info: The camera info to be checked.
+    :return: True if the camera info is valid, False otherwise.
+    """
+
+    # Check K intrinsic camera matrix is in format:
+    #     [fx  0 cx]
+    # K = [ 0 fy cy]
+    #     [ 0  0  1]
+    if (camera_info.k[0] == 0 or camera_info.k[1] != 0 or camera_info.k[2] == 0 or
+        camera_info.k[3] != 0 or camera_info.k[4] == 0 or camera_info.k[5] == 0 or
+        camera_info.k[6] != 0 or camera_info.k[7] != 0 or camera_info.k[8] != 1):
+        return False
+    return True

ipm_library/test/test_ipm.py

@@ -40,7 +40,7 @@ def test_ipm_camera_info():
     ipm2.set_camera_info(cam)
     assert ipm1.camera_info_received(), 'Failed to set camera info'
     # Set another camera info
-    ipm2.set_camera_info(CameraInfo(header=Header(frame_id='test')))
+    ipm2.set_camera_info(CameraInfo(k=[1000.0, 0., 1000.0, 0., 1500.0, 500.0, 0., 0., 1.]))
     assert ipm2._camera_info != cam, 'Camera info not updated'
 
 

ipm_library/test/test_sanity_camera_info.py

@@ -0,0 +1,34 @@
+# Copyright (c) 2022 Kenji Brameld
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ipm_library.sanity_camera_info import sanity_check
+from sensor_msgs.msg import CameraInfo
+
+
+def test_sanity_check():
+
+    # Check K intrinsic camera matrix is in format:
+    #     [fx  0 cx]
+    # K = [ 0 fy cy]
+    #     [ 0  0  1]
+    assert sanity_check(CameraInfo(k=[1, 0, 1, 0, 1, 1, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[0, 0, 1, 0, 1, 1, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 1, 1, 0, 1, 1, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 0, 0, 1, 1, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 1, 1, 1, 1, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 1, 0, 0, 1, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 1, 0, 1, 0, 0, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 1, 0, 1, 1, 1, 0, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 1, 0, 1, 1, 0, 1, 1]))
+    assert not sanity_check(CameraInfo(k=[1, 0, 1, 0, 1, 1, 0, 0, 0]))