Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update fisheye distortion model definition #581

Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .github/workflows/basic-build-ci.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,12 @@ on:
- push

jobs:
build-noetic:
build-melodic:
runs-on: ubuntu-latest
strategy:
fail-fast: false
container:
image: ros:noetic-perception
image: ros:melodic-perception
steps:
- name: Checkout repo
uses: actions/checkout@v2
Expand Down
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
image_pipeline
==============

[![](https://github.com/ros-perception/image_pipeline/workflows/Basic%20Build%20Workflow/badge.svg?branch=noetic)](https://github.com/ros-perception/image_pipeline/actions)
[![](https://github.com/ros-perception/image_pipeline/workflows/Basic%20Build%20Workflow/badge.svg?branch=melodic)](https://github.com/ros-perception/image_pipeline/actions)

This package fills the gap between getting raw images from a camera driver and higher-level vision processing.

Expand Down
17 changes: 4 additions & 13 deletions camera_calibration/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,18 +1,9 @@
1.15.2 (2020-05-19)
1.15.0 (2020-05-18)
-------------------

1.15.1 (2020-05-18)
-------------------

1.15.0 (2020-05-14)
-------------------
* Python 3 compatibility (`#530 <https://github.com/ros-perception/image_pipeline/issues/530>`_)
* cmake_minimum_required to 3.0.2
* Adapted to OpenCV4
* import setup from setuptools instead of distutils-core
* Apply `#509 <https://github.com/ros-perception/image_pipeline/issues/509>`_ and `#526 <https://github.com/ros-perception/image_pipeline/issues/526>`_ to Noetic Branch (`#528 <https://github.com/ros-perception/image_pipeline/issues/528>`_)
* Fix import path on camera_calibrator.py (`#509 <https://github.com/ros-perception/image_pipeline/issues/509>`_)
* Fixes `#501 <https://github.com/ros-perception/image_pipeline/issues/501>`_: self.size is set before dumping calibration parameters in calibrator.py do_calibration(self, dump) (`#502 <https://github.com/ros-perception/image_pipeline/issues/502>`_)
* Contributors: Joshua Whitley, Stewart Jamieson
Not sure how this one made it through. Thanks for the fix!
* Contributors: Gonçalo Camelo Neves Pereira, Stewart Jamieson

1.14.0 (2020-01-12)
-------------------
Expand Down
2 changes: 1 addition & 1 deletion camera_calibration/package.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package>
<name>camera_calibration</name>
<version>1.15.2</version>
<version>1.15.0</version>
<description>
camera_calibration allows easy calibration of monocular or stereo
cameras using a checkerboard calibration target.
Expand Down
84 changes: 30 additions & 54 deletions camera_calibration/src/camera_calibration/calibrator.py
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,11 @@
# 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:
from io import StringIO
from io import BytesIO
import cv2
import cv_bridge
Expand Down Expand Up @@ -60,7 +65,7 @@ class CalibrationException(Exception):
pass

# TODO: Make pattern per-board?
class ChessboardInfo():
class ChessboardInfo(object):
def __init__(self, pattern="chessboard", n_cols = 0, n_rows = 0, dim = 0.0, marker_size = 0.0, aruco_dict = None):
self.pattern = pattern
self.n_cols = n_cols
Expand Down Expand Up @@ -295,13 +300,13 @@ def _get_dist_model(dist_params, cam_model):
else:
dist_model = "plumb_bob"
elif CAMERA_MODEL.FISHEYE == cam_model:
dist_model = "fisheye"
dist_model = "equidistant"
else:
dist_model = "unknown"
return dist_model

# TODO self.size needs to come from CameraInfo, full resolution
class Calibrator():
class Calibrator(object):
"""
Base class for calibration system
"""
Expand Down Expand Up @@ -469,7 +474,7 @@ def mk_object_points(self, boards, use_board_size = False):
num_pts = b.n_cols * b.n_rows
opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32)
for j in range(num_pts):
opts_loc[j, 0, 0] = (j // b.n_cols)
opts_loc[j, 0, 0] = (j / b.n_cols)
if self.pattern == Patterns.ACircles:
opts_loc[j, 0, 1] = 2*(j % b.n_cols) + (opts_loc[j, 0, 0] % 2)
else:
Expand Down Expand Up @@ -649,7 +654,7 @@ def format_mat(x, precision):
" rows: 3",
" cols: 3",
" data: " + format_mat(k, 5),
"distortion_model: " + dist_model,
"camera_model: " + dist_model,
"distortion_coefficients:",
" rows: 1",
" cols: %d" % d.size,
Expand Down Expand Up @@ -684,7 +689,7 @@ def image_from_archive(archive, name):
imagefiledata.resize((1, imagefiledata.size))
return cv2.imdecode(imagefiledata, cv2.IMREAD_COLOR)

class ImageDrawable():
class ImageDrawable(object):
"""
Passed to CalibrationNode after image handled. Allows plotting of images
with detected corner points
Expand Down Expand Up @@ -753,8 +758,6 @@ def cal_fromcorners(self, good):
"""
:param good: Good corner positions and boards
:type good: [(corners, ChessboardInfo)]


"""

(ipts, ids, boards) = zip(*good)
Expand Down Expand Up @@ -872,15 +875,15 @@ def linear_error_from_image(self, image):
Detect the checkerboard and compute the linear error.
Mainly for use in tests.
"""
_, corners, _, ids, board, _ = self.downsample_and_detect(image)
_, corners, _, _, board, _ = self.downsample_and_detect(image)
if corners is None:
return None

undistorted = self.undistort_points(corners)
return self.linear_error(undistorted, ids, board)
return self.linear_error(undistorted, board)

@staticmethod
def linear_error(corners, ids, b):
def linear_error(corners, b):

"""
Returns the linear error for a set of corners detected in the unrectified image.
Expand All @@ -889,46 +892,19 @@ def linear_error(corners, ids, b):
if corners is None:
return None

corners = numpy.squeeze(corners)

def pt2line(x0, y0, x1, y1, x2, y2):
""" point is (x0, y0), line is (x1, y1, x2, y2) """
return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)

n_cols = b.n_cols
n_rows = b.n_rows
if b.pattern == 'charuco':
n_cols -= 1
n_rows -= 1
n_pts = n_cols * n_rows

if ids is None:
ids = numpy.arange(n_pts).reshape((n_pts, 1))

ids_to_idx = dict((ids[i, 0], i) for i in range(len(ids)))

cc = b.n_cols
cr = b.n_rows
errors = []
for row in range(n_rows):
row_min = row * n_cols
row_max = (row+1) * n_cols
pts_in_row = [x for x in ids if row_min <= x < row_max]

# not enough points to calculate error
if len(pts_in_row) <= 2: continue

left_pt = min(pts_in_row)[0]
right_pt = max(pts_in_row)[0]
x_left = corners[ids_to_idx[left_pt], 0]
y_left = corners[ids_to_idx[left_pt], 1]
x_right = corners[ids_to_idx[right_pt], 0]
y_right = corners[ids_to_idx[right_pt], 1]

for pt in pts_in_row:
if pt[0] in (left_pt, right_pt): continue
x = corners[ids_to_idx[pt[0]], 0]
y = corners[ids_to_idx[pt[0]], 1]
errors.append(pt2line(x, y, x_left, y_left, x_right, y_right))

for r in range(cr):
(x1, y1) = corners[(cc * r) + 0, 0]
(x2, y2) = corners[(cc * r) + cc - 1, 0]
for i in range(1, cc - 1):
(x0, y0) = corners[(cc * r) + i, 0]
errors.append(pt2line(x0, y0, x1, y1, x2, y2))
if errors:
return math.sqrt(sum([e**2 for e in errors]) / len(errors))
else:
Expand Down Expand Up @@ -961,7 +937,7 @@ def handle_msg(self, msg):
if corners is not None:
# Report linear error
undistorted = self.undistort_points(corners)
linear_error = self.linear_error(undistorted, ids, board)
linear_error = self.linear_error(undistorted, board)

# Draw rectified corners
scrib_src = undistorted.copy()
Expand Down Expand Up @@ -1016,8 +992,8 @@ def do_tarfile_save(self, tf):
""" Write images and calibration solution to a tarfile object """

def taradd(name, buf):
if isinstance(buf, str):
s = BytesIO(buf.encode('utf-8'))
if isinstance(buf, basestring):
s = StringIO(buf)
else:
s = BytesIO(buf)
ti = tarfile.TarInfo(name)
Expand Down Expand Up @@ -1323,8 +1299,8 @@ def handle_msg(self, msg):
epierror = -1

# Get display-images-to-be and detections of the calibration target
lscrib_mono, lcorners, ldownsampled_corners, lids, lboard, (x_scale, y_scale) = self.downsample_and_detect(lgray)
rscrib_mono, rcorners, rdownsampled_corners, rids, rboard, _ = self.downsample_and_detect(rgray)
lscrib_mono, lcorners, lids, ldownsampled_corners, lboard, (x_scale, y_scale) = self.downsample_and_detect(lgray)
rscrib_mono, rcorners, rids, rdownsampled_corners, rboard, _ = self.downsample_and_detect(rgray)

if self.calibrated:
# Show rectified images
Expand Down Expand Up @@ -1374,7 +1350,7 @@ def handle_msg(self, msg):
params = self.get_parameters(lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0]))
if self.is_good_sample(params, lcorners, lids, self.last_frame_corners, self.last_frame_ids):
self.db.append( (params, lgray, rgray) )
self.good_corners.append( (lcorners, rcorners, lids, rids, lboard) )
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
Expand Down Expand Up @@ -1407,8 +1383,8 @@ 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, str):
s = BytesIO(buf.encode('utf-8'))
if isinstance(buf, basestring):
s = StringIO(buf)
else:
s = BytesIO(buf)
ti = tarfile.TarInfo(name)
Expand Down
14 changes: 2 additions & 12 deletions depth_image_proc/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,17 +1,7 @@
1.15.2 (2020-05-19)
1.15.0 (2020-05-18)
-------------------

1.15.1 (2020-05-18)
-------------------

1.15.0 (2020-05-14)
-------------------
* Python 3 compatibility (`#530 <https://github.com/ros-perception/image_pipeline/issues/530>`_)
* cmake_minimum_required to 3.0.2
* Adapted to OpenCV4
* import setup from setuptools instead of distutils-core
* updated install locations for better portability. (`#500 <https://github.com/ros-perception/image_pipeline/issues/500>`_)
* Contributors: Joshua Whitley, Sean Yen
* Contributors: Sean Yen

1.14.0 (2020-01-12)
-------------------
Expand Down
2 changes: 1 addition & 1 deletion depth_image_proc/package.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package>
<name>depth_image_proc</name>
<version>1.15.2</version>
<version>1.15.0</version>
<description>

Contains nodelets for processing depth images such as those
Expand Down
13 changes: 1 addition & 12 deletions image_pipeline/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,20 +2,9 @@
Changelog for package image_pipeline
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

1.15.2 (2020-05-19)
1.15.0 (2020-05-18)
-------------------

1.15.1 (2020-05-18)
-------------------

1.15.0 (2020-05-14)
-------------------
* Python 3 compatibility (`#530 <https://github.com/ros-perception/image_pipeline/issues/530>`_)
* cmake_minimum_required to 3.0.2
* Adapted to OpenCV4
* import setup from setuptools instead of distutils-core
* Contributors: Joshua Whitley

1.14.0 (2020-01-12)
-------------------

Expand Down
2 changes: 1 addition & 1 deletion image_pipeline/package.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package>
<name>image_pipeline</name>
<version>1.15.2</version>
<version>1.15.0</version>
<description>image_pipeline fills the gap between getting raw images from a camera driver and higher-level vision processing.</description>
<author>Patrick Mihelich</author>
<author>James Bowman</author>
Expand Down
14 changes: 2 additions & 12 deletions image_proc/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,17 +1,7 @@
1.15.2 (2020-05-19)
1.15.0 (2020-05-18)
-------------------

1.15.1 (2020-05-18)
-------------------

1.15.0 (2020-05-14)
-------------------
* Python 3 compatibility (`#530 <https://github.com/ros-perception/image_pipeline/issues/530>`_)
* cmake_minimum_required to 3.0.2
* Adapted to OpenCV4
* import setup from setuptools instead of distutils-core
* updated install locations for better portability. (`#500 <https://github.com/ros-perception/image_pipeline/issues/500>`_)
* Contributors: Joshua Whitley, Sean Yen
* Contributors: Sean Yen

1.14.0 (2020-01-12)
-------------------
Expand Down
2 changes: 1 addition & 1 deletion image_proc/package.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package>
<name>image_proc</name>
<version>1.15.2</version>
<version>1.15.0</version>
<description>Single image rectification and color processing.</description>
<author>Patrick Mihelich</author>
<author>Kurt Konolige</author>
Expand Down
5 changes: 5 additions & 0 deletions image_publisher/CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,11 @@ Changelog for package image_publisher
* updated install locations for better portability. (`#500 <https://github.com/ros-perception/image_pipeline/issues/500>`_)
* Contributors: Joshua Whitley, Sean Yen

1.15.0 (2020-05-18)
-------------------
* updated install locations for better portability. (`#500 <https://github.com/ros-perception/image_pipeline/issues/500>`_)
* Contributors: Sean Yen

1.14.0 (2020-01-12)
-------------------

Expand Down
10 changes: 2 additions & 8 deletions image_publisher/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,15 +1,9 @@
cmake_minimum_required(VERSION 3.0.2)
cmake_minimum_required(VERSION 2.8)
project(image_publisher)

find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure camera_info_manager sensor_msgs image_transport nodelet roscpp)

find_package(OpenCV REQUIRED COMPONENTS core)
message(STATUS "opencv version ${OpenCV_VERSION}")
if(OpenCV_VERSION VERSION_LESS "4.0.0")
find_package(OpenCV 3 REQUIRED COMPONENTS core imgcodecs videoio)
else()
find_package(OpenCV 4 REQUIRED COMPONENTS core imgcodecs videoio)
endif()
find_package(OpenCV REQUIRED COMPONENTS core imgcodecs videoio)

# generate the dynamic_reconfigure config file
generate_dynamic_reconfigure_options(cfg/ImagePublisher.cfg)
Expand Down
2 changes: 1 addition & 1 deletion image_publisher/package.xml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<package>
<name>image_publisher</name>
<version>1.15.2</version>
<version>1.15.0</version>
<description>
<p>
Contains a node publish an image stream from single image file
Expand Down
14 changes: 2 additions & 12 deletions image_rotate/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,17 +1,7 @@
1.15.2 (2020-05-19)
1.15.0 (2020-05-18)
-------------------

1.15.1 (2020-05-18)
-------------------

1.15.0 (2020-05-14)
-------------------
* Python 3 compatibility (`#530 <https://github.com/ros-perception/image_pipeline/issues/530>`_)
* cmake_minimum_required to 3.0.2
* Adapted to OpenCV4
* import setup from setuptools instead of distutils-core
* updated install locations for better portability. (`#500 <https://github.com/ros-perception/image_pipeline/issues/500>`_)
* Contributors: Joshua Whitley, Sean Yen
* Contributors: Sean Yen

1.14.0 (2020-01-12)
-------------------
Expand Down
Loading