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.

Sign up for GitHub

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

[Iron] Improve maintainability of calibration code #975

Closed
wants to merge 2 commits into from
Closed

[Iron] Improve maintainability of calibration code #975

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
983d6cb
fix dictionary names
MRo47 Apr 26, 2024
fd096ec
separated calibration types
MRo47 Apr 26, 2024
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 camera_calibration/doc/index.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@ need to extend or make a new calibration tool.
For details on the camera model and camera calibration process, see
http://docs.opencv.org/master/d9/d0c/group__calib3d.html

.. autoclass:: camera_calibration.calibrator.MonoCalibrator
.. autoclass:: camera_calibration.mono_calibrator.MonoCalibrator
:members:

.. autoclass:: camera_calibration.calibrator.StereoCalibrator
.. autoclass:: camera_calibration.stereo_calibrator.StereoCalibrator
:members:
110 changes: 63 additions & 47 deletions camera_calibration/scripts/tarfile_calibration.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,21 +39,24 @@
import cv_bridge
import tarfile

from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo
from camera_calibration.mono_calibrator import MonoCalibrator
from camera_calibration.stereo_calibrator import StereoCalibrator
from camera_calibration.calibrator import ChessboardInfo

import rclpy
import sensor_msgs.srv


def display(win_name, img):
cv2.namedWindow(win_name, cv2.WINDOW_NORMAL)
cv2.imshow( win_name, numpy.asarray( img[:,:] ))
cv2.imshow(win_name, numpy.asarray(img[:, :]))
k = cv2.waitKey(0)
cv2.destroyWindow(win_name)
if k in [27, ord('q')]:
rclpy.shutdown()


def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0):
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:
Expand All @@ -63,89 +66,100 @@ def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags

print(calibrator.ost())

if upload:
if upload:
info = calibrator.as_message()
if mono:
set_camera_info_service = rclpy.ServiceProxy("%s/set_camera_info" % "camera", sensor_msgs.srv.SetCameraInfo)
set_camera_info_service = rclpy.ServiceProxy(
"%s/set_camera_info" % "camera", sensor_msgs.srv.SetCameraInfo)

response = set_camera_info_service.call(info)
if not response.success:
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
raise RuntimeError(
"connected to set_camera_info service, but failed setting camera_info")
else:
set_left_camera_info_service = rclpy.ServiceProxy("%s/set_camera_info" % "left_camera", sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rclpy.ServiceProxy("%s/set_camera_info" % "right_camera", sensor_msgs.srv.SetCameraInfo)
set_left_camera_info_service = rclpy.ServiceProxy(
"%s/set_camera_info" % "left_camera", sensor_msgs.srv.SetCameraInfo)
set_right_camera_info_service = rclpy.ServiceProxy(
"%s/set_camera_info" % "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.result().success and response2.result().success):
raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
raise RuntimeError(
"connected to set_camera_info service, but failed setting camera_info")

if visualize:

#Show rectified images
# 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)
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")
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
# 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 = [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 = [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)))

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]
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)

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")
msg_left = bridge.cv2_to_imgmsg(im_left, "bgr8")
except cv_bridge.CvBridgeError as e:
print(e)

#RIGHT IMAGE
# 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)
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")
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] )
drawable = calibrator.handle_msg([msg_left, msg_right])

h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2]
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)
vis[:h, :w, :] = numpy.asarray(drawable.lscrib[:, :])
vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:, :])

display(l+" "+r, vis)


if __name__ == '__main__':
Expand All @@ -160,23 +174,23 @@ def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags
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")
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")
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")
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)")
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")
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)")
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")
Expand All @@ -197,7 +211,8 @@ def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags
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])
parser.error(
"Tarfile %s does not exist. Please select valid tarfile" % args[0])

tarname = args[0]

Expand Down Expand Up @@ -225,4 +240,5 @@ def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags
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)
cal_from_tarfile(boards, tarname, options.mono, options.upload,
calib_flags, options.visualize, options.alpha)
Loading