diff --git a/.travis.sh b/.travis.sh index 8b1037ef0..dad87cb97 100755 --- a/.travis.sh +++ b/.travis.sh @@ -22,18 +22,18 @@ function travis_time_end { set -x } -apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg # for docker +apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg build-essential # for docker DEBIAN_FRONTEND=noninteractive apt-get install -y tzdata # https://stackoverflow.com/questions/44331836/apt-get-install-tzdata-noninteractive travis_time_start setup.before_install #before_install: # Define some config vars. # Install ROS -sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +sudo sh -c 'echo "deb http://repositories.ros.org/ubuntu/testing $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' wget http://packages.ros.org/ros.key -O - | sudo apt-key add - sudo apt-get update -qq || echo Ignore error on apt-get update # Install ROS -sudo apt-get install -qq -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin +sudo apt-get install -qq -y python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon python3-rosdep python3-wstool ros-$ROS_DISTRO-catkin source /opt/ros/$ROS_DISTRO/setup.bash # Setup for rosdep sudo rosdep init diff --git a/.travis.yml b/.travis.yml index 19e599e88..aa3adcde3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,7 +2,7 @@ sudo: required dist: trusty language: generic env: - - ROS_DISTRO=melodic + - ROS_DISTRO=noetic # Install system dependencies, namely ROS. before_install: # Define some config vars. @@ -11,7 +11,7 @@ before_install: - export ROS_PARALLEL_JOBS='-j8 -l6' script: - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" - - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -t ubuntu:bionic sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" + - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -t ubuntu:focal sh -c "cd $CI_SOURCE_PATH; ./.travis.sh" after_failure: - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \; - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done diff --git a/LICENSE b/LICENSE new file mode 100644 index 000000000..d79557eef --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2008, Willow Garage, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/cv_bridge/CHANGELOG.rst b/cv_bridge/CHANGELOG.rst index b897384cf..02d60576a 100644 --- a/cv_bridge/CHANGELOG.rst +++ b/cv_bridge/CHANGELOG.rst @@ -2,6 +2,39 @@ Changelog for package cv_bridge ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.16.2 (2022-10-03) +------------------- + +1.16.1 (2022-09-06) +------------------- +* Fix 16U encoding type (`#445 `_) +* Add header arg to cv2_to_imgmsg (`#326 `_) +* Contributors: Ibrahim Essam, Kenji Brameld, Naoya Yamaguchi + +1.16.0 (2021-11-23) +------------------- +* prevent conversion of single channel 16bit integer images to/from colour (`#412 `_) +* decode images in mode IMREAD_UNCHANGED (`#228 `_) +* Optimize includes (`#354 `_) + As suggested by include-what-you-use +* Fix Python linking on OSX (`#331 `_) +* Fix typo (`#333 `_) +* Contributors: Christian Rauch, Markus Vieth, Matthijs van der Burgh, Tobias Fischer + +1.15.0 (2020-05-19) +------------------- +* [Noetic] Use opencv3 on buster (`#330 `_) +* more portable fixes. (`#328 `_) +* Contributors: Sean Yen, Shane Loretz + +1.14.0 (2020-04-06) +------------------- +* Noetic release (`#323 `_) +* update CMakeLists.txt for Windows build environment (`#265 `_) +* remove path splash separator from 'package_dir' (`#267 `_) +* fix travis. (`#269 `_) +* Contributors: Alejandro Hernández Cordero, James Xu, Sean Yen + 1.13.0 (2018-04-30) ------------------- * Use rosdep OpenCV and not ROS one. diff --git a/cv_bridge/CMakeLists.txt b/cv_bridge/CMakeLists.txt index 997bef3e1..ef804b91d 100644 --- a/cv_bridge/CMakeLists.txt +++ b/cv_bridge/CMakeLists.txt @@ -1,19 +1,30 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.0.2) project(cv_bridge) find_package(catkin REQUIRED COMPONENTS rosconsole sensor_msgs) if(NOT ANDROID) find_package(PythonLibs) - if(PYTHONLIBS_VERSION_STRING VERSION_LESS 3) - find_package(Boost REQUIRED python) + + if(PYTHONLIBS_VERSION_STRING VERSION_LESS "3.8") + # Debian Buster + find_package(Boost REQUIRED python37) else() - find_package(Boost REQUIRED python3) + # Ubuntu Focal + find_package(Boost REQUIRED python) endif() else() find_package(Boost REQUIRED) endif() -find_package(OpenCV 3 REQUIRED + +set(_opencv_version 4) +find_package(OpenCV 4 QUIET) +if(NOT OpenCV_FOUND) + message(STATUS "Did not find OpenCV 4, trying OpenCV 3") + set(_opencv_version 3) +endif() + +find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS opencv_core opencv_imgproc @@ -32,6 +43,7 @@ catkin_package( catkin_python_setup() include_directories(include ${Boost_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) +link_directories(${Boost_LIBRARY_DIRS}) if(NOT ANDROID) add_subdirectory(python) diff --git a/cv_bridge/include/cv_bridge/rgb_colors.h b/cv_bridge/include/cv_bridge/rgb_colors.h index 1eaa88b37..c50d10fcd 100644 --- a/cv_bridge/include/cv_bridge/rgb_colors.h +++ b/cv_bridge/include/cv_bridge/rgb_colors.h @@ -36,7 +36,7 @@ #ifndef CV_BRIDGE_RGB_COLORS_H_ #define CV_BRIDGE_RGB_COLORS_H_ -#include +#include namespace cv_bridge diff --git a/cv_bridge/package.xml b/cv_bridge/package.xml index e2dae4cdc..bb6c7346f 100644 --- a/cv_bridge/package.xml +++ b/cv_bridge/package.xml @@ -1,6 +1,6 @@ cv_bridge - 1.13.0 + 1.16.2 This contains CvBridge, which converts between ROS Image messages and OpenCV images. @@ -21,21 +21,21 @@ boost libopencv-dev - python - python-opencv + python3 + python3-opencv rosconsole sensor_msgs boost libopencv-dev - python - python-opencv + python3 + python3-opencv rosconsole libopencv-dev sensor_msgs rostest - python-numpy + python3-numpy dvipng diff --git a/cv_bridge/python/cv_bridge/core.py b/cv_bridge/python/cv_bridge/core.py index a47bbd764..423aa5466 100644 --- a/cv_bridge/python/cv_bridge/core.py +++ b/cv_bridge/python/cv_bridge/core.py @@ -124,7 +124,7 @@ def compressed_imgmsg_to_cv2(self, cmprs_img_msg, desired_encoding = "passthroug str_msg = cmprs_img_msg.data buf = np.ndarray(shape=(1, len(str_msg)), dtype=np.uint8, buffer=cmprs_img_msg.data) - im = cv2.imdecode(buf, cv2.IMREAD_ANYCOLOR) + im = cv2.imdecode(buf, cv2.IMREAD_UNCHANGED) if desired_encoding == "passthrough": return im @@ -167,8 +167,12 @@ def imgmsg_to_cv2(self, img_msg, desired_encoding = "passthrough"): im = np.ndarray(shape=(img_msg.height, img_msg.width), dtype=dtype, buffer=img_msg.data) else: - im = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels), - dtype=dtype, buffer=img_msg.data) + if(type(img_msg.data) == str): + im = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels), + dtype=dtype, buffer=img_msg.data.encode()) + else: + im = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels), + dtype=dtype, buffer=img_msg.data) # If the byt order is different between the message and the system. if img_msg.is_bigendian == (sys.byteorder == 'little'): im = im.byteswap().newbyteorder() @@ -222,7 +226,7 @@ def cv2_to_compressed_imgmsg(self, cvim, dst_format = "jpg"): return cmprs_img_msg - def cv2_to_imgmsg(self, cvim, encoding = "passthrough"): + def cv2_to_imgmsg(self, cvim, encoding = "passthrough", header = None): """ Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message. @@ -231,6 +235,7 @@ def cv2_to_imgmsg(self, cvim, encoding = "passthrough"): * ``"passthrough"`` * one of the standard strings in sensor_msgs/image_encodings.h + :param header: A std_msgs.msg.Header message :rtype: A sensor_msgs.msg.Image message :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding`` @@ -247,6 +252,8 @@ def cv2_to_imgmsg(self, cvim, encoding = "passthrough"): img_msg = sensor_msgs.msg.Image() img_msg.height = cvim.shape[0] img_msg.width = cvim.shape[1] + if header is not None: + img_msg.header = header if len(cvim.shape) < 3: cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1) else: diff --git a/cv_bridge/setup.py b/cv_bridge/setup.py index 65ae95d67..f8812a205 100644 --- a/cv_bridge/setup.py +++ b/cv_bridge/setup.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -from distutils.core import setup +from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup() diff --git a/cv_bridge/src/CMakeLists.txt b/cv_bridge/src/CMakeLists.txt index 37ba30eea..d1046922c 100644 --- a/cv_bridge/src/CMakeLists.txt +++ b/cv_bridge/src/CMakeLists.txt @@ -4,7 +4,11 @@ add_library(${PROJECT_NAME} cv_bridge.cpp rgb_colors.cpp) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) if(NOT ANDROID) # add a Boost Python library @@ -32,24 +36,30 @@ if (PYTHON_VERSION_MAJOR VERSION_EQUAL 3) add_definitions(-DPYTHON3) endif() -if (OpenCV_VERSION_MAJOR VERSION_EQUAL 3) -add_library(${PROJECT_NAME}_boost module.cpp module_opencv3.cpp) -else() -add_library(${PROJECT_NAME}_boost module.cpp module_opencv2.cpp) -endif() +add_library(${PROJECT_NAME}_boost module.cpp module_opencv4.cpp) target_link_libraries(${PROJECT_NAME}_boost ${Boost_LIBRARIES} ${catkin_LIBRARIES} - ${PYTHON_LIBRARIES} ${PROJECT_NAME} ) +if(NOT APPLE) + target_link_libraries(${PROJECT_NAME}_boost ${PYTHON_LIBRARIES}) +endif() + set_target_properties(${PROJECT_NAME}_boost PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME}/boost/ + RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}/${PROJECT_NAME}/boost/ PREFIX "" ) if(APPLE) set_target_properties(${PROJECT_NAME}_boost PROPERTIES SUFFIX ".so") + set_target_properties(${PROJECT_NAME}_boost PROPERTIES + LINK_FLAGS "-undefined dynamic_lookup") +endif() +if(MSVC) + set_target_properties(${PROJECT_NAME}_boost PROPERTIES + SUFFIX ".pyd") endif() install(TARGETS ${PROJECT_NAME}_boost DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}/boost/) diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp index 109440e37..44f23a322 100644 --- a/cv_bridge/src/cv_bridge.cpp +++ b/cv_bridge/src/cv_bridge.cpp @@ -481,7 +481,7 @@ void CvImage::toCompressedImageMsg(sensor_msgs::CompressedImage& ros_image, cons { ros_image.header = header; cv::Mat image; - if (encoding == enc::BGR8 || encoding == enc::BGRA8) + if (encoding == enc::BGR8 || encoding == enc::BGRA8 || encoding == enc::MONO8 || encoding == enc::MONO16) { image = this->image; } @@ -519,19 +519,24 @@ CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source, const std::strin // Loads as BGR or BGRA. const cv::Mat rgb_a = cv::imdecode(in, cv::IMREAD_UNCHANGED); - switch (rgb_a.channels()) - { - case 4: - return toCvCopyImpl(rgb_a, source.header, enc::BGRA8, encoding); - break; - case 3: - return toCvCopyImpl(rgb_a, source.header, enc::BGR8, encoding); - break; - case 1: - return toCvCopyImpl(rgb_a, source.header, enc::MONO8, encoding); - break; - default: - return CvImagePtr(); + if (encoding != enc::MONO16) { + switch (rgb_a.channels()) + { + case 4: + return toCvCopyImpl(rgb_a, source.header, enc::BGRA8, encoding); + break; + case 3: + return toCvCopyImpl(rgb_a, source.header, enc::BGR8, encoding); + break; + case 1: + return toCvCopyImpl(rgb_a, source.header, enc::MONO8, encoding); + break; + default: + return CvImagePtr(); + } + } + else { + return toCvCopyImpl(rgb_a, source.header, enc::MONO16, encoding); } } @@ -677,9 +682,9 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source, else if (source->encoding == "CV_8UC4") source_typed->encoding = enc::BGRA8; else if (source->encoding == "CV_16UC3") - source_typed->encoding = enc::BGR8; + source_typed->encoding = enc::BGR16; else if (source->encoding == "CV_16UC4") - source_typed->encoding = enc::BGRA8; + source_typed->encoding = enc::BGRA16; // If no conversion is needed, don't convert if (source_typed->encoding == encoding) diff --git a/cv_bridge/src/module.hpp b/cv_bridge/src/module.hpp index 92153ac80..aa8b5f6c5 100644 --- a/cv_bridge/src/module.hpp +++ b/cv_bridge/src/module.hpp @@ -33,16 +33,10 @@ int convert_to_CvMat2(const PyObject* o, cv::Mat& m); PyObject* pyopencv_from(const cv::Mat& m); -#if PYTHON3 -static int do_numpy_import( ) +static void * do_numpy_import( ) { import_array( ); + return nullptr; } -#else -static void do_numpy_import( ) -{ - import_array( ); -} -#endif #endif diff --git a/cv_bridge/src/module_opencv2.cpp b/cv_bridge/src/module_opencv2.cpp deleted file mode 100644 index 9f0752bed..000000000 --- a/cv_bridge/src/module_opencv2.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -#include "module.hpp" - -using namespace cv; - -// These are sucky, sketchy versions of the real things in OpenCV Python, -// inferior in every way. - -static int failmsg(const char *fmt, ...) -{ - char str[1000]; - - va_list ap; - va_start(ap, fmt); - vsnprintf(str, sizeof(str), fmt, ap); - va_end(ap); - - PyErr_SetString(PyExc_TypeError, str); - return 0; -} - -static PyObject* opencv_error = 0; - -class PyAllowThreads -{ -public: - PyAllowThreads() : _state(PyEval_SaveThread()) {} - ~PyAllowThreads() - { - PyEval_RestoreThread(_state); - } -private: - PyThreadState* _state; -}; - -#define ERRWRAP2(expr) \ -try \ -{ \ - PyAllowThreads allowThreads; \ - expr; \ -} \ -catch (const cv::Exception &e) \ -{ \ - PyErr_SetString(opencv_error, e.what()); \ - return 0; \ -} - -// Taken from http://stackoverflow.com/questions/19136944/call-c-opencv-functions-from-python-send-a-cv-mat-to-c-dll-which-is-usi - - -static size_t REFCOUNT_OFFSET = ( size_t )&((( PyObject* )0)->ob_refcnt ) + -( 0x12345678 != *( const size_t* )"\x78\x56\x34\x12\0\0\0\0\0" )*sizeof( int ); - - -static inline PyObject* pyObjectFromRefcount( const int* refcount ) -{ -return ( PyObject* )(( size_t )refcount - REFCOUNT_OFFSET ); -} - -static inline int* refcountFromPyObject( const PyObject* obj ) -{ -return ( int* )(( size_t )obj + REFCOUNT_OFFSET ); -} - -class NumpyAllocator : public cv::MatAllocator -{ -public: -NumpyAllocator( ) { } -~NumpyAllocator( ) { } - -void allocate( int dims, const int* sizes, int type, int*& refcount, -uchar*& datastart, uchar*& data, size_t* step ); - -void deallocate( int* refcount, uchar* datastart, uchar* data ); -}; - -void NumpyAllocator::allocate( int dims, const int* sizes, int type, int*& refcount, uchar*& datastart, uchar*& data, size_t* step ) -{ - int depth = CV_MAT_DEPTH( type ); - int cn = CV_MAT_CN( type ); - const int f = ( int )( sizeof( size_t )/8 ); - int typenum = depth == CV_8U ? NPY_UBYTE : depth == CV_8S ? NPY_BYTE : - depth == CV_16U ? NPY_USHORT : depth == CV_16S ? NPY_SHORT : - depth == CV_32S ? NPY_INT : depth == CV_32F ? NPY_FLOAT : - depth == CV_64F ? NPY_DOUBLE : f*NPY_ULONGLONG + (f^1)*NPY_UINT; - int i; - npy_intp _sizes[CV_MAX_DIM+1]; - for( i = 0; i < dims; i++ ) - _sizes[i] = sizes[i]; - if( cn > 1 ) - { - /*if( _sizes[dims-1] == 1 ) - _sizes[dims-1] = cn; - else*/ - _sizes[dims++] = cn; - } - PyObject* o = PyArray_SimpleNew( dims, _sizes, typenum ); - if( !o ) - CV_Error_(CV_StsError, ("The numpy array of typenum=%d, ndims=%d can not be created", typenum, dims)); - refcount = refcountFromPyObject(o); - npy_intp* _strides = PyArray_STRIDES((PyArrayObject*) o); - for( i = 0; i < dims - (cn > 1); i++ ) - step[i] = (size_t)_strides[i]; - datastart = data = (uchar*)PyArray_DATA((PyArrayObject*)o); - -} - -void NumpyAllocator::deallocate( int* refcount, uchar* datastart, uchar* data ) -{ - if( !refcount ) - return; - PyObject* o = pyObjectFromRefcount(refcount); - Py_INCREF(o); - Py_DECREF(o); -} - -// Declare the object -NumpyAllocator g_numpyAllocator; - -int convert_to_CvMat2(const PyObject* o, cv::Mat& m) -{ - // to avoid PyArray_Check() to crash even with valid array - do_numpy_import(); - - if(!o || o == Py_None) - { - if( !m.data ) - m.allocator = &g_numpyAllocator; - return true; - } - - if( !PyArray_Check(o) ) - { - failmsg("Not a numpy array"); - return false; - } - - // NPY_LONG (64 bit) is converted to CV_32S (32 bit) - int typenum = PyArray_TYPE((PyArrayObject*) o); - int type = typenum == NPY_UBYTE ? CV_8U : typenum == NPY_BYTE ? CV_8S : - typenum == NPY_USHORT ? CV_16U : typenum == NPY_SHORT ? CV_16S : - typenum == NPY_INT || typenum == NPY_LONG ? CV_32S : - typenum == NPY_FLOAT ? CV_32F : - typenum == NPY_DOUBLE ? CV_64F : -1; - - if( type < 0 ) - { - failmsg("data type = %d is not supported", typenum); - return false; - } - - int ndims = PyArray_NDIM((PyArrayObject*) o); - if(ndims >= CV_MAX_DIM) - { - failmsg("dimensionality (=%d) is too high", ndims); - return false; - } - - int size[CV_MAX_DIM+1]; - size_t step[CV_MAX_DIM+1], elemsize = CV_ELEM_SIZE1(type); - const npy_intp* _sizes = PyArray_DIMS((PyArrayObject*) o); - const npy_intp* _strides = PyArray_STRIDES((PyArrayObject*) o); - bool transposed = false; - - for(int i = 0; i < ndims; i++) - { - size[i] = (int)_sizes[i]; - step[i] = (size_t)_strides[i]; - } - - if( ndims == 0 || step[ndims-1] > elemsize ) { - size[ndims] = 1; - step[ndims] = elemsize; - ndims++; - } - - if( ndims >= 2 && step[0] < step[1] ) - { - std::swap(size[0], size[1]); - std::swap(step[0], step[1]); - transposed = true; - } - - if( ndims == 3 && size[2] <= CV_CN_MAX && step[1] == elemsize*size[2] ) - { - ndims--; - type |= CV_MAKETYPE(0, size[2]); - } - - if( ndims > 2 ) - { - failmsg("more than 2 dimensions"); - return false; - } - - m = cv::Mat(ndims, size, type, PyArray_DATA((PyArrayObject*) o), step); - - if( m.data ) - { - m.refcount = refcountFromPyObject(o); - m.addref(); // protect the original numpy array from deallocation - // (since Mat destructor will decrement the reference counter) - }; - m.allocator = &g_numpyAllocator; - - if( transposed ) - { - cv::Mat tmp; - tmp.allocator = &g_numpyAllocator; - transpose(m, tmp); - m = tmp; - } - return true; -} - -PyObject* pyopencv_from(const Mat& m) -{ - if( !m.data ) - Py_RETURN_NONE; - Mat temp, *p = (Mat*)&m; - if(!p->refcount || p->allocator != &g_numpyAllocator) - { - temp.allocator = &g_numpyAllocator; - ERRWRAP2(m.copyTo(temp)); - p = &temp; - } - p->addref(); - return pyObjectFromRefcount(p->refcount); -} diff --git a/cv_bridge/src/module_opencv3.cpp b/cv_bridge/src/module_opencv4.cpp similarity index 97% rename from cv_bridge/src/module_opencv3.cpp rename to cv_bridge/src/module_opencv4.cpp index 68c1b2007..c4ec5a2a7 100644 --- a/cv_bridge/src/module_opencv3.cpp +++ b/cv_bridge/src/module_opencv4.cpp @@ -90,6 +90,11 @@ static PyObject* failmsgp(const char *fmt, ...) class NumpyAllocator : public MatAllocator { +#if CV_MAJOR_VERSION == 3 +protected: + typedef int AccessFlag; +#endif + public: NumpyAllocator() { stdAllocator = Mat::getStdAllocator(); } ~NumpyAllocator() {} @@ -107,7 +112,7 @@ class NumpyAllocator : public MatAllocator return u; } - UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, int flags, UMatUsageFlags usageFlags) const + UMatData* allocate(int dims0, const int* sizes, int type, void* data, size_t* step, AccessFlag flags, UMatUsageFlags usageFlags) const { if( data != 0 ) { @@ -136,7 +141,7 @@ class NumpyAllocator : public MatAllocator return allocate(o, dims0, sizes, type, step); } - bool allocate(UMatData* u, int accessFlags, UMatUsageFlags usageFlags) const + bool allocate(UMatData* u, AccessFlag accessFlags, UMatUsageFlags usageFlags) const { return stdAllocator->allocate(u, accessFlags, usageFlags); } diff --git a/cv_bridge/src/rgb_colors.cpp b/cv_bridge/src/rgb_colors.cpp index ca2eaaa3f..67b08eac7 100644 --- a/cv_bridge/src/rgb_colors.cpp +++ b/cv_bridge/src/rgb_colors.cpp @@ -34,7 +34,6 @@ *********************************************************************/ #include "cv_bridge/rgb_colors.h" -#include namespace cv_bridge diff --git a/cv_bridge/test/conversions.py b/cv_bridge/test/conversions.py index bde1380f3..bddc17811 100644 --- a/cv_bridge/test/conversions.py +++ b/cv_bridge/test/conversions.py @@ -49,7 +49,7 @@ def test_encode_decode_cv2_compressed(self): # from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) # NOTE: remove jp2(a.k.a JPEG2000) as its JASPER codec is disabled within Ubuntu opencv library # due to security issues, but it works once you rebuild your opencv library with JASPER enabled - formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm", + formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "sr", "ras", "tif", "tiff"] # this formats rviz is not support cvb_en = CvBridge() diff --git a/cv_bridge/test/enumerants.py b/cv_bridge/test/enumerants.py index bdcc7a8ef..dbb336325 100644 --- a/cv_bridge/test/enumerants.py +++ b/cv_bridge/test/enumerants.py @@ -22,7 +22,7 @@ def test_enumerants_cv2(self): bridge_ = CvBridge() cvim = bridge_.imgmsg_to_cv2(img_msg, "rgb8") import sys - self.assertRaises(sys.getrefcount(cvim) == 2) + self.assert_(sys.getrefcount(cvim) == 2) # A 3 channel image cannot be sent as an rgba8 self.assertRaises(CvBridgeError, lambda: bridge_.cv2_to_imgmsg(cvim, "rgba8")) @@ -31,9 +31,9 @@ def test_enumerants_cv2(self): bridge_.cv2_to_imgmsg(cvim, "rgb8") bridge_.cv2_to_imgmsg(cvim, "bgr8") - self.assertRaises(getCvType("32FC4") == cv2.CV_8UC4) - self.assertRaises(getCvType("8UC1") == cv2.CV_8UC1) - self.assertRaises(getCvType("8U") == cv2.CV_8UC1) + self.assert_(getCvType("32FC4") == cv2.CV_32FC4) + self.assert_(getCvType("8UC1") == cv2.CV_8UC1) + self.assert_(getCvType("8U") == cv2.CV_8UC1) def test_numpy_types(self): import cv2 diff --git a/cv_bridge/test/python_bindings.py b/cv_bridge/test/python_bindings.py index 6dd320905..3d94f4e56 100644 --- a/cv_bridge/test/python_bindings.py +++ b/cv_bridge/test/python_bindings.py @@ -10,15 +10,15 @@ def test_cvtColorForDisplay(): height, width = label.shape[:2] label_value = 0 grid_num_y, grid_num_x = 3, 4 - for grid_row in xrange(grid_num_y): + for grid_row in range(grid_num_y): grid_size_y = height / grid_num_y min_y = grid_size_y * grid_row max_y = min_y + grid_size_y - for grid_col in xrange(grid_num_x): + for grid_col in range(grid_num_x): grid_size_x = width / grid_num_x min_x = grid_size_x * grid_col max_x = min_x + grid_size_x - label[min_y:max_y, min_x:max_x] = label_value + label[int(min_y):int(max_y), int(min_x):int(max_x)] = label_value label_value += 1 label_viz = cv_bridge.cvtColorForDisplay(label, '32SC1', 'bgr8') assert_equal(label_viz.dtype, np.uint8) diff --git a/image_geometry/CHANGELOG.rst b/image_geometry/CHANGELOG.rst index 6b19c9b2b..ed6cb5ce0 100644 --- a/image_geometry/CHANGELOG.rst +++ b/image_geometry/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package image_geometry ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.16.2 (2022-10-03) +------------------- +* Add fovX and fovY functions in python, cpp, also some typo fixes (`#428 `_) +* Contributors: Chris Thierauf, Kenji Brameld + +1.16.1 (2022-09-06) +------------------- + +1.16.0 (2021-11-23) +------------------- +* substituted missing sphinx extension (`#417 `_) +* Fix rectifyRoi when used with binning and/or ROI (`#378 `_) +* Implement unrectifyImage() (`#359 `_) +* Add equidistant distortion model (`#358 `_) +* Optimize includes (`#354 `_) +* Contributors: Markus Vieth, Martin Günther, Paddy + +1.15.0 (2020-05-19) +------------------- + +1.14.0 (2020-04-06) +------------------- +* Noetic release (`#323 `_) +* update CMakeLists.txt for Windows build environment (`#265 `_) +* add DLL import/export macros (`#266 `_) +* Contributors: Alejandro Hernández Cordero, James Xu + 1.13.0 (2018-04-30) ------------------- * Use rosdep OpenCV and not ROS one. diff --git a/image_geometry/CMakeLists.txt b/image_geometry/CMakeLists.txt index 9169c0484..c397aaffb 100644 --- a/image_geometry/CMakeLists.txt +++ b/image_geometry/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.0.2) project(image_geometry) find_package(catkin REQUIRED sensor_msgs) @@ -6,7 +6,7 @@ find_package(OpenCV REQUIRED) catkin_package(CATKIN_DEPENDS sensor_msgs DEPENDS OpenCV - INCLUDE_DIRS include + INCLUDE_DIRS include ${OpenCV_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} ) @@ -24,10 +24,11 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}/ ) +# install library install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) # add tests diff --git a/image_geometry/doc/conf.py b/image_geometry/doc/conf.py index a30006fc0..1cb492d09 100644 --- a/image_geometry/doc/conf.py +++ b/image_geometry/doc/conf.py @@ -22,7 +22,7 @@ # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. -extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.imgmath'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] diff --git a/image_geometry/include/image_geometry/exports.h b/image_geometry/include/image_geometry/exports.h new file mode 100644 index 000000000..34bafadfd --- /dev/null +++ b/image_geometry/include/image_geometry/exports.h @@ -0,0 +1,18 @@ +#ifndef IMAGE_GEOMETRY_EXPORTS_H +#define IMAGE_GEOMETRY_EXPORTS_H + +#include + +// Import/export for windows dll's and visibility for gcc shared libraries. + +#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries + #ifdef image_geometry_EXPORTS // we are building a shared lib/dll + #define IMAGE_GEOMETRY_DECL ROS_HELPER_EXPORT + #else // we are using shared lib/dll + #define IMAGE_GEOMETRY_DECL ROS_HELPER_IMPORT + #endif +#else // ros is being built around static libraries + #define IMAGE_GEOMETRY_DECL +#endif + +#endif // IMAGE_GEOMETRY_EXPORTS_H diff --git a/image_geometry/include/image_geometry/pinhole_camera_model.h b/image_geometry/include/image_geometry/pinhole_camera_model.h index d5dfdb65d..e256ab1ce 100644 --- a/image_geometry/include/image_geometry/pinhole_camera_model.h +++ b/image_geometry/include/image_geometry/pinhole_camera_model.h @@ -2,11 +2,12 @@ #define IMAGE_GEOMETRY_PINHOLE_CAMERA_MODEL_H #include -#include +#include #include -#include #include #include +#include +#include "exports.h" namespace image_geometry { @@ -20,7 +21,7 @@ class Exception : public std::runtime_error * \brief Simplifies interpreting images geometrically using the parameters from * sensor_msgs/CameraInfo. */ -class PinholeCameraModel +class IMAGE_GEOMETRY_DECL PinholeCameraModel { public: @@ -107,6 +108,7 @@ class PinholeCameraModel * \return 3d ray passing through (u,v) */ cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const; + cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const; /** * \brief Rectify a raw camera image. @@ -124,11 +126,13 @@ class PinholeCameraModel * \brief Compute the rectified image coordinates of a pixel in the raw image. */ cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const; + cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const; /** * \brief Compute the raw image coordinates of a pixel in the rectified image. */ cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const; + cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const; /** * \brief Compute the rectified ROI best fitting a raw ROI. @@ -205,6 +209,16 @@ class PinholeCameraModel */ double Ty() const; + /** + * \brief Returns the horizontal field of view in radians. + */ + double fovX() const; + + /** + * \brief Returns the vertical field of view in radians. + */ + double fovY() const; + /** * \brief Returns the number of columns in each bin. */ @@ -214,7 +228,7 @@ class PinholeCameraModel * \brief Returns the number of rows in each bin. */ uint32_t binningY() const; - + /** * \brief Compute delta u, given Z and delta X in Cartesian space. * @@ -278,6 +292,7 @@ class PinholeCameraModel #endif void initRectificationMaps() const; + void initUnrectificationMaps() const; friend class StereoCameraModel; }; @@ -311,6 +326,13 @@ inline double PinholeCameraModel::cy() const { return P_(1,2); } inline double PinholeCameraModel::Tx() const { return P_(0,3); } inline double PinholeCameraModel::Ty() const { return P_(1,3); } +inline double PinholeCameraModel::fovX() const { + return 2 * atan(rawRoi().width / (2 * fx())); +} +inline double PinholeCameraModel::fovY() const { + return 2 * atan(rawRoi().height / (2 * fy())); +} + inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; } inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; } diff --git a/image_geometry/include/image_geometry/stereo_camera_model.h b/image_geometry/include/image_geometry/stereo_camera_model.h index a4f5880cd..f95271476 100644 --- a/image_geometry/include/image_geometry/stereo_camera_model.h +++ b/image_geometry/include/image_geometry/stereo_camera_model.h @@ -2,6 +2,7 @@ #define IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H #include "image_geometry/pinhole_camera_model.h" +#include "exports.h" namespace image_geometry { @@ -9,7 +10,7 @@ namespace image_geometry { * \brief Simplifies interpreting stereo image pairs geometrically using the * parameters from the left and right sensor_msgs/CameraInfo. */ -class StereoCameraModel +class IMAGE_GEOMETRY_DECL StereoCameraModel { public: StereoCameraModel(); diff --git a/image_geometry/package.xml b/image_geometry/package.xml index 79540bad5..83224ad31 100644 --- a/image_geometry/package.xml +++ b/image_geometry/package.xml @@ -1,6 +1,6 @@ image_geometry - 1.13.0 + 1.16.2 `image_geometry` contains C++ and Python libraries for interpreting images geometrically. It interfaces the calibration parameters in sensor_msgs/CameraInfo diff --git a/image_geometry/setup.py b/image_geometry/setup.py index 407ce1e5e..3a52c4bd3 100644 --- a/image_geometry/setup.py +++ b/image_geometry/setup.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -from distutils.core import setup +from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup() diff --git a/image_geometry/src/image_geometry/cameramodels.py b/image_geometry/src/image_geometry/cameramodels.py index 139c95c38..ba0c2d00a 100644 --- a/image_geometry/src/image_geometry/cameramodels.py +++ b/image_geometry/src/image_geometry/cameramodels.py @@ -1,7 +1,4 @@ -import array - import cv2 -import sensor_msgs.msg import math import copy import numpy @@ -113,7 +110,7 @@ def project3dToPixel(self, point): Returns the rectified pixel coordinates (u, v) of the 3D point, using the camera :math:`P` matrix. - This is the inverse of :meth:`projectPixelTo3dRay`. + This is the inverse of :math:`projectPixelTo3dRay`. """ src = mkmat(4, 1, [point[0], point[1], point[2], 1.0]) dst = self.P * src @@ -132,7 +129,7 @@ def projectPixelTo3dRay(self, uv): Returns the unit vector which passes from the camera center to through rectified pixel (u, v), using the camera :math:`P` matrix. - This is the inverse of :meth:`project3dToPixel`. + This is the inverse of :math:`project3dToPixel`. """ x = (uv[0] - self.cx()) / self.fx() y = (uv[1] - self.cy()) / self.fy() @@ -151,13 +148,12 @@ def getDeltaU(self, deltaX, Z): :rtype: float Compute delta u, given Z and delta X in Cartesian space. - For given Z, this is the inverse of :meth:`getDeltaX`. + For given Z, this is the inverse of :math:`getDeltaX`. """ - fx = self.P[0, 0] if Z == 0: return float('inf') else: - return fx * deltaX / Z + return self.fx() * deltaX / Z def getDeltaV(self, deltaY, Z): """ @@ -168,13 +164,12 @@ def getDeltaV(self, deltaY, Z): :rtype: float Compute delta v, given Z and delta Y in Cartesian space. - For given Z, this is the inverse of :meth:`getDeltaY`. + For given Z, this is the inverse of :math:`getDeltaY`. """ - fy = self.P[1, 1] if Z == 0: return float('inf') else: - return fy * deltaY / Z + return self.fy() * deltaY / Z def getDeltaX(self, deltaU, Z): """ @@ -185,10 +180,9 @@ def getDeltaX(self, deltaU, Z): :rtype: float Compute delta X, given Z in cartesian space and delta u in pixels. - For given Z, this is the inverse of :meth:`getDeltaU`. + For given Z, this is the inverse of :math:`getDeltaU`. """ - fx = self.P[0, 0] - return Z * deltaU / fx + return Z * deltaU / self.fx() def getDeltaY(self, deltaV, Z): """ @@ -199,10 +193,9 @@ def getDeltaY(self, deltaV, Z): :rtype: float Compute delta Y, given Z in cartesian space and delta v in pixels. - For given Z, this is the inverse of :meth:`getDeltaV`. + For given Z, this is the inverse of :math:`getDeltaV`. """ - fy = self.P[1, 1] - return Z * deltaV / fy + return Z * deltaV / self.fy() def fullResolution(self): """Returns the full resolution of the camera""" @@ -211,18 +204,23 @@ def fullResolution(self): def intrinsicMatrix(self): """ Returns :math:`K`, also called camera_matrix in cv docs """ return self.K + def distortionCoeffs(self): """ Returns :math:`D` """ return self.D + def rotationMatrix(self): """ Returns :math:`R` """ return self.R + def projectionMatrix(self): """ Returns :math:`P` """ return self.P + def fullIntrinsicMatrix(self): """ Return the original camera matrix for full resolution """ return self.full_K + def fullProjectionMatrix(self): """ Return the projection matrix for full resolution """ return self.full_P @@ -230,12 +228,15 @@ def fullProjectionMatrix(self): def cx(self): """ Returns x center """ return self.P[0,2] + def cy(self): """ Returns y center """ return self.P[1,2] + def fx(self): """ Returns x focal length """ return self.P[0,0] + def fy(self): """ Returns y focal length """ return self.P[1,1] @@ -248,6 +249,18 @@ def Ty(self): """ Return the y-translation term of the projection matrix """ return self.P[1,3] + def fovX(self): + """ Returns the horizontal field of view in radians. + Horizontal FoV = 2 * arctan((width) / (2 * Horizontal Focal Length) ) + """ + return 2 * math.atan(self.width / (2 * self.fx())) + + def fovY(self): + """ Returns the vertical field of view in radians. + Vertical FoV = 2 * arctan((height) / (2 * Vertical Focal Length) ) + """ + return 2 * math.atan(self.height / (2 * self.fy())) + def tfFrame(self): """ Returns the tf frame name - a string - of the camera. This is the frame of the :class:`sensor_msgs.msg.CameraInfo` message. @@ -278,8 +291,8 @@ def fromCameraInfo(self, left_msg, right_msg): # [ 0, Fy, Cy, 0 ] # [ 0, 0, 1, 0 ] + assert self.right.P is not None fx = self.right.P[0, 0] - fy = self.right.P[1, 1] cx = self.right.P[0, 2] cy = self.right.P[1, 2] tx = -self.right.P[0, 3] / fx @@ -314,7 +327,7 @@ def project3dToPixel(self, point): Returns the rectified pixel coordinates (u, v) of the 3D point, for each camera, as ((u_left, v_left), (u_right, v_right)) using the cameras' :math:`P` matrices. - This is the inverse of :meth:`projectPixelTo3d`. + This is the inverse of :math:`projectPixelTo3d`. """ l = self.left.project3dToPixel(point) r = self.right.project3dToPixel(point) @@ -329,7 +342,7 @@ def projectPixelTo3d(self, left_uv, disparity): Returns the 3D point (x, y, z) for the given pixel position, using the cameras' :math:`P` matrices. - This is the inverse of :meth:`project3dToPixel`. + This is the inverse of :math:`project3dToPixel`. Note that a disparity of zero implies that the 3D point is at infinity. """ @@ -350,12 +363,13 @@ def getZ(self, disparity): :type disparity: float Returns the depth at which a point is observed with a given disparity. - This is the inverse of :meth:`getDisparity`. + This is the inverse of :math:`getDisparity`. Note that a disparity of zero implies Z is infinite. """ if disparity == 0: return float('inf') + assert self.right.P is not None Tx = -self.right.P[0, 3] return Tx / disparity @@ -365,9 +379,10 @@ def getDisparity(self, Z): :type Z: float Returns the disparity observed for a point at depth Z. - This is the inverse of :meth:`getZ`. + This is the inverse of :math:`getZ`. """ if Z == 0: return float('inf') + assert self.right.P is not None Tx = -self.right.P[0, 3] return Tx / Z diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index 69f6f2c9c..f24bb4df0 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/image_geometry/src/pinhole_camera_model.cpp @@ -1,5 +1,6 @@ #include "image_geometry/pinhole_camera_model.h" #include +#include #ifdef BOOST_SHARED_PTR_HPP_INCLUDED #include #endif @@ -7,10 +8,12 @@ namespace image_geometry { enum DistortionState { NONE, CALIBRATED, UNKNOWN }; +enum DistortionModel { EQUIDISTANT, PLUMB_BOB_OR_RATIONAL_POLYNOMIAL, UNKNOWN_MODEL }; struct PinholeCameraModel::Cache { DistortionState distortion_state; + DistortionModel distortion_model; cv::Mat_ K_binned, P_binned; // Binning applied, but not cropping @@ -20,12 +23,22 @@ struct PinholeCameraModel::Cache mutable bool reduced_maps_dirty; mutable cv::Mat reduced_map1, reduced_map2; + mutable bool unrectify_full_maps_dirty; + mutable cv::Mat unrectify_full_map1, unrectify_full_map2; + + mutable bool unrectify_reduced_maps_dirty; + mutable cv::Mat unrectify_reduced_map1, unrectify_reduced_map2; + mutable bool rectified_roi_dirty; mutable cv::Rect rectified_roi; Cache() - : full_maps_dirty(true), + : distortion_state(UNKNOWN), + distortion_model(UNKNOWN_MODEL), + full_maps_dirty(true), reduced_maps_dirty(true), + unrectify_full_maps_dirty(true), + unrectify_reduced_maps_dirty(true), rectified_roi_dirty(true) { } @@ -107,7 +120,7 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg) // Update any parameters that have changed. The full rectification maps are // invalidated by any change in the calibration parameters OR binning. - bool &full_dirty = cache_->full_maps_dirty; + bool full_dirty = false; full_dirty |= update(msg.height, cam_info_.height); full_dirty |= update(msg.width, cam_info_.width); full_dirty |= update(msg.distortion_model, cam_info_.distortion_model); @@ -117,22 +130,29 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg) full_dirty |= updateMat(msg.P, cam_info_.P, P_full_); full_dirty |= update(binning_x, cam_info_.binning_x); full_dirty |= update(binning_y, cam_info_.binning_y); + cache_->full_maps_dirty |= full_dirty; + cache_->unrectify_full_maps_dirty |= full_dirty; // The reduced rectification maps are invalidated by any of the above or a // change in ROI. - bool &reduced_dirty = cache_->reduced_maps_dirty; - reduced_dirty = full_dirty; + bool reduced_dirty = full_dirty; reduced_dirty |= update(roi.x_offset, cam_info_.roi.x_offset); reduced_dirty |= update(roi.y_offset, cam_info_.roi.y_offset); reduced_dirty |= update(roi.height, cam_info_.roi.height); reduced_dirty |= update(roi.width, cam_info_.roi.width); reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify); + cache_->reduced_maps_dirty |= reduced_dirty; + cache_->reduced_maps_dirty |= cache_->full_maps_dirty; + cache_->unrectify_reduced_maps_dirty |= reduced_dirty; + cache_->unrectify_reduced_maps_dirty |= cache_->unrectify_full_maps_dirty; + // As is the rectified ROI - cache_->rectified_roi_dirty = reduced_dirty; + cache_->rectified_roi_dirty |= reduced_dirty; // Figure out how to handle the distortion if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB || - cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) { + cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL || + cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) { // If any distortion coefficient is non-zero, then need to apply the distortion cache_->distortion_state = NONE; for (size_t i = 0; i < cam_info_.D.size(); ++i) @@ -147,6 +167,17 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg) else cache_->distortion_state = UNKNOWN; + // Get the distortion model, if supported + if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB || + cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) { + cache_->distortion_model = PLUMB_BOB_OR_RATIONAL_POLYNOMIAL; + } + else if(cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) { + cache_->distortion_model = EQUIDISTANT; + } + else + cache_->distortion_model = UNKNOWN_MODEL; + // If necessary, create new K_ and P_ adjusted for binning and ROI /// @todo Calculate and use rectified ROI bool adjust_binning = (binning_x > 1) || (binning_y > 1); @@ -253,7 +284,7 @@ cv::Rect PinholeCameraModel::rawRoi() const cv::Rect PinholeCameraModel::rectifiedRoi() const { assert( initialized() ); - + if (cache_->rectified_roi_dirty) { if (!cam_info_.roi.do_rectify) @@ -280,12 +311,24 @@ cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const } cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const +{ + return projectPixelTo3dRay(uv_rect, P_); +} + +cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const { assert( initialized() ); + const double& fx = P(0,0); + const double& fy = P(1,1); + const double& cx = P(0,2); + const double& cy = P(1,2); + const double& Tx = P(0,3); + const double& Ty = P(1,3); + cv::Point3d ray; - ray.x = (uv_rect.x - cx() - Tx()) / fx(); - ray.y = (uv_rect.y - cy() - Ty()) / fy(); + ray.x = (uv_rect.x - cx - Tx) / fx; + ray.y = (uv_rect.y - cy - Ty) / fy; ray.z = 1.0; return ray; } @@ -318,17 +361,32 @@ void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, { assert( initialized() ); - throw Exception("PinholeCameraModel::unrectifyImage is unimplemented."); - /// @todo Implement unrectifyImage() - // Similar to rectifyImage, but need to build separate set of inverse maps (raw->rectified)... - // - Build src_pt Mat with all the raw pixel coordinates (or do it one row at a time) - // - Do cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_) - // - Use convertMaps() to convert dst_pt to fast fixed-point maps - // - cv::remap(rectified, raw, ...) - // Need interpolation argument. Same caching behavior? + switch (cache_->distortion_state) { + case NONE: + rectified.copyTo(raw); + break; + case CALIBRATED: + initUnrectificationMaps(); + if (rectified.depth() == CV_32F || rectified.depth() == CV_64F) + { + cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits::quiet_NaN()); + } + else { + cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation); + } + break; + default: + assert(cache_->distortion_state == UNKNOWN); + throw Exception("Cannot call rectifyImage when distortion is unknown."); + } } cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const +{ + return rectifyPoint(uv_raw, K_, P_); +} + +cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const { assert( initialized() ); @@ -342,11 +400,27 @@ cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const cv::Point2f raw32 = uv_raw, rect32; const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x); cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x); - cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_); + + switch (cache_->distortion_model) { + case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL: + cv::undistortPoints(src_pt, dst_pt, K, D_, R_, P); + break; + case EQUIDISTANT: + cv::fisheye::undistortPoints(src_pt, dst_pt, K, D_, R_, P); + break; + default: + assert(cache_->distortion_model == UNKNOWN_MODEL); + throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT."); + } return rect32; } cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const +{ + return unrectifyPoint(uv_rect, K_, P_); +} + +cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const { assert( initialized() ); @@ -357,13 +431,24 @@ cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const assert(cache_->distortion_state == CALIBRATED); // Convert to a ray - cv::Point3d ray = projectPixelTo3dRay(uv_rect); + cv::Point3d ray = projectPixelTo3dRay(uv_rect, P); // Project the ray on the image cv::Mat r_vec, t_vec = cv::Mat_::zeros(3, 1); cv::Rodrigues(R_.t(), r_vec); std::vector image_point; - cv::projectPoints(std::vector(1, ray), r_vec, t_vec, K_, D_, image_point); + + switch (cache_->distortion_model) { + case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL: + cv::projectPoints(std::vector(1, ray), r_vec, t_vec, K, D_, image_point); + break; + case EQUIDISTANT: + cv::fisheye::projectPoints(std::vector(1, ray), image_point, r_vec, t_vec, K, D_); + break; + default: + assert(cache_->distortion_model == UNKNOWN_MODEL); + throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT."); + } return image_point[0]; } @@ -373,13 +458,14 @@ cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const assert( initialized() ); /// @todo Actually implement "best fit" as described by REP 104. - + // For now, just unrectify the four corners and take the bounding box. - cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y)); - cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y)); + // Since ROI is specified in unbinned coordinates (see REP-104), this has to use K_full_ and P_full_. + cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y), K_full_, P_full_); + cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y), K_full_, P_full_); cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, - roi_raw.y + roi_raw.height)); - cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height)); + roi_raw.y + roi_raw.height), K_full_, P_full_); + cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height), K_full_, P_full_); cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)), std::ceil (std::min(rect_tl.y, rect_tr.y))); @@ -394,7 +480,7 @@ cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const assert( initialized() ); /// @todo Actually implement "best fit" as described by REP 104. - + // For now, just unrectify the four corners and take the bounding box. cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y)); cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y)); @@ -414,7 +500,7 @@ void PinholeCameraModel::initRectificationMaps() const { /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary. /// Make sure we're handling that 100% correctly. - + if (cache_->full_maps_dirty) { // Create the full-size map at the binned resolution /// @todo Should binned resolution, K, P be part of public API? @@ -448,10 +534,21 @@ void PinholeCameraModel::initRectificationMaps() const P_binned(1,3) *= scale_y; } } - - // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap) - cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution, - CV_16SC2, cache_->full_map1, cache_->full_map2); + + switch (cache_->distortion_model) { + case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL: + // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap) + cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution, + CV_16SC2, cache_->full_map1, cache_->full_map2); + break; + case EQUIDISTANT: + cv::fisheye::initUndistortRectifyMap(K_binned,D_, R_, P_binned, binned_resolution, + CV_16SC2, cache_->full_map1, cache_->full_map2); + break; + default: + assert(cache_->distortion_model == UNKNOWN_MODEL); + throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT."); + } cache_->full_maps_dirty = false; } @@ -460,8 +557,8 @@ void PinholeCameraModel::initRectificationMaps() const cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset, cam_info_.roi.width, cam_info_.roi.height); if (roi.x != 0 || roi.y != 0 || - roi.height != (int)cam_info_.height || - roi.width != (int)cam_info_.width) { + (roi.height != 0 && roi.height != (int)cam_info_.height) || + (roi.width != 0 && roi.width != (int)cam_info_.width)) { // map1 contains integer (x,y) offsets, which we adjust by the ROI offset // map2 contains LUT index for subpixel interpolation, which we can leave as-is @@ -481,4 +578,84 @@ void PinholeCameraModel::initRectificationMaps() const } } +void PinholeCameraModel::initUnrectificationMaps() const +{ + /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary. + /// Make sure we're handling that 100% correctly. + + if (cache_->unrectify_full_maps_dirty) { + // Create the full-size map at the binned resolution + /// @todo Should binned resolution, K, P be part of public API? + cv::Size binned_resolution = fullResolution(); + binned_resolution.width /= binningX(); + binned_resolution.height /= binningY(); + + cv::Matx33d K_binned; + cv::Matx34d P_binned; + if (binningX() == 1 && binningY() == 1) { + K_binned = K_full_; + P_binned = P_full_; + } + else { + K_binned = K_full_; + P_binned = P_full_; + if (binningX() > 1) { + double scale_x = 1.0 / binningX(); + K_binned(0,0) *= scale_x; + K_binned(0,2) *= scale_x; + P_binned(0,0) *= scale_x; + P_binned(0,2) *= scale_x; + P_binned(0,3) *= scale_x; + } + if (binningY() > 1) { + double scale_y = 1.0 / binningY(); + K_binned(1,1) *= scale_y; + K_binned(1,2) *= scale_y; + P_binned(1,1) *= scale_y; + P_binned(1,2) *= scale_y; + P_binned(1,3) *= scale_y; + } + } + + cv::Mat float_map_x(binned_resolution.height, binned_resolution.width, CV_32FC1); + cv::Mat float_map_y(binned_resolution.height, binned_resolution.width, CV_32FC1); + for (size_t x = 0; x < binned_resolution.width; x++) { + for (size_t y = 0; y < binned_resolution.height; y++) { + cv::Point2f uv_raw(x, y), uv_rect; + uv_rect = rectifyPoint(uv_raw, K_binned, P_binned); + float_map_x.at(y, x) = uv_rect.x; + float_map_y.at(y, x) = uv_rect.y; + } + } + // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap) + convertMaps(float_map_x, float_map_y, cache_->unrectify_full_map1, cache_->unrectify_full_map2, CV_16SC2); + cache_->unrectify_full_maps_dirty = false; + } + + if (cache_->unrectify_reduced_maps_dirty) { + /// @todo Use rectified ROI + cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset, + cam_info_.roi.width, cam_info_.roi.height); + if (roi.x != 0 || roi.y != 0 || + (roi.height != 0 && roi.height != (int)cam_info_.height) || + (roi.width != 0 && roi.width != (int)cam_info_.width)) { + + // map1 contains integer (x,y) offsets, which we adjust by the ROI offset + // map2 contains LUT index for subpixel interpolation, which we can leave as-is + roi.x /= binningX(); + roi.y /= binningY(); + roi.width /= binningX(); + roi.height /= binningY(); + cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1(roi) - cv::Scalar(roi.x, roi.y); + cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2(roi); + } + else { + // Otherwise we're rectifying the full image + cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1; + cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2; + } + cache_->unrectify_reduced_maps_dirty = false; + } +} + } //namespace image_geometry diff --git a/image_geometry/src/stereo_camera_model.cpp b/image_geometry/src/stereo_camera_model.cpp index 062edb746..e66fdc057 100644 --- a/image_geometry/src/stereo_camera_model.cpp +++ b/image_geometry/src/stereo_camera_model.cpp @@ -1,4 +1,5 @@ #include "image_geometry/stereo_camera_model.h" +#include namespace image_geometry { diff --git a/image_geometry/test/CMakeLists.txt b/image_geometry/test/CMakeLists.txt index 6dba1e56a..7e4b07541 100644 --- a/image_geometry/test/CMakeLists.txt +++ b/image_geometry/test/CMakeLists.txt @@ -2,3 +2,6 @@ catkin_add_nosetests(directed.py) catkin_add_gtest(${PROJECT_NAME}-utest utest.cpp) target_link_libraries(${PROJECT_NAME}-utest ${PROJECT_NAME} ${OpenCV_LIBS}) + +catkin_add_gtest(${PROJECT_NAME}-utest-equi utest_equi.cpp) +target_link_libraries(${PROJECT_NAME}-utest-equi ${PROJECT_NAME} ${OpenCV_LIBS}) diff --git a/image_geometry/test/utest.cpp b/image_geometry/test/utest.cpp index 2589019d9..ee6eab3b3 100644 --- a/image_geometry/test/utest.cpp +++ b/image_geometry/test/utest.cpp @@ -4,7 +4,7 @@ /// @todo Tests with simple values (R = identity, D = 0, P = K or simple scaling) /// @todo Test projection functions for right stereo values, P(:,3) != 0 -/// @todo Tests for [un]rectifyImage +/// @todo Tests for rectifyImage /// @todo Tests using ROI, needs support from PinholeCameraModel /// @todo Tests for StereoCameraModel @@ -247,9 +247,214 @@ TEST_F(PinholeTest, rectifyIfCalibrated) model_.rectifyImage(distorted_image, rectified_image); error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); EXPECT_EQ(error, 0); +} + +void testUnrectifyImage(const sensor_msgs::CameraInfo& cam_info, const image_geometry::PinholeCameraModel& model) +{ + // test for unrectifyImage: call unrectifyImage, call unrectifyPoint in a loop, compare + + // prepare rectified_image + cv::Mat rectified_image(model.fullResolution(), CV_8UC3, cv::Scalar(0, 0, 0)); + + // draw a grid + const cv::Scalar color = cv::Scalar(255, 255, 255); + const int thickness = 7; + const int type = 8; + for (size_t y = 0; y <= rectified_image.rows; y += rectified_image.rows / 10) + { + cv::line(rectified_image, + cv::Point(0, y), cv::Point(cam_info.width, y), + color, type, thickness); + } + for (size_t x = 0; x <= rectified_image.cols; x += rectified_image.cols / 10) + { + cv::line(rectified_image, + cv::Point(x, 0), cv::Point(x, cam_info.height), + color, type, thickness); + } + + // restrict rectified_image to ROI and resize to new binning + rectified_image = rectified_image(model.rawRoi()); + cv::resize(rectified_image, rectified_image, cv::Size(), 1.0 / model.binningX(), 1.0 / model.binningY(), + cv::INTER_NEAREST); + + // unrectify image in one go using unrectifyImage + cv::Mat distorted_image; + // Just making this number up, maybe ought to be larger + // since a completely different image would be on the order of + // width * height * 255 = 78e6 + const double diff_threshold = 10000.0; + double error; + + // Test that unrectified image is sufficiently different + // using default distortion + model.unrectifyImage(rectified_image, distorted_image); + error = cv::norm(rectified_image, distorted_image, cv::NORM_L1); + // Just making this number up, maybe ought to be larger + EXPECT_GT(error, diff_threshold); + + // unrectify image pixel by pixel using unrectifyPoint + assert(rectified_image.type() == CV_8UC3); // need this for at to be correct + cv::Mat distorted_image_by_pixel = cv::Mat::zeros(rectified_image.size(), rectified_image.type()); + cv::Mat mask = cv::Mat::zeros(rectified_image.size(), CV_8UC1); + for (size_t y = 0; y < rectified_image.rows; y++) + { + for (size_t x = 0; x < rectified_image.cols; x++) + { + cv::Point2i uv_rect(x, y), uv_raw; + + uv_raw = model.unrectifyPoint(uv_rect); + + if (0 <= uv_raw.x && uv_raw.x < distorted_image_by_pixel.cols && 0 <= uv_raw.y + && uv_raw.y < distorted_image_by_pixel.rows) + { + distorted_image_by_pixel.at(uv_raw) = rectified_image.at(uv_rect); + mask.at(uv_raw) = 255; + // Test that both methods produce similar values at the pixels that unrectifyPoint hits; don't test for all + // other pixels (the images will differ there, because unrectifyPoint doesn't interpolate missing pixels). + // Also don't check for absolute equality, but allow a color difference of up to 200. This still catches + // complete misses (color difference would be 255) while allowing for interpolation at the grid borders. + EXPECT_LT(distorted_image.at(uv_raw)[0] - distorted_image_by_pixel.at(uv_raw)[0], 200); + } + } + } + + // Test that absolute error (due to interpolation) is less than 6% of the maximum possible error + error = cv::norm(distorted_image, distorted_image_by_pixel, cv::NORM_L1, mask); + EXPECT_LT(error / (distorted_image.size[0] * distorted_image.size[1] * 255), 0.06); + + // Test that unrectifyPoint hits more than 50% of the output image + EXPECT_GT((double) cv::countNonZero(mask) / (distorted_image.size[0] * distorted_image.size[1]), 0.5); +}; + +TEST_F(PinholeTest, unrectifyImage) +{ + testUnrectifyImage(cam_info_, model_); +} + +TEST_F(PinholeTest, unrectifyImageWithBinning) +{ + cam_info_.binning_x = 2; + cam_info_.binning_y = 2; + model_.fromCameraInfo(cam_info_); + + testUnrectifyImage(cam_info_, model_); +} + +TEST_F(PinholeTest, unrectifyImageWithRoi) +{ + cam_info_.roi.x_offset = 100; + cam_info_.roi.y_offset = 50; + cam_info_.roi.width = 400; + cam_info_.roi.height = 300; + cam_info_.roi.do_rectify = true; + model_.fromCameraInfo(cam_info_); + + testUnrectifyImage(cam_info_, model_); +} + +TEST_F(PinholeTest, unrectifyImageWithBinningAndRoi) +{ + cam_info_.binning_x = 2; + cam_info_.binning_y = 2; + cam_info_.roi.x_offset = 100; + cam_info_.roi.y_offset = 50; + cam_info_.roi.width = 400; + cam_info_.roi.height = 300; + cam_info_.roi.do_rectify = true; + model_.fromCameraInfo(cam_info_); - // restore original distortion + testUnrectifyImage(cam_info_, model_); +} + +TEST_F(PinholeTest, rectifiedRoiSize) { + + cv::Rect rectified_roi = model_.rectifiedRoi(); + cv::Size reduced_resolution = model_.reducedResolution(); + EXPECT_EQ(0, rectified_roi.x); + EXPECT_EQ(0, rectified_roi.y); + EXPECT_EQ(640, rectified_roi.width); + EXPECT_EQ(480, rectified_roi.height); + EXPECT_EQ(640, reduced_resolution.width); + EXPECT_EQ(480, reduced_resolution.height); + + cam_info_.binning_x = 2; + cam_info_.binning_y = 2; model_.fromCameraInfo(cam_info_); + rectified_roi = model_.rectifiedRoi(); + reduced_resolution = model_.reducedResolution(); + EXPECT_EQ(0, rectified_roi.x); + EXPECT_EQ(0, rectified_roi.y); + EXPECT_EQ(640, rectified_roi.width); + EXPECT_EQ(480, rectified_roi.height); + EXPECT_EQ(320, reduced_resolution.width); + EXPECT_EQ(240, reduced_resolution.height); + + cam_info_.binning_x = 1; + cam_info_.binning_y = 1; + cam_info_.roi.x_offset = 100; + cam_info_.roi.y_offset = 50; + cam_info_.roi.width = 400; + cam_info_.roi.height = 300; + cam_info_.roi.do_rectify = true; + model_.fromCameraInfo(cam_info_); + rectified_roi = model_.rectifiedRoi(); + reduced_resolution = model_.reducedResolution(); + EXPECT_EQ(137, rectified_roi.x); + EXPECT_EQ(82, rectified_roi.y); + EXPECT_EQ(321, rectified_roi.width); + EXPECT_EQ(242, rectified_roi.height); + EXPECT_EQ(321, reduced_resolution.width); + EXPECT_EQ(242, reduced_resolution.height); + + cam_info_.binning_x = 2; + cam_info_.binning_y = 2; + cam_info_.roi.x_offset = 100; + cam_info_.roi.y_offset = 50; + cam_info_.roi.width = 400; + cam_info_.roi.height = 300; + cam_info_.roi.do_rectify = true; + model_.fromCameraInfo(cam_info_); + rectified_roi = model_.rectifiedRoi(); + reduced_resolution = model_.reducedResolution(); + EXPECT_EQ(137, rectified_roi.x); + EXPECT_EQ(82, rectified_roi.y); + EXPECT_EQ(321, rectified_roi.width); + EXPECT_EQ(242, rectified_roi.height); + EXPECT_EQ(160, reduced_resolution.width); + EXPECT_EQ(121, reduced_resolution.height); +} + +TEST_F(PinholeTest, rectifiedRoiCaching) +{ + // Test that the following sequence works correctly: + // 1. fromCameraInfo is called with ROI A. | rectified_roi_dirty = true + // (already happened in SetUp()) + + // 2. rectifiedRoi is called | rectified_roi_dirty = false + cv::Rect actual_roi_a = model_.rectifiedRoi(); + cv::Rect expected_roi_a(0, 0, 640, 480); + EXPECT_EQ(expected_roi_a, actual_roi_a); + + // 3. fromCameraInfo is called with ROI B. | rectified_roi_dirty = true + cam_info_.roi.x_offset = 100; + cam_info_.roi.y_offset = 50; + cam_info_.roi.width = 400; + cam_info_.roi.height = 300; + cam_info_.roi.do_rectify = true; + model_.fromCameraInfo(cam_info_); + + // 4. fromCameraInfo is called again with ROI B. | rectified_roi_dirty should still be true! + model_.fromCameraInfo(cam_info_); + + // 5. rectifiedRoi is called + // There was a bug before where rectified_roi_dirty was incorrectly set to `false` by step 4. + // If rectifiedRoi was called again, the cached rectified_roi for + // ROI A was returned, but it should be recalculated based on ROI B. + // This test checks that this behavior is correct. + cv::Rect actual_roi_b = model_.rectifiedRoi(); + cv::Rect expected_roi_b(137, 82, 321, 242); + EXPECT_EQ(expected_roi_b, actual_roi_b); } int main(int argc, char** argv) diff --git a/image_geometry/test/utest_equi.cpp b/image_geometry/test/utest_equi.cpp new file mode 100644 index 000000000..4692bf163 --- /dev/null +++ b/image_geometry/test/utest_equi.cpp @@ -0,0 +1,257 @@ +#include "image_geometry/pinhole_camera_model.h" +#include +#include + +/// @todo Tests with simple values (R = identity, D = 0, P = K or simple scaling) +/// @todo Test projection functions for right stereo values, P(:,3) != 0 +/// @todo Tests for [un]rectifyImage +/// @todo Tests using ROI, needs support from PinholeCameraModel +/// @todo Tests for StereoCameraModel + +class EquidistantTest : public testing::Test +{ +protected: + virtual void SetUp() + { + /// @todo Just load these from file + // These parameters are taken from a real camera calibration + double D[] = {-0.08857683871674071, 0.0708113094372378, -0.09127623055964429, 0.04006922269778478}; + double K[] = {403.603063319358, 0.0, 306.15842863283063, + 0.0, 403.7028851121003, 261.09715697592696, + 0.0, 0.0, 1.0}; + double R[] = {0.999963944103842, -0.008484152966323483, 0.00036005656766869323, + 0.008484153516269438, 0.9999640089218772, 0.0, + -0.0003600436088446379, 3.0547751946422504e-06, 0.999999935179632}; + double P[] = {347.2569964503485, 0.0, 350.5, 0.0, + 0.0, 347.2569964503485, 256.0, 0.0, + 0.0, 0.0, 1.0, 0.0}; + + cam_info_.header.frame_id = "tf_frame"; + cam_info_.height = 512; + cam_info_.width = 640; + // No ROI + cam_info_.D.resize(4); + std::copy(D, D+4, cam_info_.D.begin()); + std::copy(K, K+9, cam_info_.K.begin()); + std::copy(R, R+9, cam_info_.R.begin()); + std::copy(P, P+12, cam_info_.P.begin()); + cam_info_.distortion_model = sensor_msgs::distortion_models::EQUIDISTANT; + + model_.fromCameraInfo(cam_info_); + } + + sensor_msgs::CameraInfo cam_info_; + image_geometry::PinholeCameraModel model_; +}; + +TEST_F(EquidistantTest, accessorsCorrect) +{ + EXPECT_STREQ("tf_frame", model_.tfFrame().c_str()); + EXPECT_EQ(cam_info_.P[0], model_.fx()); + EXPECT_EQ(cam_info_.P[5], model_.fy()); + EXPECT_EQ(cam_info_.P[2], model_.cx()); + EXPECT_EQ(cam_info_.P[6], model_.cy()); +} + +TEST_F(EquidistantTest, projectPoint) +{ + // Spot test an arbitrary point. + { + cv::Point2d uv(100, 100); + cv::Point3d xyz = model_.projectPixelTo3dRay(uv); + EXPECT_NEAR(-0.72136775518018115, xyz.x, 1e-8); + EXPECT_NEAR(-0.449235009214005, xyz.y, 1e-8); + EXPECT_DOUBLE_EQ(1.0, xyz.z); + } + + // Principal point should project straight out. + { + cv::Point2d uv(model_.cx(), model_.cy()); + cv::Point3d xyz = model_.projectPixelTo3dRay(uv); + EXPECT_DOUBLE_EQ(0.0, xyz.x); + EXPECT_DOUBLE_EQ(0.0, xyz.y); + EXPECT_DOUBLE_EQ(1.0, xyz.z); + } + + // Check projecting to 3d and back over entire image is accurate. + const size_t step = 10; + for (size_t row = 0; row <= cam_info_.height; row += step) { + for (size_t col = 0; col <= cam_info_.width; col += step) { + cv::Point2d uv(row, col), uv_back; + cv::Point3d xyz = model_.projectPixelTo3dRay(uv); + uv_back = model_.project3dToPixel(xyz); + // Measured max error at 1.13687e-13 + EXPECT_NEAR(uv.x, uv_back.x, 1.14e-13) << "at (" << row << ", " << col << ")"; + EXPECT_NEAR(uv.y, uv_back.y, 1.14e-13) << "at (" << row << ", " << col << ")"; + } + } +} + +TEST_F(EquidistantTest, rectifyPoint) +{ + // Spot test an arbitrary point. + { + cv::Point2d uv_raw(100, 100), uv_rect; + uv_rect = model_.rectifyPoint(uv_raw); + EXPECT_DOUBLE_EQ(135.45747375488281, uv_rect.x); + EXPECT_DOUBLE_EQ(84.945091247558594, uv_rect.y); + } + + /// @todo Need R = identity for the principal point tests. +#if 0 + // Test rectifyPoint takes (c'x, c'y) [from K] -> (cx, cy) [from P]. + double cxp = model_.intrinsicMatrix()(0,2), cyp = model_.intrinsicMatrix()(1,2); + { + cv::Point2d uv_raw(cxp, cyp), uv_rect; + model_.rectifyPoint(uv_raw, uv_rect); + EXPECT_NEAR(uv_rect.x, model_.cx(), 1e-4); + EXPECT_NEAR(uv_rect.y, model_.cy(), 1e-4); + } + + // Test unrectifyPoint takes (cx, cy) [from P] -> (c'x, c'y) [from K]. + { + cv::Point2d uv_rect(model_.cx(), model_.cy()), uv_raw; + model_.unrectifyPoint(uv_rect, uv_raw); + EXPECT_NEAR(uv_raw.x, cxp, 1e-4); + EXPECT_NEAR(uv_raw.y, cyp, 1e-4); + } +#endif + + // Check rectifying then unrectifying is accurate. + const size_t step = 5; + for (size_t row = 0; row <= cam_info_.height; row += step) { + for (size_t col = 0; col <= cam_info_.width; col += step) { + cv::Point2d uv_raw(row, col), uv_rect, uv_unrect; + uv_rect = model_.rectifyPoint(uv_raw); + uv_unrect = model_.unrectifyPoint(uv_rect); + EXPECT_NEAR(uv_raw.x, uv_unrect.x, 0.01); + EXPECT_NEAR(uv_raw.y, uv_unrect.y, 0.01); + } + } +} + +TEST_F(EquidistantTest, getDeltas) +{ + double u = 100.0, v = 200.0, du = 17.0, dv = 23.0, Z = 2.0; + cv::Point2d uv0(u, v), uv1(u + du, v + dv); + cv::Point3d xyz0, xyz1; + xyz0 = model_.projectPixelTo3dRay(uv0); + xyz0 *= (Z / xyz0.z); + xyz1 = model_.projectPixelTo3dRay(uv1); + xyz1 *= (Z / xyz1.z); + + EXPECT_NEAR(model_.getDeltaU(xyz1.x - xyz0.x, Z), du, 1e-4); + EXPECT_NEAR(model_.getDeltaV(xyz1.y - xyz0.y, Z), dv, 1e-4); + EXPECT_NEAR(model_.getDeltaX(du, Z), xyz1.x - xyz0.x, 1e-4); + EXPECT_NEAR(model_.getDeltaY(dv, Z), xyz1.y - xyz0.y, 1e-4); +} + +TEST_F(EquidistantTest, initialization) +{ + + sensor_msgs::CameraInfo info; + image_geometry::PinholeCameraModel camera; + + camera.fromCameraInfo(info); + + EXPECT_EQ(camera.initialized(), 1); + EXPECT_EQ(camera.projectionMatrix().rows, 3); + EXPECT_EQ(camera.projectionMatrix().cols, 4); +} + +TEST_F(EquidistantTest, rectifyIfCalibrated) +{ + /// @todo use forward distortion for a better test + // Ideally this test would have two images stored on disk + // one which is distorted and the other which is rectified, + // and then rectification would take place here and the output + // image compared to the one on disk (which would mean if + // the distortion coefficients above can't change once paired with + // an image). + + // Later could incorporate distort code + // (https://github.com/lucasw/vimjay/blob/master/src/standalone/distort_image.cpp) + // to take any image distort it, then undistort with rectifyImage, + // and given the distortion coefficients are consistent the input image + // and final output image should be mostly the same (though some + // interpolation error + // creeps in), except for outside a masked region where information was lost. + // The masked region can be generated with a pure white image that + // goes through the same process (if it comes out completely black + // then the distortion parameters are problematic). + + // For now generate an image and pass the test simply if + // the rectified image does not match the distorted image. + // Then zero out the first distortion coefficient and run + // the test again. + // Then zero out all the distortion coefficients and test + // that the output image is the same as the input. + cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0)); + + // draw a grid + const cv::Scalar color = cv::Scalar(255, 255, 255); + // draw the lines thick so the proportion of error due to + // interpolation is reduced + const int thickness = 7; + const int type = 8; + for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10) + { + cv::line(distorted_image, + cv::Point(0, y), cv::Point(cam_info_.width, y), + color, type, thickness); + } + for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10) + { + // draw the lines thick so the prorportion of interpolation error is reduced + cv::line(distorted_image, + cv::Point(x, 0), cv::Point(x, cam_info_.height), + color, type, thickness); + } + + cv::Mat rectified_image; + // Just making this number up, maybe ought to be larger + // since a completely different image would be on the order of + // width * height * 255 = 78e6 + const double diff_threshold = 10000.0; + double error; + + // Test that rectified image is sufficiently different + // using default distortion + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + // Just making this number up, maybe ought to be larger + EXPECT_GT(error, diff_threshold); + + // Test that rectified image is sufficiently different + // using default distortion but with first element zeroed + // out. + sensor_msgs::CameraInfo cam_info_2 = cam_info_; + cam_info_2.D[0] = 0.0; + model_.fromCameraInfo(cam_info_2); + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + EXPECT_GT(error, diff_threshold); + + // Test that rectified image is the same using zero distortion + cam_info_2.D.assign(cam_info_2.D.size(), 0); + model_.fromCameraInfo(cam_info_2); + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + EXPECT_EQ(error, 0); + + // Test that rectified image is the same using empty distortion + cam_info_2.D.clear(); + model_.fromCameraInfo(cam_info_2); + model_.rectifyImage(distorted_image, rectified_image); + error = cv::norm(distorted_image, rectified_image, cv::NORM_L1); + EXPECT_EQ(error, 0); + + // restore original distortion + model_.fromCameraInfo(cam_info_); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/opencv_tests/CHANGELOG.rst b/opencv_tests/CHANGELOG.rst index 206a5a909..5a0edc929 100644 --- a/opencv_tests/CHANGELOG.rst +++ b/opencv_tests/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package opencv_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.16.2 (2022-10-03) +------------------- + +1.16.1 (2022-09-06) +------------------- + +1.16.0 (2021-11-23) +------------------- + +1.15.0 (2020-05-19) +------------------- + +1.14.0 (2020-04-06) +------------------- +* Noetic release (`#323 `_) +* Contributors: Alejandro Hernández Cordero + 1.13.0 (2018-04-30) ------------------- diff --git a/opencv_tests/CMakeLists.txt b/opencv_tests/CMakeLists.txt index b45ba8c77..6b4d2a7ce 100644 --- a/opencv_tests/CMakeLists.txt +++ b/opencv_tests/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.0.2) project(opencv_tests) find_package(catkin REQUIRED) diff --git a/opencv_tests/launch/pong.launch b/opencv_tests/launch/pong.launch index c57124337..b8038b416 100644 --- a/opencv_tests/launch/pong.launch +++ b/opencv_tests/launch/pong.launch @@ -1,5 +1,5 @@ - - + + diff --git a/opencv_tests/nodes/broadcast.py b/opencv_tests/nodes/broadcast.py index 0df824dbb..f0290f84d 100755 --- a/opencv_tests/nodes/broadcast.py +++ b/opencv_tests/nodes/broadcast.py @@ -70,7 +70,7 @@ def main(args): rospy.spin() outcome = 'test completed' except KeyboardInterrupt: - print "shutting down" + print("shutting down") outcome = 'keyboard interrupt' rospy.core.signal_shutdown(outcome) diff --git a/opencv_tests/nodes/source.py b/opencv_tests/nodes/source.py index 4662fbef8..e4c2bfedc 100755 --- a/opencv_tests/nodes/source.py +++ b/opencv_tests/nodes/source.py @@ -87,7 +87,7 @@ def main(args): rospy.spin() outcome = 'test completed' except KeyboardInterrupt: - print "shutting down" + print("shutting down") outcome = 'keyboard interrupt' rospy.core.signal_shutdown(outcome) diff --git a/opencv_tests/package.xml b/opencv_tests/package.xml index 8368b676e..dfc3ad9ea 100644 --- a/opencv_tests/package.xml +++ b/opencv_tests/package.xml @@ -1,6 +1,6 @@ opencv_tests - 1.13.0 + 1.16.2 Tests the enumerants of the ROS Image message, and functionally tests the Python and C++ implementations of CvBridge. diff --git a/vision_opencv/CHANGELOG.rst b/vision_opencv/CHANGELOG.rst index 87d150ae7..53ba01647 100644 --- a/vision_opencv/CHANGELOG.rst +++ b/vision_opencv/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package vision_opencv ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.16.2 (2022-10-03) +------------------- + +1.16.1 (2022-09-06) +------------------- + +1.16.0 (2021-11-23) +------------------- + +1.15.0 (2020-05-19) +------------------- + +1.14.0 (2020-04-06) +------------------- +* Noetic release (`#323 `_) +* Contributors: Alejandro Hernández Cordero + 1.13.0 (2018-04-30) ------------------- diff --git a/vision_opencv/CMakeLists.txt b/vision_opencv/CMakeLists.txt index 8f1965de2..c5742e30f 100644 --- a/vision_opencv/CMakeLists.txt +++ b/vision_opencv/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(vision_opencv) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/vision_opencv/package.xml b/vision_opencv/package.xml index 2a65d8d50..46eeccc60 100644 --- a/vision_opencv/package.xml +++ b/vision_opencv/package.xml @@ -1,6 +1,6 @@ vision_opencv - 1.13.0 + 1.16.2 Packages for interfacing ROS with OpenCV, a library of programming functions for real time computer vision. Patrick Mihelich James Bowman