diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 000000000..7cde0b6af --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,66 @@ +version: 2 +jobs: + kinetic: + docker: + - image: ros:kinetic-perception + steps: + - checkout + - run: + name: Set Up Container + command: | + apt-get update -qq && apt-get install -y python-catkin-tools + source `find /opt/ros -name setup.bash | sort | head -1` + rosdep update + rosdep install --from-paths . --ignore-src + cd .. + catkin init + catkin config --extend /opt/ros/$ROS_DISTRO + - run: + name: Build + command: | + cd .. + catkin build --no-status -j4 + - run: + name: Run Tests + command: | + source `find /opt/ros -name setup.bash | sort | head -1` + cd .. + catkin run_tests -j4 + catkin_test_results + working_directory: ~/src + + melodic: + docker: + - image: ros:melodic-perception + steps: + - checkout + - run: + name: Set Up Container + command: | + apt update -qq && apt install -y python-catkin-tools + source `find /opt/ros -name setup.bash | sort | head -1` + rosdep update + rosdep install --from-paths . --ignore-src + cd .. + catkin init + catkin config --extend /opt/ros/$ROS_DISTRO + - run: + name: Build + command: | + cd .. + catkin build --no-status -j4 + - run: + name: Run Tests + command: | + source `find /opt/ros -name setup.bash | sort | head -1` + cd .. + catkin run_tests -j4 + catkin_test_results + working_directory: ~/src + +workflows: + version: 2 + ros_build: + jobs: + - kinetic + - melodic diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index fa58ee93d..000000000 --- a/.travis.yml +++ /dev/null @@ -1,66 +0,0 @@ -sudo: required -dist: trusty -language: generic -env: - - OPENCV_VERSION=2 ROS_DISTRO=indigo - - OPENCV_VERSION=3 ROS_DISTRO=indigo - - OPENCV_VERSION=2 ROS_DISTRO=jade - - OPENCV_VERSION=3 ROS_DISTRO=jade -# Version of 4 does not make sense but it's to pass the test below. -# Disabled as Travis does not support Xenial -# - OPENCV_VERSION=4 ROS_DISTRO=kinetic -# Install system dependencies, namely ROS. -before_install: - # Define some config vars. - - export ROS_CI_DESKTOP=`lsb_release -cs` # e.g. [precise|trusty] - - export CI_SOURCE_PATH=$(pwd) - - export REPOSITORY_NAME=${PWD##*/} - - export CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options - - export ROS_PARALLEL_JOBS='-j8 -l6' - # Install ROS - - sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu $ROS_CI_DESKTOP 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 - # Install ROS - - sudo apt-get install -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin - - source /opt/ros/$ROS_DISTRO/setup.bash - # Setup for rosdep - - sudo rosdep init - - rosdep update - -# Create a catkin workspace with the package under test. -install: - - mkdir -p ~/catkin_ws/src - - # Add the package under test to the workspace. - - cd ~/catkin_ws/src - - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace - - - if [ $OPENCV_VERSION == 3 ]; then sed -i 's@libopencv-dev@opencv3@' $REPOSITORY_NAME/*/package.xml ; fi - -# Install all dependencies, using wstool and rosdep. -# wstool looks for a ROSINSTALL_FILE defined in before_install. -before_script: - # source dependencies: install using wstool. - - cd ~/catkin_ws/src - - wstool init - - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi - - wstool up - - # package depdencies: install using rosdep. - - cd ~/catkin_ws - - rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO - -# Compile and test. -script: - - source /opt/ros/$ROS_DISTRO/setup.bash - - cd ~/catkin_ws - - catkin build -p1 -j1 --no-status - - catkin run_tests -p1 -j1 - - catkin_test_results --all build - - catkin clean -b --yes - - catkin config --install - - catkin build -p1 -j1 --no-status -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/README.md b/README.md new file mode 100644 index 000000000..22114993e --- /dev/null +++ b/README.md @@ -0,0 +1,10 @@ +image_pipeline +============== + +[![CircleCI](https://circleci.com/gh/ros-perception/image_pipeline.svg?style=svg)](https://circleci.com/gh/ros-perception/image_pipeline) + +This package fills the gap between getting raw images from a camera driver and higher-level vision processing. + +For more information on this metapackage and underlying packages, please see [the ROS wiki entry](http://wiki.ros.org/image_pipeline). + +For examples, see the [image_pipeline tutorials entry](http://wiki.ros.org/image_pipeline/Tutorials) on the ROS Wiki. diff --git a/README.rst b/README.rst deleted file mode 100644 index c09f14244..000000000 --- a/README.rst +++ /dev/null @@ -1,7 +0,0 @@ -image_pipeline -============== - -.. image:: https://travis-ci.org/ros-perception/image_pipeline.svg?branch=indigo - :target: https://travis-ci.org/ros-perception/image_pipeline - -This package fills the gap between getting raw images from a camera driver and higher-level vision processing. diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst index aa32b37e8..2a24ae774 100644 --- a/camera_calibration/CHANGELOG.rst +++ b/camera_calibration/CHANGELOG.rst @@ -1,3 +1,18 @@ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#356 `_ from sevangelatos/feature/calibrator_rolling_shutter +* Add max-chessboard-speed option to allow more accurate calibration of rolling shutter cameras. +* Merge pull request `#334 `_ from Fruchtzwerg94/patch-2 + Scale pixels down from 16 to 8 bits instead of just clipping +* Merge pull request `#340 `_ from k-okada/286 + use waitKey(0) instead of while loop +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Scale pixels down from 16 to 8 bits instead of just clipping + Clipping 16 bit pixels just to 8 bit pixels leads to white images if the original image uses the full range of the 16 bits. Instead the pixel should be scaled down to 8 bits. +* Contributors: Joshua Whitley, Kei Okada, Philipp, Spiros Evangelatos, Yoshito Okada, stevemacenski + 1.12.23 (2018-05-10) -------------------- * camera_checker: Ensure cols + rows are in correct order (`#319 `_) diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index 206962e1d..2a6cb5e49 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/nodes/cameracalibrator.py @@ -32,10 +32,10 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import print_function import cv2 import functools import message_filters -import os import rospy from camera_calibration.camera_calibrator import OpenCVCalibrationNode from camera_calibration.calibrator import ChessboardInfo, Patterns @@ -68,6 +68,9 @@ def main(): group.add_option("--no-service-check", action="store_false", dest="service_check", default=True, help="disable check for set_camera_info services at startup") + group.add_option("--queue-size", + type="int", default=1, + help="image queue size (default %default, set to 0 for unlimited)") parser.add_option_group(group) group = OptionGroup(parser, "Calibration Optimizer Options") group.add_option("--fix-principal-point", @@ -84,6 +87,9 @@ def main(): help="number of radial distortion coefficients to use (up to 6, default %default)") group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False, help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners") + group.add_option("--max-chessboard-speed", type="float", default=-1.0, + help="Do not use samples where the calibration pattern is moving faster \ + than this speed in px/frame. Set to eg. 0.5 for rolling shutter cameras.") parser.add_option_group(group) options, args = parser.parse_args() @@ -143,7 +149,8 @@ def main(): rospy.init_node('cameracalibrator') node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name, - checkerboard_flags=checkerboard_flags) + checkerboard_flags=checkerboard_flags, max_chessboard_speed=options.max_chessboard_speed, + queue_size=options.queue_size) rospy.spin() if __name__ == "__main__": diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml index 7946de45a..9745335bf 100644 --- a/camera_calibration/package.xml +++ b/camera_calibration/package.xml @@ -1,6 +1,6 @@ camera_calibration - 1.12.23 + 1.13.0 camera_calibration allows easy calibration of monocular or stereo cameras using a checkerboard calibration target. @@ -8,6 +8,8 @@ James Bowman Patrick Mihelich Vincent Rabaud + Steven Macenski + Autonomoustuff team catkin diff --git a/camera_calibration/scripts/tarfile_calibration.py b/camera_calibration/scripts/tarfile_calibration.py index 5d552ac5f..17e23fb46 100755 --- a/camera_calibration/scripts/tarfile_calibration.py +++ b/camera_calibration/scripts/tarfile_calibration.py @@ -32,8 +32,8 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import print_function import os -import sys import numpy import cv2 @@ -45,16 +45,10 @@ import rospy import sensor_msgs.srv -def waitkey(): - k = cv2.waitKey(6) - return k - def display(win_name, img): cv2.namedWindow(win_name, cv2.WINDOW_NORMAL) cv2.imshow( win_name, numpy.asarray( img[:,:] )) - k=-1 - while k ==-1: - k=waitkey() + k = cv2.waitKey(0) cv2.destroyWindow(win_name) if k in [27, ord('q')]: rospy.signal_shutdown('Quit') diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 4d68f9d6b..0bd58c873 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -32,6 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import print_function try: from cStringIO import StringIO except ImportError: @@ -218,7 +219,8 @@ class Calibrator(object): """ Base class for calibration system """ - def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK): + def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', + checkerboard_flags=cv2.CALIB_CB_FAST_CHECK, max_chessboard_speed = -1.0): # Ordering the dimensions for the different detectors is actually a minefield... if pattern == Patterns.Chessboard: # Make sure n_cols > n_rows to agree with OpenCV CB detector output @@ -247,6 +249,8 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checke self.goodenough = False self.param_ranges = [0.7, 0.7, 0.4, 0.5] self.name = name + self.last_frame_corners = None + self.max_chessboard_speed = max_chessboard_speed def mkgray(self, msg): """ @@ -256,7 +260,7 @@ def mkgray(self, msg): # TODO: get a Python API in cv_bridge to check for the image depth. if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') - mono8 = numpy.array(numpy.clip(mono16, 0, 255), dtype=numpy.uint8) + mono8 = numpy.array(mono16 / 256, dtype=numpy.uint8) return mono8 elif 'FC1' in msg.encoding: # floating point image handling @@ -289,7 +293,21 @@ def get_parameters(self, corners, board, size): params = [p_x, p_y, p_size, skew] return params - def is_good_sample(self, params): + def is_slow_moving(self, corners, last_frame_corners): + """ + Returns true if the motion of the checkerboard is sufficiently low between + this and the previous frame. + """ + # If we don't have previous frame corners, we can't accept the sample + if last_frame_corners is None: + return False + num_corners = len(corners) + corner_deltas = (corners - last_frame_corners).reshape(num_corners, 2) + # Average distance travelled overall for all corners + average_motion = numpy.average(numpy.linalg.norm(corner_deltas, axis = 1)) + return average_motion <= self.max_chessboard_speed + + def is_good_sample(self, params, corners, last_frame_corners): """ Returns true if the checkerboard detection described by params should be added to the database. """ @@ -303,7 +321,15 @@ def param_distance(p1, p2): d = min([param_distance(params, p) for p in db_params]) #print "d = %.3f" % d #DEBUG # TODO What's a good threshold here? Should it be configurable? - return d > 0.2 + if d <= 0.2: + return False + + if self.max_chessboard_speed > 0: + if not self.is_slow_moving(corners, last_frame_corners): + return False + + # All tests passed, image should be good for calibration + return True _param_names = ["X", "Y", "Size", "Skew"] @@ -427,10 +453,11 @@ def downsample_and_detect(self, img): return (scrib, corners, downsampled_corners, board, (x_scale, y_scale)) - def lrmsg(self, d, k, r, p): + @staticmethod + def lrmsg(d, k, r, p, size): """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ msg = sensor_msgs.msg.CameraInfo() - (msg.width, msg.height) = self.size + msg.width, msg.height = size if d.size > 5: msg.distortion_model = "rational_polynomial" else: @@ -441,71 +468,88 @@ def lrmsg(self, d, k, r, p): msg.P = numpy.ravel(p).copy().tolist() return msg - def lrreport(self, d, k, r, p): - print("D = ", numpy.ravel(d).tolist()) - print("K = ", numpy.ravel(k).tolist()) - print("R = ", numpy.ravel(r).tolist()) - print("P = ", numpy.ravel(p).tolist()) - - def lrost(self, name, d, k, r, p): - calmessage = ( - "# oST version 5.0 parameters\n" - + "\n" - + "\n" - + "[image]\n" - + "\n" - + "width\n" - + str(self.size[0]) + "\n" - + "\n" - + "height\n" - + str(self.size[1]) + "\n" - + "\n" - + "[%s]" % name + "\n" - + "\n" - + "camera matrix\n" - + " ".join(["%8f" % k[0,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % k[1,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % k[2,i] for i in range(3)]) + "\n" - + "\n" - + "distortion\n" - + " ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "\n" - + "\n" - + "rectification\n" - + " ".join(["%8f" % r[0,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % r[1,i] for i in range(3)]) + "\n" - + " ".join(["%8f" % r[2,i] for i in range(3)]) + "\n" - + "\n" - + "projection\n" - + " ".join(["%8f" % p[0,i] for i in range(4)]) + "\n" - + " ".join(["%8f" % p[1,i] for i in range(4)]) + "\n" - + " ".join(["%8f" % p[2,i] for i in range(4)]) + "\n" - + "\n") + @staticmethod + def lrreport(d, k, r, p): + print("D =", numpy.ravel(d).tolist()) + print("K =", numpy.ravel(k).tolist()) + print("R =", numpy.ravel(r).tolist()) + print("P =", numpy.ravel(p).tolist()) + + @staticmethod + def lrost(name, d, k, r, p, size): + assert k.shape == (3, 3) + assert r.shape == (3, 3) + assert p.shape == (3, 4) + calmessage = "\n".join([ + "# oST version 5.0 parameters", + "", + "", + "[image]", + "", + "width", + "%d" % size[0], + "", + "height", + "%d" % size[1], + "", + "[%s]" % name, + "", + "camera matrix", + " ".join("%8f" % k[0,i] for i in range(3)), + " ".join("%8f" % k[1,i] for i in range(3)), + " ".join("%8f" % k[2,i] for i in range(3)), + "", + "distortion", + " ".join("%8f" % x for x in d.flat), + "", + "rectification", + " ".join("%8f" % r[0,i] for i in range(3)), + " ".join("%8f" % r[1,i] for i in range(3)), + " ".join("%8f" % r[2,i] for i in range(3)), + "", + "projection", + " ".join("%8f" % p[0,i] for i in range(4)), + " ".join("%8f" % p[1,i] for i in range(4)), + " ".join("%8f" % p[2,i] for i in range(4)), + "" + ]) assert len(calmessage) < 525, "Calibration info must be less than 525 bytes" return calmessage - def lryaml(self, name, d, k, r, p): - calmessage = ("" - + "image_width: " + str(self.size[0]) + "\n" - + "image_height: " + str(self.size[1]) + "\n" - + "camera_name: " + name + "\n" - + "camera_matrix:\n" - + " rows: 3\n" - + " cols: 3\n" - + " data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n" - + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n" - + "distortion_coefficients:\n" - + " rows: 1\n" - + " cols: 5\n" - + " data: [" + ", ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "]\n" - + "rectification_matrix:\n" - + " rows: 3\n" - + " cols: 3\n" - + " data: [" + ", ".join(["%8f" % i for i in r.reshape(1,9)[0]]) + "]\n" - + "projection_matrix:\n" - + " rows: 3\n" - + " cols: 4\n" - + " data: [" + ", ".join(["%8f" % i for i in p.reshape(1,12)[0]]) + "]\n" - + "") + @staticmethod + def lryaml(name, d, k, r, p, size): + def format_mat(x, precision): + return ("[%s]" % ( + numpy.array2string(x, precision=precision, suppress_small=True, separator=", ") + .replace("[", "").replace("]", "").replace("\n", "\n ") + )) + + assert k.shape == (3, 3) + assert r.shape == (3, 3) + assert p.shape == (3, 4) + calmessage = "\n".join([ + "image_width: %d" % size[0], + "image_height: %d" % size[1], + "camera_name: " + name, + "camera_matrix:", + " rows: 3", + " cols: 3", + " data: " + format_mat(k, 5), + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob"), + "distortion_coefficients:", + " rows: 1", + " cols: %d" % d.size, + " data: [%s]" % ", ".join("%8f" % x for x in d.flat), + "rectification_matrix:", + " rows: 3", + " cols: 3", + " data: " + format_mat(r, 8), + "projection_matrix:", + " rows: 3", + " cols: 4", + " data: " + format_mat(p, 5), + "" + ]) return calmessage def do_save(self): @@ -604,19 +648,17 @@ def cal_fromcorners(self, good): ipts = [ points for (points, _) in good ] opts = self.mk_object_points(boards) - self.intrinsics = numpy.zeros((3, 3), numpy.float64) - if self.calib_flags & cv2.CALIB_RATIONAL_MODEL: - self.distortion = numpy.zeros((8, 1), numpy.float64) # rational polynomial - else: - self.distortion = numpy.zeros((5, 1), numpy.float64) # plumb bob # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio - self.intrinsics[0,0] = 1.0 - self.intrinsics[1,1] = 1.0 - cv2.calibrateCamera( + intrinsics_in = numpy.eye(3, dtype=numpy.float64) + reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( opts, ipts, - self.size, self.intrinsics, - self.distortion, + self.size, + intrinsics_in, + None, flags = self.calib_flags) + # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set. + # The extra ones include e.g. thin prism coefficients, which we are not interested in. + self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) # R is identity matrix for monocular calibration self.R = numpy.eye(3, dtype=numpy.float64) @@ -662,7 +704,7 @@ def undistort_points(self, src): def as_message(self): """ Return the camera calibration as a CameraInfo message """ - return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P) + return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size) def from_message(self, msg, alpha = 0.0): """ Initialize the camera calibration from a CameraInfo message """ @@ -679,10 +721,10 @@ def report(self): self.lrreport(self.distortion, self.intrinsics, self.R, self.P) def ost(self): - return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P) + return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) def yaml(self): - return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P) + return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) def linear_error_from_image(self, image): """ @@ -767,11 +809,12 @@ def handle_msg(self, msg): # Add sample to database only if it's sufficiently different from any previous sample. params = self.get_parameters(corners, board, (gray.shape[1], gray.shape[0])) - if self.is_good_sample(params): + if self.is_good_sample(params, corners, self.last_frame_corners): self.db.append((params, gray)) self.good_corners.append((corners, board)) print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) - + + self.last_frame_corners = corners rv = MonoDrawable() rv.scrib = scrib rv.params = self.compute_goodenough() @@ -942,8 +985,8 @@ def as_message(self): and right cameras respectively. """ - return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P), - self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)) + return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size), + self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) def from_message(self, msgs, alpha = 0.0): """ Initialize the camera calibration from a pair of CameraInfo messages. """ @@ -963,15 +1006,15 @@ def report(self): self.lrreport(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) print("\nRight:") self.lrreport(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P) - print("self.T ", numpy.ravel(self.T).tolist()) - print("self.R ", numpy.ravel(self.R).tolist()) + print("self.T =", numpy.ravel(self.T).tolist()) + print("self.R =", numpy.ravel(self.R).tolist()) def ost(self): - return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) + - self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)) + return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size) + + self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) def yaml(self, suffix, info): - return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P) + return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size) # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners def epipolar_error_from_images(self, limage, rimage): @@ -1088,11 +1131,12 @@ def handle_msg(self, msg): # Add sample to database only if it's sufficiently different from any previous sample if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): params = self.get_parameters(lcorners, lboard, (lgray.shape[1], lgray.shape[0])) - if self.is_good_sample(params): + if self.is_good_sample(params, lcorners, self.last_frame_corners): self.db.append( (params, lgray, rgray) ) self.good_corners.append( (lcorners, rcorners, lboard) ) print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params))) - + + self.last_frame_corners = lcorners rv = StereoDrawable() rv.lscrib = lscrib rv.rscrib = rscrib diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index bb9af7c14..79f89b0f9 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -32,6 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import print_function import cv2 import message_filters import numpy @@ -41,11 +42,27 @@ import sensor_msgs.srv import threading import time -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo, Patterns -from collections import deque -from message_filters import ApproximateTimeSynchronizer -from std_msgs.msg import String -from std_srvs.srv import Empty +from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, Patterns +try: + from queue import Queue +except ImportError: + from Queue import Queue + + +class BufferQueue(Queue): + """Slight modification of the standard Queue that discards the oldest item + when adding an item and the queue is full. + """ + def put(self, item, *args, **kwargs): + # The base implementation, for reference: + # https://github.com/python/cpython/blob/2.7/Lib/Queue.py#L107 + # https://github.com/python/cpython/blob/3.8/Lib/queue.py#L121 + with self.mutex: + if self.maxsize > 0 and self._qsize() == self.maxsize: + self._get() + self._put(item) + self.unfinished_tasks += 1 + self.not_empty.notify() class DisplayThread(threading.Thread): @@ -59,22 +76,23 @@ def __init__(self, queue, opencv_calibration_node): threading.Thread.__init__(self) self.queue = queue self.opencv_calibration_node = opencv_calibration_node + self.image = None def run(self): cv2.namedWindow("display", cv2.WINDOW_NORMAL) cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse) cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale) while True: - # wait for an image (could happen at the very beginning when the queue is still empty) - while len(self.queue) == 0: + if self.queue.qsize() > 0: + self.image = self.queue.get() + cv2.imshow("display", self.image) + else: time.sleep(0.1) - im = self.queue[0] - cv2.imshow("display", im) k = cv2.waitKey(6) & 0xFF if k in [27, ord('q')]: rospy.signal_shutdown('Quit') - elif k == ord('s'): - self.opencv_calibration_node.screendump(im) + elif k == ord('s') and self.image is not None: + self.opencv_calibration_node.screendump(self.image) class ConsumerThread(threading.Thread): def __init__(self, queue, function): @@ -84,15 +102,14 @@ def __init__(self, queue, function): def run(self): while True: - # wait for an image (could happen at the very beginning when the queue is still empty) - while len(self.queue) == 0: - time.sleep(0.1) - self.function(self.queue[0]) + m = self.queue.get() + self.function(m) class CalibrationNode: def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, - pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0): + pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, max_chessboard_speed = -1, + queue_size = 1): if service_check: # assume any non-default service names have been set. Wait for the service to become ready for svcname in ["camera", "left_camera", "right_camera"]: @@ -112,6 +129,7 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name + self._max_chessboard_speed = max_chessboard_speed lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) ts = synchronizer([lsub, rsub], 4) @@ -127,10 +145,12 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) - self.q_mono = deque([], 1) - self.q_stereo = deque([], 1) + self.q_mono = BufferQueue(queue_size) + self.q_stereo = BufferQueue(queue_size) self.c = None + + self._last_display = None mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) @@ -146,19 +166,21 @@ def redraw_monocular(self, *args): pass def queue_monocular(self, msg): - self.q_mono.append(msg) + self.q_mono.put(msg) def queue_stereo(self, lmsg, rmsg): - self.q_stereo.append((lmsg, rmsg)) + self.q_stereo.put((lmsg, rmsg)) def handle_monocular(self, msg): if self.c == None: if self._camera_name: self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, - checkerboard_flags=self._checkerboard_flags) + checkerboard_flags=self._checkerboard_flags, + max_chessboard_speed = self._max_chessboard_speed) else: self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, - checkerboard_flags=self.checkerboard_flags) + checkerboard_flags=self.checkerboard_flags, + max_chessboard_speed = self._max_chessboard_speed) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) @@ -169,10 +191,12 @@ def handle_stereo(self, msg): if self.c == None: if self._camera_name: self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, - checkerboard_flags=self._checkerboard_flags) + checkerboard_flags=self._checkerboard_flags, + max_chessboard_speed = self._max_chessboard_speed) else: self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, - checkerboard_flags=self._checkerboard_flags) + checkerboard_flags=self._checkerboard_flags, + max_chessboard_speed = self._max_chessboard_speed) drawable = self.c.handle_msg(msg) self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1] @@ -221,7 +245,7 @@ def __init__(self, *args, **kwargs): CalibrationNode.__init__(self, *args, **kwargs) - self.queue_display = deque([], 1) + self.queue_display = BufferQueue(maxsize=1) self.display_thread = DisplayThread(self.queue_display, self) self.display_thread.setDaemon(True) self.display_thread.start() @@ -238,7 +262,10 @@ def on_mouse(self, event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x: if self.c.goodenough: if 180 <= y < 280: + print("**** Calibrating ****") self.c.do_calibration() + self.buttons(self._last_display) + self.queue_display.put(self._last_display) if self.c.calibrated: if 280 <= y < 380: self.c.do_save() @@ -277,6 +304,7 @@ def screendump(self, im): while os.access("/tmp/dump%d.png" % i, os.R_OK): i += 1 cv2.imwrite("/tmp/dump%d.png" % i, im) + print("Saved screen dump to /tmp/dump%d.png" % i) def redraw_monocular(self, drawable): height = drawable.scrib.shape[0] @@ -286,7 +314,6 @@ def redraw_monocular(self, drawable): display[0:height, 0:width,:] = drawable.scrib display[0:height, width:width+100,:].fill(255) - self.buttons(display) if not self.c.calibrated: if drawable.params: @@ -311,7 +338,8 @@ def redraw_monocular(self, drawable): #print "linear", linerror self.putText(display, msg, (width, self.y(1))) - self.queue_display.append(display) + self._last_display = display + self.queue_display.put(display) def redraw_stereo(self, drawable): height = drawable.lscrib.shape[0] @@ -349,4 +377,5 @@ def redraw_stereo(self, drawable): self.putText(display, "dim", (2 * width, self.y(2))) self.putText(display, "%.3f" % drawable.dim, (2 * width, self.y(3))) - self.queue_display.append(display) + self._last_display = display + self.queue_display.put(display) diff --git a/camera_calibration/src/camera_calibration/camera_checker.py b/camera_calibration/src/camera_calibration/camera_checker.py index d27b893e0..e00c033cb 100755 --- a/camera_calibration/src/camera_calibration/camera_checker.py +++ b/camera_calibration/src/camera_calibration/camera_checker.py @@ -32,6 +32,7 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import print_function import cv2 import cv_bridge import functools diff --git a/camera_calibration/test/directed.py b/camera_calibration/test/directed.py index 43a7aaa4e..287063748 100644 --- a/camera_calibration/test/directed.py +++ b/camera_calibration/test/directed.py @@ -32,16 +32,14 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from __future__ import print_function import roslib import rosunit -import rospy import cv2 import collections import copy import numpy -import os -import sys import tarfile import unittest @@ -163,7 +161,6 @@ class TestArtificial(unittest.TestCase): def setUp(self): # Define some image transforms that will simulate a camera position M = [] - cv2.getPerspectiveTransform self.K = numpy.array([[500, 0, 250], [0, 500, 250], [0, 0, 1]], numpy.float32) self.D = numpy.array([]) # physical size of the board @@ -269,6 +266,28 @@ def test_monocular(self): 'intrinsics error is %f for resolution i = %d' % (err_intrinsics, i)) print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf)) + def test_rational_polynomial_model(self): + """Test that the distortion coefficients returned for a rational_polynomial model are not empty.""" + for i, setup in enumerate(self.setups): + board = ChessboardInfo() + board.n_cols = setup.cols + board.n_rows = setup.rows + board.dim = self.board_width_dim + + mc = MonoCalibrator([ board ], flags=cv2.CALIB_RATIONAL_MODEL, pattern=setup.pattern) + mc.cal(self.limages[i]) + self.assertEqual(len(mc.distortion.flat), 8, + 'length of distortion coefficients is %d' % len(mc.distortion.flat)) + self.assert_(all(mc.distortion.flat != 0), + 'some distortion coefficients are zero: %s' % str(mc.distortion.flatten())) + self.assertEqual(mc.as_message().distortion_model, 'rational_polynomial') + self.assert_good_mono(mc, self.limages[i], setup.lin_err) + + yaml = mc.yaml() + # Issue #278 + self.assertIn('cols: 8', yaml) + + if __name__ == '__main__': #rosunit.unitrun('camera_calibration', 'directed', TestDirected) rosunit.unitrun('camera_calibration', 'artificial', TestArtificial) diff --git a/depth_image_proc/CHANGELOG.rst b/depth_image_proc/CHANGELOG.rst index 3ea67b2b7..50879ad34 100644 --- a/depth_image_proc/CHANGELOG.rst +++ b/depth_image_proc/CHANGELOG.rst @@ -1,3 +1,10 @@ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Contributors: Joshua Whitley, Yoshito Okada, stevemacenski + 1.12.23 (2018-05-10) -------------------- diff --git a/depth_image_proc/package.xml b/depth_image_proc/package.xml index 8a1abb7b8..47d5d9bfd 100644 --- a/depth_image_proc/package.xml +++ b/depth_image_proc/package.xml @@ -1,6 +1,6 @@ depth_image_proc - 1.12.23 + 1.13.0 Contains nodelets for processing depth images such as those @@ -11,6 +11,8 @@ Patrick Mihelich Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://ros.org/wiki/depth_image_proc diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp index 92604423b..a1aaa6034 100644 --- a/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp @@ -199,7 +199,7 @@ void PointCloudXyziNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg, intensity_msg = intensity_msg_in; // Supported color encodings: MONO8, MONO16 - if (intensity_msg->encoding != enc::MONO8 || intensity_msg->encoding != enc::MONO16) + if (intensity_msg->encoding != enc::MONO8 && intensity_msg->encoding != enc::MONO16) { try { diff --git a/image_pipeline/CHANGELOG.rst b/image_pipeline/CHANGELOG.rst index 8e0904fcd..2eb6bb682 100644 --- a/image_pipeline/CHANGELOG.rst +++ b/image_pipeline/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package image_pipeline ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Contributors: Joshua Whitley, Yoshito Okada, stevemacenski + 1.12.23 (2018-05-10) -------------------- diff --git a/image_pipeline/package.xml b/image_pipeline/package.xml index 6dacd334e..e65f92d58 100644 --- a/image_pipeline/package.xml +++ b/image_pipeline/package.xml @@ -1,10 +1,12 @@ image_pipeline - 1.12.23 + 1.13.0 image_pipeline fills the gap between getting raw images from a camera driver and higher-level vision processing. Patrick Mihelich James Bowman Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://www.ros.org/wiki/image_pipeline diff --git a/image_proc/CHANGELOG.rst b/image_proc/CHANGELOG.rst index 6275315d1..9e1ea0a46 100644 --- a/image_proc/CHANGELOG.rst +++ b/image_proc/CHANGELOG.rst @@ -1,3 +1,10 @@ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Contributors: Joshua Whitley, Yoshito Okada, stevemacenski + 1.12.23 (2018-05-10) -------------------- diff --git a/image_proc/package.xml b/image_proc/package.xml index 315d31e6a..cfe083882 100644 --- a/image_proc/package.xml +++ b/image_proc/package.xml @@ -1,11 +1,13 @@ image_proc - 1.12.23 + 1.13.0 Single image rectification and color processing. Patrick Mihelich Kurt Konolige Jeremy Leibs Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://www.ros.org/wiki/image_proc diff --git a/image_proc/src/nodelets/crop_decimate.cpp b/image_proc/src/nodelets/crop_decimate.cpp index 46a329128..55a1f1b9c 100644 --- a/image_proc/src/nodelets/crop_decimate.cpp +++ b/image_proc/src/nodelets/crop_decimate.cpp @@ -196,7 +196,19 @@ void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg, } int max_width = image_msg->width - config.x_offset; + if (max_width <= 0) + { + ROS_WARN_THROTTLE(30., "x offset is outside the input image width: " + "%i, x offset: %i.", image_msg->width, config.x_offset); + return; + } int max_height = image_msg->height - config.y_offset; + if (max_height <= 0) + { + ROS_WARN_THROTTLE(30., "y offset is outside the input image height: " + "%i, y offset: %i", image_msg->height, config.y_offset); + return; + } int width = config.width; int height = config.height; if (width == 0 || width > max_width) diff --git a/image_proc/src/nodelets/debayer.cpp b/image_proc/src/nodelets/debayer.cpp index 608cfe4a6..a95866703 100644 --- a/image_proc/src/nodelets/debayer.cpp +++ b/image_proc/src/nodelets/debayer.cpp @@ -230,7 +230,16 @@ void DebayerNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg) if (algorithm == Debayer_VNG) code += cv::COLOR_BayerBG2BGR_VNG - cv::COLOR_BayerBG2BGR; - cv::cvtColor(bayer, color, code); + try + { + cv::cvtColor(bayer, color, code); + } + catch (cv::Exception &e) + { + NODELET_WARN_THROTTLE(30, "cvtColor error: '%s', bayer code: %d, width %d, height %d", + e.what(), code, bayer.cols, bayer.rows); + return; + } } pub_color_.publish(color_msg); diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp index 44fc9a30e..ee36d2965 100644 --- a/image_proc/src/nodelets/resize.cpp +++ b/image_proc/src/nodelets/resize.cpp @@ -31,86 +31,108 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include #include #include #include -#include #include #include #include +#include #include "image_proc/ResizeConfig.h" namespace image_proc { - -class ResizeNodelet : public nodelet_topic_tools::NodeletLazy +class ResizeNodelet : public nodelet::Nodelet { protected: // ROS communication - ros::Publisher pub_image_; + std::shared_ptr nh_; + std::shared_ptr pnh_; + image_transport::Publisher pub_image_; ros::Publisher pub_info_; + image_transport::Subscriber sub_image_; ros::Subscriber sub_info_; - ros::Subscriber sub_image_; + + std::shared_ptr it_, private_it_; + std::mutex connect_mutex_; // Dynamic reconfigure - boost::recursive_mutex config_mutex_; + std::mutex config_mutex_; typedef image_proc::ResizeConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; + std::shared_ptr reconfigure_server_; Config config_; virtual void onInit(); - virtual void subscribe(); - virtual void unsubscribe(); + void connectCb(); void imageCb(const sensor_msgs::ImageConstPtr& image_msg); void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg); - void configCb(Config &config, uint32_t level); + void configCb(Config& config, uint32_t level); }; void ResizeNodelet::onInit() { - nodelet_topic_tools::NodeletLazy::onInit(); + nh_.reset(new ros::NodeHandle(getNodeHandle())); + pnh_.reset(new ros::NodeHandle(getPrivateNodeHandle())); + it_.reset(new image_transport::ImageTransport(*nh_)); + private_it_.reset(new image_transport::ImageTransport(*pnh_)); // Set up dynamic reconfigure - reconfigure_server_.reset(new ReconfigureServer(config_mutex_, *pnh_)); - ReconfigureServer::CallbackType f = boost::bind(&ResizeNodelet::configCb, this, _1, _2); + reconfigure_server_.reset(new ReconfigureServer(*pnh_)); + ReconfigureServer::CallbackType f = + std::bind(&ResizeNodelet::configCb, this, std::placeholders::_1, std::placeholders::_2); reconfigure_server_->setCallback(f); - pub_info_ = advertise(*pnh_, "camera_info", 1); - pub_image_ = advertise(*pnh_, "image", 1); - - onInitPostProcess(); -} - -void ResizeNodelet::configCb(Config &config, uint32_t level) -{ - config_ = config; + // Monitor whether anyone is subscribed to the output + auto connect_cb = std::bind(&ResizeNodelet::connectCb, this); + // Make sure we don't enter connectCb() between advertising and assigning to + // pub_XXX + std::lock_guard lock(connect_mutex_); + pub_image_ = private_it_->advertise("image", 1, connect_cb, connect_cb); + pub_info_ = pnh_->advertise("camera_info", 1, connect_cb, connect_cb); } -void ResizeNodelet::subscribe() +// Handles (un)subscribing when clients (un)subscribe +void ResizeNodelet::connectCb() { - sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this); - sub_image_ = nh_->subscribe("image", 1, &ResizeNodelet::imageCb, this); + std::lock_guard lock(connect_mutex_); + if (pub_image_.getNumSubscribers() == 0) + { + sub_image_.shutdown(); + } + else if (!sub_image_) + { + sub_image_ = it_->subscribe("image", 1, &ResizeNodelet::imageCb, this); + } + if (pub_info_.getNumSubscribers() == 0) + { + sub_info_.shutdown(); + } + else if (!sub_info_) + { + sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this); + } } -void ResizeNodelet::unsubscribe() +void ResizeNodelet::configCb(Config& config, uint32_t level) { - sub_info_.shutdown(); - sub_image_.shutdown(); + std::lock_guard lock(config_mutex_); + config_ = config; } void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) { Config config; { - boost::lock_guard lock(config_mutex_); + std::lock_guard lock(config_mutex_); config = config_; } - sensor_msgs::CameraInfo dst_info_msg = *info_msg; + sensor_msgs::CameraInfoPtr dst_info_msg(new sensor_msgs::CameraInfo(*info_msg)); double scale_y; double scale_x; @@ -118,27 +140,27 @@ void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg) { scale_y = config.scale_height; scale_x = config.scale_width; - dst_info_msg.height = static_cast(info_msg->height * config.scale_height); - dst_info_msg.width = static_cast(info_msg->width * config.scale_width); + dst_info_msg->height = static_cast(info_msg->height * config.scale_height); + dst_info_msg->width = static_cast(info_msg->width * config.scale_width); } else { scale_y = static_cast(config.height) / info_msg->height; scale_x = static_cast(config.width) / info_msg->width; - dst_info_msg.height = config.height; - dst_info_msg.width = config.width; + dst_info_msg->height = config.height; + dst_info_msg->width = config.width; } - dst_info_msg.K[0] = dst_info_msg.K[0] * scale_x; // fx - dst_info_msg.K[2] = dst_info_msg.K[2] * scale_x; // cx - dst_info_msg.K[4] = dst_info_msg.K[4] * scale_y; // fy - dst_info_msg.K[5] = dst_info_msg.K[5] * scale_y; // cy + dst_info_msg->K[0] = dst_info_msg->K[0] * scale_x; // fx + dst_info_msg->K[2] = dst_info_msg->K[2] * scale_x; // cx + dst_info_msg->K[4] = dst_info_msg->K[4] * scale_y; // fy + dst_info_msg->K[5] = dst_info_msg->K[5] * scale_y; // cy - dst_info_msg.P[0] = dst_info_msg.P[0] * scale_x; // fx - dst_info_msg.P[2] = dst_info_msg.P[2] * scale_x; // cx - dst_info_msg.P[3] = dst_info_msg.P[3] * scale_x; // T - dst_info_msg.P[5] = dst_info_msg.P[5] * scale_y; // fy - dst_info_msg.P[6] = dst_info_msg.P[6] * scale_y; // cy + dst_info_msg->P[0] = dst_info_msg->P[0] * scale_x; // fx + dst_info_msg->P[2] = dst_info_msg->P[2] * scale_x; // cx + dst_info_msg->P[3] = dst_info_msg->P[3] * scale_x; // T + dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y; // fy + dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y; // cy pub_info_.publish(dst_info_msg); } @@ -147,16 +169,25 @@ void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg) { Config config; { - boost::lock_guard lock(config_mutex_); + std::lock_guard lock(config_mutex_); config = config_; } - cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg); + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(image_msg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } if (config.use_scale) { - cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), - config.scale_width, config.scale_height, config.interpolation); + cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0), config.scale_width, config.scale_height, + config.interpolation); } else { diff --git a/image_publisher/CHANGELOG.rst b/image_publisher/CHANGELOG.rst index 272171197..fd55423cc 100644 --- a/image_publisher/CHANGELOG.rst +++ b/image_publisher/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package image_publisher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#358 `_ from lucasw/image_pub_dr_private_namespace +* Use a shared_ptr for the dynamic reconfigure pointer, and create it with the private node handle so that the parameters for the dynamic reconfigure server are in the private namespace and two image publishers can coexist in the same manager `#357 `_ +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Contributors: Joshua Whitley, Lucas Walter, Yoshito Okada, stevemacenski + 1.12.23 (2018-05-10) -------------------- * fix 'VideoCapture' undefined symbol error (`#318 `_) diff --git a/image_publisher/package.xml b/image_publisher/package.xml index e78875a5e..76a2f3e09 100644 --- a/image_publisher/package.xml +++ b/image_publisher/package.xml @@ -1,6 +1,6 @@ image_publisher - 1.12.23 + 1.13.0

Contains a node publish an image stream from single image file @@ -9,6 +9,8 @@ Kei Okada Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://ros.org/wiki/image_publisher diff --git a/image_publisher/src/nodelet/image_publisher_nodelet.cpp b/image_publisher/src/nodelet/image_publisher_nodelet.cpp index 26e135221..49d9dc6e3 100644 --- a/image_publisher/src/nodelet/image_publisher_nodelet.cpp +++ b/image_publisher/src/nodelet/image_publisher_nodelet.cpp @@ -48,7 +48,8 @@ using namespace boost::assign; namespace image_publisher { class ImagePublisherNodelet : public nodelet::Nodelet { - dynamic_reconfigure::Server srv; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::shared_ptr srv; image_transport::CameraPublisher pub_; @@ -184,9 +185,10 @@ class ImagePublisherNodelet : public nodelet::Nodelet timer_ = nh_.createTimer(ros::Duration(1), &ImagePublisherNodelet::do_work, this); - dynamic_reconfigure::Server::CallbackType f = + srv.reset(new ReconfigureServer(getPrivateNodeHandle())); + ReconfigureServer::CallbackType f = boost::bind(&ImagePublisherNodelet::reconfigureCallback, this, _1, _2); - srv.setCallback(f); + srv->setCallback(f); } }; } diff --git a/image_rotate/CHANGELOG.rst b/image_rotate/CHANGELOG.rst index 228c9ed2b..1101a4c9d 100644 --- a/image_rotate/CHANGELOG.rst +++ b/image_rotate/CHANGELOG.rst @@ -1,3 +1,16 @@ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#382 `_ from garaemon/intiialzie-prev-stamp + Fix tf timeout of image_rotate +* Initialize prev_stamp\_ as ros::Timw::now() + * If prev_stamp\_ is initialized as 0.0, tf may wait transformation for + long duration at the first time. In order to give up the wait in + reasonable duration, initialize prev_stamp\_ as current time. +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Contributors: Joshua Whitley, Ryohei Ueda, Yoshito Okada, stevemacenski + 1.12.23 (2018-05-10) -------------------- diff --git a/image_rotate/package.xml b/image_rotate/package.xml index a7fbb747f..5049d0099 100644 --- a/image_rotate/package.xml +++ b/image_rotate/package.xml @@ -1,6 +1,6 @@ image_rotate - 1.12.23 + 1.13.0

Contains a node that rotates an image stream in a way that minimizes @@ -23,6 +23,8 @@ Blaise Gassend Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://ros.org/wiki/image_rotate diff --git a/image_rotate/src/nodelet/image_rotate_nodelet.cpp b/image_rotate/src/nodelet/image_rotate_nodelet.cpp index 066ef4e7a..2fc023d7b 100644 --- a/image_rotate/src/nodelet/image_rotate_nodelet.cpp +++ b/image_rotate/src/nodelet/image_rotate_nodelet.cpp @@ -265,7 +265,7 @@ class ImageRotateNodelet : public nodelet::Nodelet it_ = boost::shared_ptr(new image_transport::ImageTransport(nh_)); subscriber_count_ = 0; angle_ = 0; - prev_stamp_ = ros::Time(0, 0); + prev_stamp_ = ros::Time::now(); tf_sub_.reset(new tf2_ros::TransformListener(tf_buffer_)); image_transport::SubscriberStatusCallback connect_cb = boost::bind(&ImageRotateNodelet::connectCb, this, _1); image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1); diff --git a/image_view/CHANGELOG.rst b/image_view/CHANGELOG.rst index f5756fa4d..58a986729 100644 --- a/image_view/CHANGELOG.rst +++ b/image_view/CHANGELOG.rst @@ -1,3 +1,34 @@ +1.13.0 (2019-06-12) +------------------- +* Implemented extracting raw image data (`#329 `_) + Implementation of the raw image extraction if the file extension is .raw. This file extension is not supported by cv::imwrite so there is be no conflict. The raw files only containing the pixel data without any meta data which allows usage in MATLAB or other tools. +* Merge pull request `#375 `_ from fizyr-forks/opencv4 +* Fix OpenCV4 compatibility. +* Merge pull request `#337 `_ from yoshito-okada/fix_image_view_nodelet + Fix threading issue in image_view nodelet. Closes `#331 `_. +* Merge pull request `#394 `_ from angeltop/indigo + image_view: video recorder fix for conversion of fps to ros::Duration +* Merge pull request `#379 `_ from fizyr-forks/boost-1.69 + Fix boost 1.69 compatibility +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding stevemacenski as maintainer to get emails +* adding autonomoustuff mainainer +* Merge pull request `#343 `_ from fkie-forks/work_around_opencv_highgui_bug + Work around OpenCV highgui bug + I had to remove the GTK workaround, since it creates symbol collisions between GTK2 and GTK3. +* Refresh GUI on image update as well +* Use WallTimer for backwards compatibility with ROS Indigo +* Switch to SteadyTimer as suggested in review +* Remove unused `signals` from find_package(Boost COMPONENTS ...) + The signals library was not used at all, and it has been removed from + boost 1.69. As a result, the package doesn't build anymore with boost + 1.69 without this change. +* While we're at it, work around the mutex assertion failure on exit +* Work around an OpenCV bug with GTK and threading +* add ThreadSageImage class to encapsilate mutex operation for image +* handle window in single thread +* Contributors: Hans Gaiser, Joshua Whitley, Maarten de Vries, Philipp, Timo Röhling, Yoshito Okada, angeltop, stevemacenski + 1.12.23 (2018-05-10) -------------------- diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt index 0b9814860..ce7b8dcf8 100644 --- a/image_view/CMakeLists.txt +++ b/image_view/CMakeLists.txt @@ -5,7 +5,7 @@ find_package(catkin REQUIRED COMPONENTS camera_calibration_parsers cv_bridge dyn generate_dynamic_reconfigure_options(cfg/ImageView.cfg) catkin_package(CATKIN_DEPENDS dynamic_reconfigure) -find_package(Boost REQUIRED COMPONENTS signals thread) +find_package(Boost REQUIRED COMPONENTS thread) find_package(OpenCV REQUIRED) include_directories(${Boost_INCLUDE_DIRS} @@ -42,6 +42,10 @@ find_package(GTK2) add_definitions(-DHAVE_GTK) include_directories(${GTK2_INCLUDE_DIRS}) +find_package(PkgConfig REQUIRED) +pkg_check_modules(PC_HB REQUIRED harfbuzz) +include_directories(${PC_HB_INCLUDE_DIRS}) + # Nodelet library add_library(image_view src/nodelets/image_nodelet.cpp src/nodelets/disparity_nodelet.cpp src/nodelets/window_thread.cpp) target_link_libraries(image_view ${catkin_LIBRARIES} diff --git a/image_view/package.xml b/image_view/package.xml index 34637328c..d972bbb0a 100644 --- a/image_view/package.xml +++ b/image_view/package.xml @@ -1,12 +1,14 @@ image_view - 1.12.23 + 1.13.0 A simple viewer for ROS image topics. Includes a specialized viewer for stereo + disparity images. Patrick Mihelich Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://www.ros.org/wiki/image_view diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp index 3456243da..57467d739 100644 --- a/image_view/src/nodelets/image_nodelet.cpp +++ b/image_view/src/nodelets/image_nodelet.cpp @@ -39,51 +39,78 @@ #include #include "window_thread.h" +#include #include #include -#ifdef HAVE_GTK -#include -// Platform-specific workaround for #3026: image_view doesn't close when -// closing image window. On platforms using GTK+ we connect this to the -// window's "destroy" event so that image_view exits. -static void destroyNode(GtkWidget *widget, gpointer data) +namespace image_view { + +class ThreadSafeImage { - /// @todo On ros::shutdown(), the node hangs. Why? - //ros::shutdown(); - exit(0); // brute force solution -} + boost::mutex mutex_; + boost::condition_variable condition_; + cv::Mat image_; + +public: + void set(const cv::Mat& image); + + cv::Mat get(); -static void destroyNodelet(GtkWidget *widget, gpointer data) + cv::Mat pop(); +}; + +void ThreadSafeImage::set(const cv::Mat& image) { - // We can't actually unload the nodelet from here, but we can at least - // unsubscribe from the image topic. - reinterpret_cast(data)->shutdown(); + boost::unique_lock lock(mutex_); + image_ = image; + condition_.notify_one(); } -#endif +cv::Mat ThreadSafeImage::get() +{ + boost::unique_lock lock(mutex_); + return image_; +} -namespace image_view { +cv::Mat ThreadSafeImage::pop() +{ + cv::Mat image; + { + boost::unique_lock lock(mutex_); + while (image_.empty()) + { + condition_.wait(lock); + } + image = image_; + image_.release(); + } + return image; +} class ImageNodelet : public nodelet::Nodelet { image_transport::Subscriber sub_; - boost::mutex image_mutex_; - cv::Mat last_image_; + boost::thread window_thread_; + + ThreadSafeImage queued_image_, shown_image_; std::string window_name_; bool autosize_; boost::format filename_format_; int count_; - bool initialized_; + + ros::WallTimer gui_timer_; virtual void onInit(); void imageCb(const sensor_msgs::ImageConstPtr& msg); static void mouseCb(int event, int x, int y, int flags, void* param); + static void guiCb(const ros::WallTimerEvent&); + + void windowThread(); public: ImageNodelet(); @@ -92,13 +119,17 @@ class ImageNodelet : public nodelet::Nodelet }; ImageNodelet::ImageNodelet() - : filename_format_(""), count_(0), initialized_(false) + : filename_format_(""), count_(0) { } ImageNodelet::~ImageNodelet() { - cv::destroyWindow(window_name_); + if (window_thread_.joinable()) + { + window_thread_.interrupt(); + window_thread_.join(); + } } void ImageNodelet::onInit() @@ -134,17 +165,13 @@ void ImageNodelet::onInit() local_nh.param("filename_format", format_string, std::string("frame%04i.jpg")); filename_format_.parse(format_string); -#ifdef HAVE_GTK - // Register appropriate handler for when user closes the display window - GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) ); - if (shutdown_on_close) - g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL); - else - g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_); -#endif + // Since cv::startWindowThread() triggers a crash in cv::waitKey() + // if OpenCV is compiled against GTK, we call cv::waitKey() from + // the ROS event loop periodically, instead. + /*cv::startWindowThread();*/ + gui_timer_ = local_nh.createWallTimer(ros::WallDuration(0.1), ImageNodelet::guiCb); - // Start the OpenCV window thread so we don't have to waitKey() somewhere - startWindowThread(); + window_thread_ = boost::thread(&ImageNodelet::windowThread, this); image_transport::ImageTransport it(nh); image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle()); @@ -153,14 +180,6 @@ void ImageNodelet::onInit() void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) { - if ( ! initialized_ ) { - cv::namedWindow(window_name_, autosize_ ? cv::WND_PROP_AUTOSIZE : 0); - cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this); - initialized_ = true; - } - - image_mutex_.lock(); - // We want to scale floating point images so that they display nicely bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos); @@ -168,20 +187,18 @@ void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg) try { cv_bridge::CvtColorForDisplayOptions options; options.do_dynamic_scaling = do_dynamic_scaling; - last_image_ = cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image; + queued_image_.set(cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image); } catch (cv_bridge::Exception& e) { NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'", msg->encoding.c_str(), e.what()); } +} - // Must release the mutex before calling cv::imshow, or can deadlock against - // OpenCV's window mutex. - image_mutex_.unlock(); - if (!last_image_.empty()) { - cv::imshow(window_name_, last_image_); - cv::waitKey(1); - } +void ImageNodelet::guiCb(const ros::WallTimerEvent&) +{ + // Process pending GUI events and return immediately + cv::waitKey(1); } void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param) @@ -199,9 +216,7 @@ void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param) if (event != cv::EVENT_RBUTTONDOWN) return; - boost::lock_guard guard(this_->image_mutex_); - - const cv::Mat &image = this_->last_image_; + cv::Mat image(this_->shown_image_.get()); if (image.empty()) { NODELET_WARN("Couldn't save image, no data!"); @@ -221,6 +236,28 @@ void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param) } } +void ImageNodelet::windowThread() +{ + cv::namedWindow(window_name_, autosize_ ? cv::WND_PROP_AUTOSIZE : 0); + cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this); + + try + { + while (true) + { + cv::Mat image(queued_image_.pop()); + cv::imshow(window_name_, image); + cv::waitKey(1); + shown_image_.set(image); + } + } + catch (const boost::thread_interrupted&) + { + } + + cv::destroyWindow(window_name_); +} + } // namespace image_view // Register the nodelet diff --git a/image_view/src/nodes/extract_images.cpp b/image_view/src/nodes/extract_images.cpp index 9ebdd483a..3264996f5 100644 --- a/image_view/src/nodes/extract_images.cpp +++ b/image_view/src/nodes/extract_images.cpp @@ -42,6 +42,8 @@ #include #include +#include + class ExtractImages { private: @@ -117,7 +119,29 @@ class ExtractImages std::string filename = (filename_format_ % count_).str(); #if !defined(_VIDEO) - cv::imwrite(filename, image); + // Save raw image if the defined file extension is ".raw", otherwise use OpenCV + std::string file_extension = filename.substr(filename.length() - 4, 4); + if (filename.length() >= 4 && file_extension == ".raw") + { + std::ofstream raw_file; + raw_file.open(filename.c_str()); + if (raw_file.is_open() == false) + { + ROS_WARN_STREAM("Failed to open file " << filename); + } + else + { + raw_file.write((char*)(msg->data.data()), msg->data.size()); + raw_file.close(); + } + } + else + { + if (cv::imwrite(filename, image) == false) + { + ROS_WARN_STREAM("Failed to save image " << filename); + } + } #else if(!video_writer) { diff --git a/image_view/src/nodes/image_view.cpp b/image_view/src/nodes/image_view.cpp index fdb8eb3ec..477abfc2f 100644 --- a/image_view/src/nodes/image_view.cpp +++ b/image_view/src/nodes/image_view.cpp @@ -99,7 +99,7 @@ void imageCb(const sensor_msgs::ImageConstPtr& msg) if (g_gui && !g_last_image.empty()) { const cv::Mat &image = g_last_image; cv::imshow(g_window_name, image); - cv::waitKey(3); + cv::waitKey(1); } if (g_pub.getNumSubscribers() > 0) { g_pub.publish(cv_ptr); @@ -134,6 +134,11 @@ static void mouseCb(int event, int x, int y, int flags, void* param) } } +static void guiCb(const ros::WallTimerEvent&) +{ + // Process pending GUI events and return immediately + cv::waitKey(1); +} int main(int argc, char **argv) { @@ -145,6 +150,7 @@ int main(int argc, char **argv) ros::NodeHandle nh; ros::NodeHandle local_nh("~"); + ros::WallTimer gui_timer; // Default window name is the resolved topic name std::string topic = nh.resolveName("image"); @@ -159,7 +165,7 @@ int main(int argc, char **argv) // Handle window size bool autosize; local_nh.param("autosize", autosize, false); - cv::namedWindow(g_window_name, autosize ? (CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED) : 0); + cv::namedWindow(g_window_name, autosize ? (cv::WINDOW_AUTOSIZE | cv::WINDOW_KEEPRATIO | cv::WINDOW_GUI_EXPANDED) : 0); cv::setMouseCallback(g_window_name, &mouseCb); if(autosize == false) @@ -174,8 +180,11 @@ int main(int argc, char **argv) } } - // Start the OpenCV window thread so we don't have to waitKey() somewhere - cv::startWindowThread(); + // Since cv::startWindowThread() triggers a crash in cv::waitKey() + // if OpenCV is compiled against GTK, we call cv::waitKey() from + // the ROS event loop periodically, instead. + /*cv::startWindowThread();*/ + gui_timer = local_nh.createWallTimer(ros::WallDuration(0.1), guiCb); } // Handle transport @@ -208,5 +217,11 @@ int main(int argc, char **argv) if (g_gui) { cv::destroyWindow(g_window_name); } + // The publisher is a global variable, and therefore its scope exceeds those + // of the node handles in main(). Unfortunately, this will cause a crash + // when the publisher tries to shut down and all node handles are gone + // already. Therefore, we shut down the publisher now and avoid the annoying + // mutex assertion. + g_pub.shutdown(); return 0; } diff --git a/image_view/src/nodes/video_recorder.cpp b/image_view/src/nodes/video_recorder.cpp index 00041dc31..3b3420eeb 100644 --- a/image_view/src/nodes/video_recorder.cpp +++ b/image_view/src/nodes/video_recorder.cpp @@ -48,7 +48,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) cv::Size size(image_msg->width, image_msg->height); outputVideo.open(filename, -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 cv::VideoWriter::fourcc(codec.c_str()[0], #else CV_FOURCC(codec.c_str()[0], @@ -70,7 +70,7 @@ void callback(const sensor_msgs::ImageConstPtr& image_msg) } - if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1 / fps)) + if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1.0 / fps)) { // Skip to get video with correct fps return; diff --git a/stereo_image_proc/CHANGELOG.rst b/stereo_image_proc/CHANGELOG.rst index 40ab5174a..4b8db3b1c 100644 --- a/stereo_image_proc/CHANGELOG.rst +++ b/stereo_image_proc/CHANGELOG.rst @@ -1,3 +1,18 @@ +1.13.0 (2019-06-12) +------------------- +* Merge pull request `#375 `_ from fizyr-forks/opencv4 +* Fix OpenCV4 compatibility. +* Merge pull request `#338 `_ from k-okada/arg_sync +* add approximate_sync args in stereo_image_proc.launch +* Merge pull request `#395 `_ from ros-perception/steve_maintain +* adding autonomoustuff mainainer +* adding stevemacenski as maintainer to get emails +* Merge pull request `#392 `_ from bknight-i3drobotics/patch-1 +* Fix typo + Typo in line: 14. Changed 'sterel algorithm' to 'stereo algorithm' +* add approximate_sync args in stereo_image_proc.launch +* Contributors: Hans Gaiser, Joshua Whitley, Kei Okada, Steven Macenski, Yoshito Okada, bknight-i3drobotics, stevemacenski + 1.12.23 (2018-05-10) -------------------- * Removed unused mutable scratch buffers (`#315 `_) diff --git a/stereo_image_proc/include/stereo_image_proc/processor.h b/stereo_image_proc/include/stereo_image_proc/processor.h index 22abdd4b2..e644c2a7c 100644 --- a/stereo_image_proc/include/stereo_image_proc/processor.h +++ b/stereo_image_proc/include/stereo_image_proc/processor.h @@ -56,7 +56,7 @@ class StereoProcessor public: StereoProcessor() -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 { block_matcher_ = cv::StereoBM::create(); sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10); @@ -168,7 +168,7 @@ class StereoProcessor image_proc::Processor mono_processor_; mutable cv::Mat_ disparity16_; // scratch buffer for 16-bit signed disparity image -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 mutable cv::Ptr block_matcher_; // contains scratch buffers for block matching mutable cv::Ptr sg_block_matcher_; #else @@ -220,7 +220,7 @@ inline void StereoProcessor::SET(TYPE param) \ sg_block_matcher_->SET_OPENCV(param); \ } -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 STEREO_IMAGE_PROC_OPENCV3(getPreFilterCap, setPreFilterCap, int, getPreFilterCap, setPreFilterCap) STEREO_IMAGE_PROC_OPENCV3(getCorrelationWindowSize, setCorrelationWindowSize, int, getBlockSize, setBlockSize) STEREO_IMAGE_PROC_OPENCV3(getMinDisparity, setMinDisparity, int, getMinDisparity, setMinDisparity) @@ -272,7 +272,7 @@ inline void StereoProcessor::SET(TYPE param) \ } // BM only -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getPreFilterSize, setPreFilterSize, int, getPreFilterSize, setPreFilterSize) STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getTextureThreshold, setTextureThreshold, int, getTextureThreshold, setTextureThreshold) #else @@ -281,7 +281,7 @@ STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getTextureThreshold, setTextureThreshold, int, #endif // SGBM specific -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 // getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getSgbmMode, setSgbmMode, int, getMode, setMode) STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP1, setP1, int, getP1, setP1) diff --git a/stereo_image_proc/launch/stereo_image_proc.launch b/stereo_image_proc/launch/stereo_image_proc.launch index 605efd79e..c82316fd3 100644 --- a/stereo_image_proc/launch/stereo_image_proc.launch +++ b/stereo_image_proc/launch/stereo_image_proc.launch @@ -5,7 +5,7 @@ - + @@ -27,10 +27,14 @@ + respawn="$(arg respawn)" > + + + respawn="$(arg respawn)" > + + diff --git a/stereo_image_proc/package.xml b/stereo_image_proc/package.xml index ec853e7d0..fb95bd2dc 100644 --- a/stereo_image_proc/package.xml +++ b/stereo_image_proc/package.xml @@ -1,11 +1,13 @@ stereo_image_proc - 1.12.23 + 1.13.0 Stereo and single image rectification and disparity processing. Patrick Mihelich Kurt Konolige Jeremy Leibs Vincent Rabaud + Steven Macenski + Autonomoustuff team BSD http://www.ros.org/wiki/stereo_image_proc diff --git a/stereo_image_proc/src/libstereo_image_proc/processor.cpp b/stereo_image_proc/src/libstereo_image_proc/processor.cpp index cecbf9728..4eec6346a 100644 --- a/stereo_image_proc/src/libstereo_image_proc/processor.cpp +++ b/stereo_image_proc/src/libstereo_image_proc/processor.cpp @@ -90,7 +90,7 @@ void StereoProcessor::processDisparity(const cv::Mat& left_rect, const cv::Mat& // Block matcher produces 16-bit signed (fixed point) disparity image if (current_stereo_algorithm_ == BM) -#if CV_MAJOR_VERSION == 3 +#if CV_MAJOR_VERSION >= 3 block_matcher_->compute(left_rect, right_rect, disparity16_); else sg_block_matcher_->compute(left_rect, right_rect, disparity16_);