From 1d708a84fabd63418c8962a17e53b8600cf7092a Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sun, 7 Aug 2022 14:17:36 +0900 Subject: [PATCH] add sanity check for camera info Signed-off-by: Kenji Brameld --- ipm_library/ipm_library/exceptions.py | 6 ++++ ipm_library/ipm_library/ipm.py | 14 ++++++-- ipm_library/ipm_library/sanity_camera_info.py | 34 +++++++++++++++++++ ipm_library/test/test_ipm.py | 2 +- ipm_library/test/test_sanity_camera_info.py | 34 +++++++++++++++++++ 5 files changed, 87 insertions(+), 3 deletions(-) create mode 100644 ipm_library/ipm_library/sanity_camera_info.py create mode 100644 ipm_library/test/test_sanity_camera_info.py diff --git a/ipm_library/ipm_library/exceptions.py b/ipm_library/ipm_library/exceptions.py index db8e58a..2d087a3 100644 --- a/ipm_library/ipm_library/exceptions.py +++ b/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.""" diff --git a/ipm_library/ipm_library/ipm.py b/ipm_library/ipm_library/ipm.py index 2f96b5a..f6d217d 100644 --- a/ipm_library/ipm_library/ipm.py +++ b/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): diff --git a/ipm_library/ipm_library/sanity_camera_info.py b/ipm_library/ipm_library/sanity_camera_info.py new file mode 100644 index 0000000..6fd58ce --- /dev/null +++ b/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 diff --git a/ipm_library/test/test_ipm.py b/ipm_library/test/test_ipm.py index aef373e..f3284d4 100644 --- a/ipm_library/test/test_ipm.py +++ b/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' diff --git a/ipm_library/test/test_sanity_camera_info.py b/ipm_library/test/test_sanity_camera_info.py new file mode 100644 index 0000000..e877fdf --- /dev/null +++ b/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]))