diff --git a/camera_calibration/CMakeLists.txt b/camera_calibration/CMakeLists.txt deleted file mode 100644 index 469d3b844..000000000 --- a/camera_calibration/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 2.8) -project(camera_calibration) - -find_package(catkin REQUIRED) -catkin_package() - -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - # Unit test of calibrator.py - catkin_add_nosetests(test/directed.py) - - # Tests simple calibration dataset - catkin_download_test_data(camera_calibration.tar.gz http://download.ros.org/data/camera_calibration/camera_calibration.tar.gz - DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - MD5 6da43ea314640a4c15dd7a90cbc3aee0 - ) - - # Tests multiple checkerboards - catkin_download_test_data(multi_board_calibration.tar.gz http://download.ros.org/data/camera_calibration/multi_board_calibration.tar.gz - DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests - MD5 ddc0f69582d140e33f9d3bfb681956bb - ) - catkin_add_nosetests(test/multiple_boards.py) -endif() - -catkin_install_python(PROGRAMS nodes/cameracalibrator.py - nodes/cameracheck.py - scripts/tarfile_calibration.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/camera_calibration/src/camera_calibration/__init__.py b/camera_calibration/camera_calibration/__init__.py similarity index 100% rename from camera_calibration/src/camera_calibration/__init__.py rename to camera_calibration/camera_calibration/__init__.py diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/camera_calibration/calibrator.py similarity index 98% rename from camera_calibration/src/camera_calibration/calibrator.py rename to camera_calibration/camera_calibration/calibrator.py index 4d68f9d6b..7c74877ec 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/camera_calibration/calibrator.py @@ -39,6 +39,7 @@ from io import BytesIO import cv2 import cv_bridge +print(cv_bridge.__file__) import image_geometry import math import numpy.linalg @@ -49,7 +50,6 @@ import time from distutils.version import LooseVersion - # Supported calibration patterns class Patterns: Chessboard, Circles, ACircles = list(range(3)) @@ -435,10 +435,10 @@ def lrmsg(self, d, k, r, p): msg.distortion_model = "rational_polynomial" else: msg.distortion_model = "plumb_bob" - msg.D = numpy.ravel(d).copy().tolist() - msg.K = numpy.ravel(k).copy().tolist() - msg.R = numpy.ravel(r).copy().tolist() - msg.P = numpy.ravel(p).copy().tolist() + msg.d = numpy.ravel(d).copy().tolist() + msg.k = numpy.ravel(k).copy().tolist() + msg.r = numpy.ravel(r).copy().tolist() + msg.p = numpy.ravel(p).copy().tolist() return msg def lrreport(self, d, k, r, p): @@ -668,10 +668,10 @@ def from_message(self, msg, alpha = 0.0): """ Initialize the camera calibration from a CameraInfo message """ self.size = (msg.width, msg.height) - self.intrinsics = numpy.array(msg.K, dtype=numpy.float64, copy=True).reshape((3, 3)) - self.distortion = numpy.array(msg.D, dtype=numpy.float64, copy=True).reshape((len(msg.D), 1)) - self.R = numpy.array(msg.R, dtype=numpy.float64, copy=True).reshape((3, 3)) - self.P = numpy.array(msg.P, dtype=numpy.float64, copy=True).reshape((3, 4)) + self.intrinsics = numpy.array(msg.k, dtype=numpy.float64, copy=True).reshape((3, 3)) + self.distortion = numpy.array(msg.d, dtype=numpy.float64, copy=True).reshape((len(msg.d), 1)) + self.R = numpy.array(msg.r, dtype=numpy.float64, copy=True).reshape((3, 3)) + self.P = numpy.array(msg.p, dtype=numpy.float64, copy=True).reshape((3, 4)) self.set_alpha(0.0) @@ -798,8 +798,9 @@ def do_tarfile_save(self, tf): """ Write images and calibration solution to a tarfile object """ def taradd(name, buf): - if isinstance(buf, basestring): - s = StringIO(buf) + if isinstance(buf, str): + buf = bytes(buf, encoding="utf-8") + s = BytesIO(buf) else: s = BytesIO(buf) ti = tarfile.TarInfo(name) @@ -1121,8 +1122,9 @@ def do_tarfile_save(self, tf): [("right-%04d.png" % i, im) for i,(_, _, im) in enumerate(self.db)]) def taradd(name, buf): - if isinstance(buf, basestring): - s = StringIO(buf) + if isinstance(buf, str): + buf = bytes(buf, encoding="utf-8") + s = BytesIO(buf) else: s = BytesIO(buf) ti = tarfile.TarInfo(name) @@ -1147,4 +1149,4 @@ def do_tarfile_calibration(self, filename): ##\todo Check that the filenames match and stuff - self.cal(limages, rimages) + self.cal(limages, rimages) \ No newline at end of file diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/camera_calibration/camera_calibrator.py similarity index 81% rename from camera_calibration/src/camera_calibration/camera_calibrator.py rename to camera_calibration/camera_calibration/camera_calibrator.py index bb9af7c14..3c51e8a5a 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/camera_calibration/camera_calibrator.py @@ -36,7 +36,8 @@ import message_filters import numpy import os -import rospy +import rclpy +from rclpy.node import Node import sensor_msgs.msg import sensor_msgs.srv import threading @@ -72,7 +73,7 @@ def run(self): cv2.imshow("display", im) k = cv2.waitKey(6) & 0xFF if k in [27, ord('q')]: - rospy.signal_shutdown('Quit') + rclpy.shutdown() elif k == ord('s'): self.opencv_calibration_node.screendump(im) @@ -90,42 +91,40 @@ def run(self): self.function(self.queue[0]) -class CalibrationNode: - def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, - pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0): - if service_check: +class CalibrationNode(Node): + def __init__(self, boards, service_check = False, synchronizer = message_filters.TimeSynchronizer, flags = 0, + pattern=Patterns.Chessboard, camera_name='camera',camera='camera',left_camera='left_camera',right_camera='right_camera', image='image', checkerboard_flags=0): + # TODO will enable this function as soon as set_camera_info enabled + # 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"]: - remapped = rospy.remap_name(svcname) - if remapped != svcname: - fullservicename = "%s/set_camera_info" % remapped - print("Waiting for service", fullservicename, "...") - try: - rospy.wait_for_service(fullservicename, 5) - print("OK") - except rospy.ROSException: - print("Service not found") - rospy.signal_shutdown('Quit') - + # for svcname in ["camera", "left_camera", "right_camera"]: + # remapped = svcname + # if remapped != svcname: + # fullservicename = "%s/set_camera_info" % remapped + # print("Waiting for service", fullservicename, "...") + # try: + # rclpy.wait_for_service(fullservicename, 5) + # print("OK") + # except: + # print("Service not found") + # rclpy.shutdown() + super().__init__(self._node_name) self._boards = boards self._calib_flags = flags self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name - lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) - rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) + lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, left_camera) + rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, right_camera) ts = synchronizer([lsub, rsub], 4) ts.registerCallback(self.queue_stereo) - msub = message_filters.Subscriber('image', sensor_msgs.msg.Image) + msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, image) msub.registerCallback(self.queue_monocular) - self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), - sensor_msgs.srv.SetCameraInfo) - self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), - sensor_msgs.srv.SetCameraInfo) - self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), - sensor_msgs.srv.SetCameraInfo) + self.set_camera_info_cli = self.create_client(sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % camera) + self.set_left_camera_cli = self.create_client(sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % left_camera) + self.set_right_camera_cli = self.create_client(sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % right_camera) self.q_mono = deque([], 1) self.q_stereo = deque([], 1) @@ -180,33 +179,36 @@ def handle_stereo(self, msg): def check_set_camera_info(self, response): - if response.success: + if response.result().success: return True for i in range(10): print("!" * 80) print() - print("Attempt to set camera info failed: " + response.status_message) + print("Attempt to set camera info failed: " + response.result().status_message) print() for i in range(10): print("!" * 80) print() - rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message) + self.get_logger().error('Unable to set camera info for calibration. Failure message: %s' % response.result().status_message) return False def do_upload(self): self.c.report() print(self.c.ost()) info = self.c.as_message() - + req = sensor_msgs.srv.SetCameraInfo.Request() rv = True if self.c.is_mono: - response = self.set_camera_info_service(info) + req.camera_info = info + response = self.set_camera_info_cli.call_async(req) rv = self.check_set_camera_info(response) else: - response = self.set_left_camera_info_service(info[0]) + req.camera_info = info[0] + response = self.set_left_camera_info_service(req) rv = rv and self.check_set_camera_info(response) - response = self.set_right_camera_info_service(info[1]) + req.camera_info = info[1] + response = self.set_right_camera_info_service(req) rv = rv and self.check_set_camera_info(response) return rv @@ -218,14 +220,12 @@ class OpenCVCalibrationNode(CalibrationNode): FONT_THICKNESS = 2 def __init__(self, *args, **kwargs): - - CalibrationNode.__init__(self, *args, **kwargs) - + self._node_name = 'camera_calibration' self.queue_display = deque([], 1) self.display_thread = DisplayThread(self.queue_display, self) self.display_thread.setDaemon(True) self.display_thread.start() - + CalibrationNode.__init__(self, *args, **kwargs) @classmethod def putText(cls, img, text, org, color = (0,0,0)): cv2.putText(img, text, org, cls.FONT_FACE, cls.FONT_SCALE, color, thickness = cls.FONT_THICKNESS) @@ -243,9 +243,11 @@ def on_mouse(self, event, x, y, flags, param): if 280 <= y < 380: self.c.do_save() elif 380 <= y < 480: - # Only shut down if we set camera info correctly, #3993 - if self.do_upload(): - rospy.signal_shutdown('Quit') + # TODO will enabel do_upload function as soon as set_camera_info service is ready for ros2 + print("set_camera_info isn't ready for ros2, do nothing!") + # Only shut down if we set camera info correctly, #399f we set camera info correctly, #3993 + # if self.do_upload(): + # rclpy.shutdown() def on_scale(self, scalevalue): if self.c.calibrated: @@ -349,4 +351,4 @@ 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.queue_display.append(display) \ No newline at end of file diff --git a/camera_calibration/src/camera_calibration/camera_checker.py b/camera_calibration/camera_calibration/camera_checker.py similarity index 91% rename from camera_calibration/src/camera_calibration/camera_checker.py rename to camera_calibration/camera_calibration/camera_checker.py index d27b893e0..03a485885 100755 --- a/camera_calibration/src/camera_calibration/camera_checker.py +++ b/camera_calibration/camera_calibration/camera_checker.py @@ -37,7 +37,8 @@ import functools import message_filters import numpy -import rospy +import rclpy +from rclpy.node import Node import sensor_msgs.msg import sensor_msgs.srv import threading @@ -69,27 +70,28 @@ def __init__(self, queue, function): self.function = function def run(self): - while not rospy.is_shutdown(): - while not rospy.is_shutdown(): + while not rclpy.is_shutdown(): + while not rclpy.is_shutdown(): m = self.queue.get() if self.queue.empty(): break self.function(m) -class CameraCheckerNode: +class CameraCheckerNode(Node): - def __init__(self, chess_size, dim, approximate=0): + def __init__(self, chess_size, dim, monocular, stereo, approximate=0): self.board = ChessboardInfo() self.board.n_cols = chess_size[0] self.board.n_rows = chess_size[1] self.board.dim = dim - + self._node_name = "camera_checker" + super().__init__(self._node_name) # make sure n_cols is not smaller than n_rows, otherwise error computation will be off if self.board.n_cols < self.board.n_rows: self.board.n_cols, self.board.n_rows = self.board.n_rows, self.board.n_cols - image_topic = rospy.resolve_name("monocular") + "/image_rect" - camera_topic = rospy.resolve_name("monocular") + "/camera_info" + image_topic = monocular + "/image_rect" + camera_topic = monocular + "/camera_info" tosync_mono = [ (image_topic, sensor_msgs.msg.Image), @@ -104,10 +106,10 @@ def __init__(self, chess_size, dim, approximate=0): tsm = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10) tsm.registerCallback(self.queue_monocular) - left_topic = rospy.resolve_name("stereo") + "/left/image_rect" - left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info" - right_topic = rospy.resolve_name("stereo") + "/right/image_rect" - right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info" + left_topic = stereo + "/left/image_rect" + left_camera_topic = stereo + "/left/camera_info" + right_topic = stereo + "/right/image_rect" + right_camera_topic = stereo + "/right/camera_info" tosync_stereo = [ (left_topic, sensor_msgs.msg.Image), diff --git a/camera_calibration/camera_calibration/nodes/__init__.py b/camera_calibration/camera_calibration/nodes/__init__.py new file mode 100644 index 000000000..59c2b95c5 --- /dev/null +++ b/camera_calibration/camera_calibration/nodes/__init__.py @@ -0,0 +1,15 @@ +""" + Copyright (c) 2018 Intel Corporation + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +""" diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/camera_calibration/nodes/cameracalibrator.py similarity index 85% rename from camera_calibration/nodes/cameracalibrator.py rename to camera_calibration/camera_calibration/nodes/cameracalibrator.py index 206962e1d..c5db9285c 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/camera_calibration/nodes/cameracalibrator.py @@ -36,19 +36,31 @@ import functools import message_filters import os -import rospy +import rclpy from camera_calibration.camera_calibrator import OpenCVCalibrationNode from camera_calibration.calibrator import ChessboardInfo, Patterns from message_filters import ApproximateTimeSynchronizer -def main(): +def main(args=None): from optparse import OptionParser, OptionGroup parser = OptionParser("%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]", description=None) parser.add_option("-c", "--camera_name", type="string", default='narrow_stereo', help="name of the camera to appear in the calibration file") + parser.add_option("--image", + type="string", default='image', + help="name of the image topic to appear in the calibration file") + parser.add_option("--camera", + type="string", default='camera', + help="name of the camera topic to appear in the calibration file") + parser.add_option("--left_camera", + type="string", default='left_camera', + help="name of the left camera to appear in the calibration file") + parser.add_option("--right_camera", + type="string", default='right_camera', + help="name of the right camera to appear in the calibration file") group = OptionGroup(parser, "Chessboard Options", "You must specify one or more chessboards as pairs of --size and --square options.") group.add_option("-p", "--pattern", @@ -66,7 +78,7 @@ def main(): type="float", default=0.0, help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras") group.add_option("--no-service-check", - action="store_false", dest="service_check", default=True, + action="store_false", dest="service_check", default=False, help="disable check for set_camera_info services at startup") parser.add_option_group(group) group = OptionGroup(parser, "Calibration Optimizer Options") @@ -141,14 +153,12 @@ def main(): else: checkerboard_flags = cv2.CALIB_CB_FAST_CHECK - rospy.init_node('cameracalibrator') - node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name, - checkerboard_flags=checkerboard_flags) - rospy.spin() - + rclpy.init(args=args) + node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, camera_name=options.camera_name, camera=options.camera, + left_camera=options.left_camera, right_camera=options.right_camera,image=options.image, checkerboard_flags=checkerboard_flags) + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() if __name__ == "__main__": - try: - main() - except Exception as e: - import traceback - traceback.print_exc() + main() + diff --git a/camera_calibration/nodes/cameracheck.py b/camera_calibration/camera_calibration/nodes/cameracheck.py similarity index 88% rename from camera_calibration/nodes/cameracheck.py rename to camera_calibration/camera_calibration/nodes/cameracheck.py index 6af00d621..a20a642cc 100755 --- a/camera_calibration/nodes/cameracheck.py +++ b/camera_calibration/camera_calibration/nodes/cameracheck.py @@ -32,16 +32,18 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rospy +import rclpy from camera_calibration.camera_checker import CameraCheckerNode -def main(): +def main(args=None): from optparse import OptionParser - rospy.init_node('cameracheck') + rclpy.init(agrs=args) parser = OptionParser() parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]") parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]") + parser.add_option("--monocular", defalut="", help="specify monocular topic") + parser.add_option("--stereo", defalut="", help="specify stereo topic") parser.add_option("--approximate", type="float", default=0.0, help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras") @@ -50,8 +52,8 @@ def main(): size = tuple([int(c) for c in options.size.split('x')]) dim = float(options.square) approximate = float(options.approximate) - CameraCheckerNode(size, dim, approximate) - rospy.spin() + node = CameraCheckerNode(size, dim, options.monocular, options.stereo, approximate) + rclpy.spin(node) if __name__ == "__main__": main() diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml index 7946de45a..b4f78f679 100644 --- a/camera_calibration/package.xml +++ b/camera_calibration/package.xml @@ -9,22 +9,22 @@ Patrick Mihelich Vincent Rabaud - catkin - - rostest - - cv_bridge - image_geometry - message_filters - rospy - std_srvs - sensor_msgs + example_interfaces + cv_bridge + image_geometry + message_filters + rclpy + std_srvs + sensor_msgs + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest BSD http://www.ros.org/wiki/camera_calibration - - + ament_python diff --git a/camera_calibration/resource/camera_calibration b/camera_calibration/resource/camera_calibration new file mode 100644 index 000000000..e69de29bb diff --git a/camera_calibration/scripts/tarfile_calibration.py b/camera_calibration/scripts/tarfile_calibration.py deleted file mode 100755 index 5d552ac5f..000000000 --- a/camera_calibration/scripts/tarfile_calibration.py +++ /dev/null @@ -1,235 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, 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. - -import os -import sys -import numpy - -import cv2 -import cv_bridge -import tarfile - -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo - -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() - cv2.destroyWindow(win_name) - if k in [27, ord('q')]: - rospy.signal_shutdown('Quit') - - -def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0): - if mono: - calibrator = MonoCalibrator(boards, calib_flags) - else: - calibrator = StereoCalibrator(boards, calib_flags) - - calibrator.do_tarfile_calibration(tarname) - - print(calibrator.ost()) - - if upload: - info = calibrator.as_message() - if mono: - set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo) - - response = set_camera_info_service(info) - if not response.success: - raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") - else: - set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo) - set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo) - - response1 = set_left_camera_info_service(info[0]) - response2 = set_right_camera_info_service(info[1]) - if not (response1.success and response2.success): - raise RuntimeError("connected to set_camera_info service, but failed setting camera_info") - - if visualize: - - #Show rectified images - calibrator.set_alpha(alpha) - - archive = tarfile.open(tarname, 'r') - if mono: - for f in archive.getnames(): - if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')): - filedata = archive.extractfile(f).read() - file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) - im=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) - - bridge = cv_bridge.CvBridge() - try: - msg=bridge.cv2_to_imgmsg(im, "bgr8") - except cv_bridge.CvBridgeError as e: - print(e) - - #handle msg returns the recitifed image with corner detection once camera is calibrated. - drawable=calibrator.handle_msg(msg) - vis=numpy.asarray( drawable.scrib[:,:]) - #Display. Name of window:f - display(f, vis) - else: - limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ] - limages.sort() - rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ] - rimages.sort() - - if not len(limages) == len(rimages): - raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages))) - - for i in range(len(limages)): - l=limages[i] - r=rimages[i] - - if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')): - # LEFT IMAGE - filedata = archive.extractfile(l).read() - file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) - im_left=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) - - bridge = cv_bridge.CvBridge() - try: - msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8") - except cv_bridge.CvBridgeError as e: - print(e) - - #RIGHT IMAGE - filedata = archive.extractfile(r).read() - file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8) - im_right=cv2.imdecode(file_bytes,cv2.IMREAD_COLOR) - try: - msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8") - except cv_bridge.CvBridgeError as e: - print(e) - - drawable=calibrator.handle_msg([ msg_left,msg_right] ) - - h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2] - vis = numpy.zeros((h, w*2, 3), numpy.uint8) - vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:]) - vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:]) - - display(l+" "+r,vis) - - -if __name__ == '__main__': - from optparse import OptionParser - parser = OptionParser("%prog TARFILE [ opts ]") - parser.add_option("--mono", default=False, action="store_true", dest="mono", - help="Monocular calibration only. Calibrates left images.") - parser.add_option("-s", "--size", default=[], action="append", dest="size", - help="specify chessboard size as NxM [default: 8x6]") - parser.add_option("-q", "--square", default=[], action="append", dest="square", - help="specify chessboard square size in meters [default: 0.108]") - parser.add_option("--upload", default=False, action="store_true", dest="upload", - help="Upload results to camera(s).") - parser.add_option("--fix-principal-point", action="store_true", default=False, - help="fix the principal point at the image center") - parser.add_option("--fix-aspect-ratio", action="store_true", default=False, - help="enforce focal lengths (fx, fy) are equal") - parser.add_option("--zero-tangent-dist", action="store_true", default=False, - help="set tangential distortion coefficients (p1, p2) to zero") - parser.add_option("-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS", - help="number of radial distortion coefficients to use (up to 6, default %default)") - parser.add_option("--visualize", action="store_true", default=False, - help="visualize rectified images after calibration") - parser.add_option("-a", "--alpha", type="float", default=1.0, metavar="ALPHA", - help="zoom for visualization of rectifies images. Ranges from 0 (zoomed in, all pixels in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). default %default)") - - options, args = parser.parse_args() - - if len(options.size) != len(options.square): - parser.error("Number of size and square inputs must be the same!") - - if not options.square: - options.square.append("0.108") - options.size.append("8x6") - - boards = [] - for (sz, sq) in zip(options.size, options.square): - info = ChessboardInfo() - info.dim = float(sq) - size = tuple([int(c) for c in sz.split('x')]) - info.n_cols = size[0] - info.n_rows = size[1] - - boards.append(info) - - if not boards: - parser.error("Must supply at least one chessboard") - - if not args: - parser.error("Must give tarfile name") - if not os.path.exists(args[0]): - parser.error("Tarfile %s does not exist. Please select valid tarfile" % args[0]) - - tarname = args[0] - - num_ks = options.k_coefficients - - calib_flags = 0 - if options.fix_principal_point: - calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - if options.fix_aspect_ratio: - calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO - if options.zero_tangent_dist: - calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST - if (num_ks > 3): - calib_flags |= cv2.CALIB_RATIONAL_MODEL - if (num_ks < 6): - calib_flags |= cv2.CALIB_FIX_K6 - if (num_ks < 5): - calib_flags |= cv2.CALIB_FIX_K5 - if (num_ks < 4): - calib_flags |= cv2.CALIB_FIX_K4 - if (num_ks < 3): - calib_flags |= cv2.CALIB_FIX_K3 - if (num_ks < 2): - calib_flags |= cv2.CALIB_FIX_K2 - if (num_ks < 1): - calib_flags |= cv2.CALIB_FIX_K1 - - cal_from_tarfile(boards, tarname, options.mono, options.upload, calib_flags, options.visualize, options.alpha) diff --git a/camera_calibration/setup.cfg b/camera_calibration/setup.cfg new file mode 100644 index 000000000..5cc768acf --- /dev/null +++ b/camera_calibration/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/camera_calibration +[install] +install-scripts=$base/lib/camera_calibration diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py index fd51af2ee..e8e12bfa9 100644 --- a/camera_calibration/setup.py +++ b/camera_calibration/setup.py @@ -1,9 +1,55 @@ #!/usr/bin/env python -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +""" + Copyright (c) 2018 Intel Corporation -d = generate_distutils_setup() -d['packages'] = ['camera_calibration'] -d['package_dir'] = {'':'src'} + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +""" + +from setuptools import find_packages +from setuptools import setup + +package_name = 'camera_calibration' + +setup( + name=package_name, + version='1.12.23', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + author='James Bowman', + author_email='vincent.rabaud@gmail.com', + zip_safe=True, + keywords=['ROS', 'camera_calibration'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description=( + 'camera_calibration for ROS2' + ), + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'cameracalibrator = camera_calibration.nodes.cameracalibrator:main', + 'cameracheck = camera_calibration.nodes.cameracheck:main', + ], + }, +) -setup(**d)