def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split("x")[0]),
int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
self.ts = TorsoState()
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split("x")[0]),
int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
topic_name = '/stereo/left/'
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber(topic_name + 'camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(topic_name + 'image_color', Image,
self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
self.frame = self.camera_info.header.frame_id
def test_detect_image_points_color_image(self):
# create Checkerboard instance and read image
c = Checkerboard((9, 6), 0.03)
image = cv2.imread(CHECKERBOARD_IMAGE, 0)
# create CheckerboardDetector instance and detect image points
cd = CheckerboardDetector(c)
points = cd.detect_image_points(image, True)
#dump = {'points': points.flatten().tolist()}
#yaml.dump(dump, open(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML, "w"))
# load expected points
points_expected = yaml.load(file(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML))['points']
self.assertTrue(np.allclose(points.flatten().tolist(), points_expected))
def __init__(self):
rospy.init_node(NODE)
pattern = rospy.get_param('~calibration_pattern')
self.cameras = rospy.get_param('~cameras')
pattern_string = pattern["pattern_size"].split("x")
pattern_size = (int(pattern_string[0]), int(pattern_string[1]))
cb = Checkerboard(pattern_size, pattern["square_size"])
self.detector = CheckerboardDetector(cb)
self.bridge = CvBridge()
self.pub = rospy.Publisher("cb_detections", MarkerArray)
self.topics = [self.cameras["reference"]["topic"]]
for camera in self.cameras["further"]:
self.topics.append(camera["topic"])
self.info_topics = [
t.replace("image_raw", "camera_info") for t in self.topics
]
self.info_topics = [
t.replace("image_color", "camera_info") for t in self.info_topics
]
print self.topics
print self.info_topics
self.images = dict.fromkeys(self.topics)
self.camera_infos = dict.fromkeys(self.topics)
self.colors = [ColorRGBA() for i in range(len(self.topics))]
hue_values = np.linspace(0, 1, len(self.topics) + 1)
for c, hue, topic in zip(self.colors, hue_values, self.topics):
c.a = 1
(c.r, c.g, c.b) = hsv_to_rgb(hue, 1, 1)
print "Listening to: %s - Color RGB: (%d, %d, %d)" % (
topic, c.r * 255, c.g * 255, c.b * 255)
self.points = []
for point in cb.get_pattern_points():
p = Point()
p.x = point[0]
p.y = point[1]
p.z = point[2]
self.points.append(p)
for topic, info_topic in zip(self.topics, self.info_topics):
rospy.Subscriber(topic, Image, self.callback_image, topic)
rospy.Subscriber(info_topic, CameraInfo, self.callback_info, topic)
rospy.sleep(3)
def run_mono_calibration(self):
'''
Runs the calibration
'''
print "==> starting monocular calibration"
# set up Checkerboard, CheckerboardDetector and MonoCalibrator
board = Checkerboard(self.pattern_size, self.square_size)
detector = CheckerboardDetector(board)
calibrator = MonoCalibrator(board, detector, self.folder,
self.image_prefix)
# run calibration
(rms, camera_matrix, projection_matrix, dist_coeffs, (h, w), _,
_) = calibrator.calibrate_monocular_camera(self.alpha)
print "==> successfully calibrated, reprojection RMS (in pixels):", rms
# create CalibrationData object with results
camera_info = CalibrationData(self.camera_name, self.frame_id, w, h)
camera_info.camera_matrix = camera_matrix
camera_info.distortion_coefficients = dist_coeffs
camera_info.projection_matrix = projection_matrix
camera_info.dist_coeffs = dist_coeffs
# save results
camera_info.save_camera_yaml_file(self.output_file)
print "==> saved results to:", self.output_file
# verbose mode
if self.verbose:
print "--> results:"
np.set_printoptions(suppress=1)
print "camera matrix:\n", camera_matrix
print "distortion coefficients:\n", dist_coeffs
print "projection matrix:\n", projection_matrix
def __init__(self):
'''
Initializes storage, gets parameters from parameter server and logs to rosinfo
'''
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard((9, 6), 0.03)
self.detector = CheckerboardDetector(self.board)
rospy.init_node(NODE)
self.counter = 0
# Get params from ros parameter server or use default
self.cams = rospy.get_param("~cameras")
self.camera = [self.cams["reference"]["topic"]]
for cam in self.cams["further"]:
self.camera.append(cam["topic"])
self.numCams = len(self.camera)
# Init images
self.image = []
self.image_received = [False] * self.numCams
for id in range(self.numCams):
self.image.append(Image())
# Subscribe to images
self.imageSub = []
for id in range(self.numCams):
self.imageSub.append(rospy.Subscriber(
self.camera[id], Image, self._imageCallback, id))
# Wait for image messages
for id in range(self.numCams):
rospy.wait_for_message(self.camera[id], Image, 5)
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split(
"x")[0]), int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
self.ts = TorsoState()
def get_corners(self):
# --------------------
# receive images
# -----------------
for v in self._images_received:
self._images_received[v] = False
start_time = rospy.Time.now()
while (not all(self._images_received.values())
or not all(self.transformations.values())):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still
# missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
for name, v in self._images_received.iteritems():
if not v:
print "--> still waiting for sample from %s" % name
for name, v in self._transformations_received.iteritems():
if not v:
print "--> still waiting for sample from %s" % name
start_time = rospy.Time.now()
# set up checkerboard and checkerboard detector
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb
for camera, image in self._images.iteritems():
image = image["image"]
cvImage = self.bridge.imgmsg_to_cv2(image, "mono8")
#imagecv = cv2util.cvmat2np(cvImage)
self._images[camera]['corners'] = None
try:
corners = checkerboard_detector.detect_image_points(
cvImage, is_grayscale=True)
except:
rospy.logwarn("No calibration pattern found for: '%s'" %
camera)
else:
print "cb found: %s" % camera
#img_points = []
#for (x, y) in corners.reshape(-1, 2):
# img_points.append(ImagePoint(x, y))
self._images[camera]['corners'] = corners
return self._images
def test_detect_image_points_color_image(self):
# create Checkerboard instance and read image
c = Checkerboard((9, 6), 0.03)
image = cv2.imread(CHECKERBOARD_IMAGE, 0)
# create CheckerboardDetector instance and detect image points
cd = CheckerboardDetector(c)
points = cd.detect_image_points(image, True)
#dump = {'points': points.flatten().tolist()}
#yaml.dump(dump, open(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML, "w"))
# load expected points
points_expected = yaml.load(
file(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML))['points']
self.assertTrue(np.allclose(points.flatten().tolist(),
points_expected))
def __init__(self):
'''
Initializes storage, gets parameters from parameter server and logs to rosinfo
'''
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(
(9, 6), 0.03) ### Take the parameters from the parameter server
self.detector = CheckerboardDetector(self.board)
rospy.init_node(NODE)
self.counter = 0
# Get params from ros parameter server or use default
self.cams = rospy.get_param("~cameras")
self.camera = [self.cams["reference"]["topic"]]
if self.cams.has_key("further"):
rospy.loginfo("FURTHER cameras specified")
for cam in self.cams["further"]:
self.camera.append(cam["topic"])
else:
rospy.logwarn("No FURTHER cameras specified")
self.numCams = len(self.camera)
# Init images
self.image = []
self.image_received = [False] * self.numCams
for id in range(self.numCams):
self.image.append(
Image()
) # wait for a message from each camera this ensures that all cameras are working
# Subscribe to images
self.imageSub = []
for id in range(self.numCams):
self.imageSub.append(
rospy.Subscriber(self.camera[id], Image, self._imageCallback,
id))
# Wait for image messages
for id in range(self.numCams):
print "now checking for incoming frames on the %s topic" % self.camera[
id]
# pdb.set_trace()
rospy.wait_for_message(self.camera[id], Image, 5)
def test_calculate_object_pose(self):
# create Checkerboard instance and read image
c = Checkerboard((9, 6), 0.03)
image = cv2.imread(CHECKERBOARD_IMAGE, 0)
# create CheckerboardDetector instance and detect object pose
# (only dummy matrices, does not correspond to real camera used)
cd = CheckerboardDetector(c)
camera_matrix = np.matrix([1200, 0, 600, 0, 1200, 600, 0, 0, 1], dtype=np.float32).reshape((3,3))
dist_coeffs = np.matrix([0, 0, 0, 0, 0], dtype=np.float32).reshape((1,5))
(rvec, tvec) = cd.calculate_object_pose(image, camera_matrix, dist_coeffs, True)
rvec_expected = np.matrix([[0.99905897, 0.035207, -0.02533079],
[-0.0208742, 0.90224111, 0.43072642],
[ 0.03801906, -0.42979234, 0.90212699]])
tvec_expected = np.matrix([[-0.03181351], [-0.13593217], [ 0.7021291]])
self.assertTrue(np.allclose(rvec_expected, rvec))
self.assertTrue(np.allclose(tvec_expected, tvec))
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split(
"x")[0]), int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
topic_name = '/stereo/left/'
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber(topic_name + 'camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(
topic_name + 'image_color', Image, self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
self.frame = self.camera_info.header.frame_id
def test_calculate_object_pose(self):
# create Checkerboard instance and read image
c = Checkerboard((9, 6), 0.03)
image = cv2.imread(CHECKERBOARD_IMAGE, 0)
# create CheckerboardDetector instance and detect object pose
# (only dummy matrices, does not correspond to real camera used)
cd = CheckerboardDetector(c)
camera_matrix = np.matrix([1200, 0, 600, 0, 1200, 600, 0, 0, 1],
dtype=np.float32).reshape((3, 3))
dist_coeffs = np.matrix([0, 0, 0, 0, 0], dtype=np.float32).reshape(
(1, 5))
(rvec, tvec) = cd.calculate_object_pose(image, camera_matrix,
dist_coeffs, True)
rvec_expected = np.matrix([[0.99905897, 0.035207, -0.02533079],
[-0.0208742, 0.90224111, 0.43072642],
[0.03801906, -0.42979234, 0.90212699]])
tvec_expected = np.matrix([[-0.03181351], [-0.13593217], [0.7021291]])
self.assertTrue(np.allclose(rvec_expected, rvec))
self.assertTrue(np.allclose(tvec_expected, tvec))
def run_stereo_calibration(self):
'''
Runs the calibration
'''
# set up Checkerboard, CheckerboardDetector and MonoCalibrator
board = Checkerboard(self.pattern_size, self.square_size)
detector = CheckerboardDetector(board)
calibrator = StereoCalibrator(board, detector, self.folder,
self.image_prefix_l, self.image_prefix_r)
# run calibration
((rms_l, rms_r, rms_stereo), camera_matrix_l, dist_coeffs_l,
rectification_matrix_l, projection_matrix_l, camera_matrix_r,
dist_coeffs_r, rectification_matrix_r, projection_matrix_r, (h, w), R,
T) = calibrator.calibrate_stereo_camera(self.alpha)
print "==> successfully calibrated, stereo reprojection RMS (in pixels):", rms_stereo
# create CalibrationData object with results
camera_info_l = CalibrationData(self.camera_name_l, self.frame_id_l, w,
h)
camera_info_l.camera_matrix = camera_matrix_l
camera_info_l.rectification_matrix = rectification_matrix_l
camera_info_l.projection_matrix = projection_matrix_l
camera_info_l.distortion_coefficients = dist_coeffs_l
camera_info_r = CalibrationData(self.camera_name_r, self.frame_id_r, w,
h)
camera_info_r.camera_matrix = camera_matrix_r
camera_info_r.rectification_matrix = rectification_matrix_r
camera_info_r.projection_matrix = projection_matrix_r
camera_info_r.distortion_coefficients = dist_coeffs_r
# save results
camera_info_l.save_camera_yaml_file(self.output_file_l)
print "==> saved left results to:", self.output_file_l
camera_info_r.save_camera_yaml_file(self.output_file_r)
print "==> saved right results to:", self.output_file_r
# convert baseline (invert transfrom as T and R bring right frame into left
# and we need transform from left to right for urdf!)
M = np.matrix(np.vstack((np.hstack(
(R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix
M_inv = M.I
T_inv = np.array(M_inv[:3,
3]).flatten().tolist() # T as list (x, y, z)
R_inv = list(tf.transformations.euler_from_matrix(
M_inv[:3, :3])) # convert R to (roll, pitch, yaw)
# save baseline
if (self.calibration_urdf_in != ""
and self.calibration_urdf_out != ""):
attributes2update = {
self.baseline_prop_prefix + 'x': T_inv[0],
self.baseline_prop_prefix + 'y': T_inv[1],
self.baseline_prop_prefix + 'z': T_inv[2],
self.baseline_prop_prefix + 'roll': R_inv[0],
self.baseline_prop_prefix + 'pitch': R_inv[1],
self.baseline_prop_prefix + 'yaw': R_inv[2]
}
urdf_updater = CalibrationUrdfUpdater(self.calibration_urdf_in,
self.calibration_urdf_out,
self.verbose)
urdf_updater.update(attributes2update)
print "==> updated baseline in:", self.calibration_urdf_out
else:
print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..."
# verbose mode
if self.verbose:
print "--> results:"
np.set_printoptions(suppress=1)
print "left\n----"
print "rms left monocular calibration:", rms_l
print "camera matrix:\n", camera_matrix_l
print "distortion coefficients:\n", dist_coeffs_l
print "rectification matrix:\n", rectification_matrix_l
print "projection matrix:\n", projection_matrix_l
print
print "right\n-----"
print "rms right monocular calibration:", rms_r
print "camera matrix:\n", camera_matrix_r
print "distortion coefficients:\n", dist_coeffs_r
print "rectification matrix:\n", rectification_matrix_r
print "projection matrix:\n", projection_matrix_r
print
print "baseline (transform from left to right camera)\n--------"
print "R (in rpy):\n", R_inv
print "T (in xyz):\n", T_inv
class Cb_marker_publish():
def __init__(self):
rospy.init_node(NODE)
pattern = rospy.get_param('~calibration_pattern')
self.cameras = rospy.get_param('~cameras')
pattern_string = pattern["pattern_size"].split("x")
pattern_size = (int(pattern_string[0]), int(pattern_string[1]))
cb = Checkerboard(pattern_size, pattern["square_size"])
self.detector = CheckerboardDetector(cb)
self.bridge = CvBridge()
self.pub = rospy.Publisher("cb_detections", MarkerArray)
self.topics = [self.cameras["reference"]["topic"]]
for camera in self.cameras["further"]:
self.topics.append(camera["topic"])
self.info_topics = [
t.replace("image_raw", "camera_info") for t in self.topics
]
self.info_topics = [
t.replace("image_color", "camera_info") for t in self.info_topics
]
print self.topics
print self.info_topics
self.images = dict.fromkeys(self.topics)
self.camera_infos = dict.fromkeys(self.topics)
self.colors = [ColorRGBA() for i in range(len(self.topics))]
hue_values = np.linspace(0, 1, len(self.topics) + 1)
for c, hue, topic in zip(self.colors, hue_values, self.topics):
c.a = 1
(c.r, c.g, c.b) = hsv_to_rgb(hue, 1, 1)
print "Listening to: %s - Color RGB: (%d, %d, %d)" % (
topic, c.r * 255, c.g * 255, c.b * 255)
self.points = []
for point in cb.get_pattern_points():
p = Point()
p.x = point[0]
p.y = point[1]
p.z = point[2]
self.points.append(p)
for topic, info_topic in zip(self.topics, self.info_topics):
rospy.Subscriber(topic, Image, self.callback_image, topic)
rospy.Subscriber(info_topic, CameraInfo, self.callback_info, topic)
rospy.sleep(3)
def callback_image(self, data, topic):
self.images[topic] = data
def callback_info(self, data, topic):
self.camera_infos[topic] = data
def get_pose(self, topic):
image = self.images[topic]
cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8')
img_raw = cv2util.cvmat2np(cvImage)
camera_matrix = np.matrix(
np.reshape(self.camera_infos[topic].K, (3, 3)))
dist_coeffs = np.matrix(self.camera_infos[topic].D)
return self.detector.calculate_object_pose(img_raw, camera_matrix,
dist_coeffs, False)
def run(self):
rate = rospy.Rate(10)
br = tf.TransformBroadcaster()
print "Everything up and running"
while not rospy.is_shutdown():
# put code here
msgs = MarkerArray()
for topic in self.topics:
# pick color as defined before
c = self.colors[self.topics.index(topic)]
if self.images[topic] is None:
continue
try:
# compute pose of detected pattern
p = self.get_pose(topic)
except CheckerboardDetector.NoPatternFoundException:
rospy.logwarn("No pattern found on topic: '%s'" % topic)
continue
else:
rmat = np.array(p[0])
rmat = np.append(rmat, [[0, 0, 0]], 0)
rmat = np.append(rmat, [[0], [0], [0], [1]], 1)
q = tf.transformations.quaternion_from_matrix(rmat)
msg = Marker()
msg.type = Marker.SPHERE_LIST
msg.header.frame_id = self.camera_infos[
topic].header.frame_id
msg.header.stamp = rospy.Time()
msg.id = self.topics.index(topic)
msg.pose.position.x = p[1][0]
msg.pose.position.y = p[1][1]
msg.pose.position.z = p[1][2]
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
msg.scale.x = 0.01
msg.scale.y = 0.01
msg.scale.z = 0.01
msg.color = c
msg.points = self.points
msgs.markers.append(msg)
br.sendTransform(p[1], q, rospy.Time.now(), "cb",
msg.header.frame_id)
self.pub.publish(msgs)
rate.sleep()
class VisibilityCheckerNode():
'''
@summary: Captures Images from one or more cameras (Image message topics) to files.
Number of cameras, output folder and file names are configurable via ROS parameters.
After startup call "~capture_images" ROS service to save images of all
cameras to output folder.
'''
def __init__(self):
'''
Initializes storage, gets parameters from parameter server and logs to rosinfo
'''
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard((9, 6), 0.03)
self.detector = CheckerboardDetector(self.board)
rospy.init_node(NODE)
self.counter = 0
# Get params from ros parameter server or use default
self.cams = rospy.get_param("~cameras")
self.camera = [self.cams["reference"]["topic"]]
for cam in self.cams["further"]:
self.camera.append(cam["topic"])
self.numCams = len(self.camera)
# Init images
self.image = []
self.image_received = [False] * self.numCams
for id in range(self.numCams):
self.image.append(Image())
# Subscribe to images
self.imageSub = []
for id in range(self.numCams):
self.imageSub.append(rospy.Subscriber(
self.camera[id], Image, self._imageCallback, id))
# Wait for image messages
for id in range(self.numCams):
rospy.wait_for_message(self.camera[id], Image, 5)
def _imageCallback(self, data, id):
'''
Copy image message to local storage
@param data: Currently received image message
@type data: ROS Image() message
@param id: Id of camera from which the image was received
@type id: integer
'''
#print "cb executed"
self.image[id] = data
self.image_received[id] = True
def _checkHandle(self, req):
'''
Service handle for "Capture" Service
Grabs images, converts them and saves them to files
@param req: service request
@type req: CaptureRequest() message
@return: CaptureResponse() message
'''
self.image_received = [False] * self.numCams
visible = []
while not all(self.image_received):
print "waiting for images"
rospy.sleep(1)
if all(self.image_received):
print "=== ALL IMAGES RECEIVED ==="
self.images_received = [False] * self.numCams
for id in range(self.numCams):
image = self.image[id]
cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8')
img_raw = cv2util.cvmat2np(cvImage)
visible.append(self.detector.detect_image_points(
img_raw, False, True) is not None)
# grab image messages
#print '%s checkerboards found --> return %s'%(sum(visible),all(visible))
response = VisibleResponse()
response.every = False
response.master = False
if all(visible):
response.every = True
response.master = True
elif visible[0]:
response.master = True
return response
def run(self):
# Start service
srv = rospy.Service(
'/image_capture/visibility_check', Visible, self._checkHandle)
rospy.loginfo("service '/image_capture/visibility_check' started, waiting for requests...")
rospy.spin()
class VisibilityCheckerNode():
'''
@summary: Captures Images from one or more cameras (Image message topics) to files.
Number of cameras, output folder and file names are configurable via ROS parameters.
After startup call "~capture_images" ROS service to save images of all
cameras to output folder.
'''
def __init__(self):
'''
Initializes storage, gets parameters from parameter server and logs to rosinfo
'''
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard((9, 6), 0.03)
self.detector = CheckerboardDetector(self.board)
rospy.init_node(NODE)
self.counter = 0
# Get params from ros parameter server or use default
self.cams = rospy.get_param("~cameras")
self.camera = [self.cams["reference"]["topic"]]
if self.cams.has_key("further"):
rospy.loginfo("FURTHER cameras specified")
for cam in self.cams["further"]:
self.camera.append(cam["topic"])
else:
rospy.logwarn("No FURTHER cameras specified")
self.numCams = len(self.camera)
# Init images
self.image = []
self.image_received = [False] * self.numCams
for id in range(self.numCams):
self.image.append(Image())
# Subscribe to images
self.imageSub = []
for id in range(self.numCams):
self.imageSub.append(rospy.Subscriber(
self.camera[id], Image, self._imageCallback, id))
# Wait for image messages
for id in range(self.numCams):
rospy.wait_for_message(self.camera[id], Image, 5)
def _imageCallback(self, data, id):
'''
Copy image message to local storage
@param data: Currently received image message
@type data: ROS Image() message
@param id: Id of camera from which the image was received
@type id: integer
'''
#print "cb executed"
self.image[id] = data
self.image_received[id] = True
def _checkHandle(self, req):
'''
Service handle for "Capture" Service
Grabs images, converts them and saves them to files
@param req: service request
@type req: CaptureRequest() message
@return: CaptureResponse() message
'''
self.image_received = [False] * self.numCams
visible = []
while not all(self.image_received):
print "waiting for images"
rospy.sleep(1)
if all(self.image_received):
print "=== ALL IMAGES RECEIVED ==="
self.images_received = [False] * self.numCams
for id in range(self.numCams):
image = self.image[id]
cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8')
img_raw = cv2util.cvmat2np(cvImage)
try:
self.detector.detect_image_points( img_raw, False, True)
visible.append(True)
except self.detector.NoPatternFoundException:
rospy.logwarn("No cb found")
visible.append(False)
# grab image messages
#print '%s checkerboards found --> return %s'%(sum(visible),all(visible))
response = VisibleResponse()
response.every = False
response.master = False
if all(visible):
response.every = True
response.master = True
#elif visible[0]:
# response.master = True
return response
def run(self):
# Start service
srv = rospy.Service(
'/image_capture/visibility_check', Visible, self._checkHandle)
rospy.loginfo("service '/image_capture/visibility_check' started, waiting for requests...")
rospy.spin()
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size,
square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
self._left_received = False
self._right_received = False
self._kinect_rgb_received = False
start_time = rospy.Time.now()
while (not self._left_received or not self._right_received
or not self._kinect_rgb_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._left_received:
print "--> still waiting for sample from left"
if not self._right_received:
print "--> still waiting for sample from right"
if not self._kinect_rgb_received:
print "--> still waiting for sample from kinect"
start_time = rospy.Time.now()
latest_left = self._left
latest_right = self._right
latest_kinect_rgb = self._kinect_rgb
self._torso_joint_msg_received = False
self._arm_joint_msg_received = False
start_time = rospy.Time.now()
while (not self._torso_joint_msg_received
or not self._arm_joint_msg_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._torso_joint_msg_received:
print "--> still waiting for torso joint states"
if not self._arm_joint_msg_received:
print "--> still waiting for srm joint states"
start_time = rospy.Time.now()
latest_torso = self._torso_joint_msg
latest_arm = self._arm_joint_msg
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb left
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image,
is_grayscale=True)
if corners != None:
print "cb found: left"
img_points_left = []
for (x, y) in corners.reshape(-1, 2):
img_points_left.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg left
# ----------------------
cam_msg_left = CameraMeasurement()
cam_msg_left.camera_id = "left"
cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp
cam_msg_left.cam_info = latest_left["camera_info"]
cam_msg_left.image_points = img_points_left
cam_msg_left.verbose = False
#cam_ms_leftg.image = latest_left["image_color"]
#cam_msg_left.image_rect = latest_left["image_rect"]
#cam_msg_left.features = # Not implemented here
# detect cb right
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image,
is_grayscale=True)
if corners != None:
print "cb found: right"
img_points_right = []
for (x, y) in corners.reshape(-1, 2):
img_points_right.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg right
# -----------------------
cam_msg_right = CameraMeasurement()
cam_msg_right.camera_id = "right"
cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp
cam_msg_right.cam_info = latest_right["camera_info"]
cam_msg_right.image_points = img_points_right
cam_msg_right.verbose = False
#cam_msg_right.image = latest_right["image_color"]
#cam_msg_right.image_rect = latest_right["image_rect"]
#cam_msg_right.features = # Not implemented here
# detect cb kinect_rgb
# --------------------
cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"],
"mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image,
is_grayscale=True)
if corners != None:
print "cb found: kinect_rgb"
img_points_kinect_rgb = []
for (x, y) in corners.reshape(-1, 2):
img_points_kinect_rgb.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg kinect_rgb
# ----------------------------
cam_msg_kinect_rgb = CameraMeasurement()
cam_msg_kinect_rgb.camera_id = "kinect_rgb"
cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb[
"camera_info"].header.stamp
cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"]
cam_msg_kinect_rgb.image_points = img_points_kinect_rgb
cam_msg_kinect_rgb.verbose = False
#cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"]
#cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"]
#cam_msg_kinect_rgb.features = # Not implemented here
# create torso_chain msg
# ----------------------
torso_chain_msg = ChainMeasurement()
torso_chain_msg.header = latest_torso.header
torso_chain_msg.chain_id = "torso_chain"
torso_chain_msg.chain_state = latest_torso
# create arm_chain msg
# --------------------
arm_chain_msg = ChainMeasurement()
arm_chain_msg.header = latest_arm.header
arm_chain_msg.chain_id = "arm_chain"
arm_chain_msg.chain_state = latest_arm
# DEBUG publish pic
# -----------------
self._image_pub_left.publish(latest_left["image_color"])
self._image_pub_right.publish(latest_right["image_color"])
self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"])
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb]
robot_msg.M_chain = [torso_chain_msg, arm_chain_msg]
self._robot_measurement_pub.publish(robot_msg)
return True
class Detection():
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split("x")[0]),
int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
topic_name = '/stereo/left/'
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber(topic_name + 'camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(topic_name + 'image_color', Image,
self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
self.frame = self.camera_info.header.frame_id
# initialize torso for movement
def __camera_info_callback__(self, data):
'''
executed on new message in /cam3d/rgb/camera_info
stores received data
'''
self.camera_info = data
self.camera_info_received = True
def __image_raw_callback__(self, data):
'''
executed on new message in /cam3d/rgb/image_raw
stores newest image
'''
self.latest_image = data
def get_position(self):
'''
returns the Y value in subpixel accuracy for the center of the recognized checkerboard
'''
r = rospy.Rate(10) # Hz
while rospy.Time.now(
) - self.latest_image.header.stamp > rospy.Duration(
1) and not rospy.is_shutdown():
print 'no image received'
print rospy.Time.now(), self.latest_image.header.stamp
r.sleep()
cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8")
image_raw = cv2util.cvmat2np(cvImage)
#image_processed = cv2.undistort(image_raw, self.cm, self.dc)
#points=self.detector.detect_image_points(image_processed,True)
(rmat,
tvec) = self.detector.calculate_object_pose(image_raw, self.cm,
self.dc, True)
#print tvec[1]
return (rmat, tvec), self.latest_image.header.frame_id
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
self._left_received = False
self._right_received = False
self._kinect_rgb_received = False
start_time = rospy.Time.now()
while (not self._left_received or not self._right_received or not self._kinect_rgb_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._left_received: print "--> still waiting for sample from left"
if not self._right_received: print "--> still waiting for sample from right"
if not self._kinect_rgb_received: print "--> still waiting for sample from kinect"
start_time = rospy.Time.now()
latest_left = self._left
latest_right = self._right
latest_kinect_rgb = self._kinect_rgb
self._torso_joint_msg_received = False
self._arm_joint_msg_received = False
start_time = rospy.Time.now()
while (not self._torso_joint_msg_received or not self._arm_joint_msg_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._torso_joint_msg_received: print "--> still waiting for torso joint states"
if not self._arm_joint_msg_received: print "--> still waiting for srm joint states"
start_time = rospy.Time.now()
latest_torso = self._torso_joint_msg
latest_arm = self._arm_joint_msg
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb left
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image, is_grayscale=True)
if corners != None:
print "cb found: left"
img_points_left = []
for (x, y) in corners.reshape(-1, 2):
img_points_left.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg left
# ----------------------
cam_msg_left = CameraMeasurement()
cam_msg_left.camera_id = "left"
cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp
cam_msg_left.cam_info = latest_left["camera_info"]
cam_msg_left.image_points = img_points_left
cam_msg_left.verbose = False
#cam_ms_leftg.image = latest_left["image_color"]
#cam_msg_left.image_rect = latest_left["image_rect"]
#cam_msg_left.features = # Not implemented here
# detect cb right
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image, is_grayscale=True)
if corners != None:
print "cb found: right"
img_points_right = []
for (x, y) in corners.reshape(-1, 2):
img_points_right.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg right
# -----------------------
cam_msg_right = CameraMeasurement()
cam_msg_right.camera_id = "right"
cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp
cam_msg_right.cam_info = latest_right["camera_info"]
cam_msg_right.image_points = img_points_right
cam_msg_right.verbose = False
#cam_msg_right.image = latest_right["image_color"]
#cam_msg_right.image_rect = latest_right["image_rect"]
#cam_msg_right.features = # Not implemented here
# detect cb kinect_rgb
# --------------------
cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image, is_grayscale=True)
if corners != None:
print "cb found: kinect_rgb"
img_points_kinect_rgb = []
for (x, y) in corners.reshape(-1, 2):
img_points_kinect_rgb.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg kinect_rgb
# ----------------------------
cam_msg_kinect_rgb = CameraMeasurement()
cam_msg_kinect_rgb.camera_id = "kinect_rgb"
cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb["camera_info"].header.stamp
cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"]
cam_msg_kinect_rgb.image_points = img_points_kinect_rgb
cam_msg_kinect_rgb.verbose = False
#cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"]
#cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"]
#cam_msg_kinect_rgb.features = # Not implemented here
# create torso_chain msg
# ----------------------
torso_chain_msg = ChainMeasurement()
torso_chain_msg.header = latest_torso.header
torso_chain_msg.chain_id = "torso_chain"
torso_chain_msg.chain_state = latest_torso
# create arm_chain msg
# --------------------
arm_chain_msg = ChainMeasurement()
arm_chain_msg.header = latest_arm.header
arm_chain_msg.chain_id = "arm_chain"
arm_chain_msg.chain_state = latest_arm
# DEBUG publish pic
# -----------------
self._image_pub_left.publish(latest_left["image_color"])
self._image_pub_right.publish(latest_right["image_color"])
self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"])
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb]
robot_msg.M_chain = [torso_chain_msg, arm_chain_msg]
self._robot_measurement_pub.publish(robot_msg)
return True
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
self._left_received = False
self._right_received = False
self._kinect_rgb_received = False
start_time = rospy.Time.now()
while (not self._left_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._left_received:
print "--> still waiting for sample from left"
start_time = rospy.Time.now()
print "got sample"
latest_left = self._left
self._torso_joint_msg_received = False
self._arm_joint_msg_received = False
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb left
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_left["image"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(
image, is_grayscale=True)
if corners is not None:
print "cb found: left"
img_points_left = []
for (x, y) in corners.reshape(-1, 2):
img_points_left.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg left
# ----------------------
cam_msg_left = CameraMeasurement()
cam_msg_left.camera_id = "left"
cam_msg_left.header.stamp = latest_left["image"].header.stamp
#cam_msg_left.cam_info = latest_left["camera_info"]
cam_msg_left.image_points = img_points_left
cam_msg_left.verbose = False
#cam_ms_leftg.image = latest_left["image_color"]
#cam_msg_left.image_rect = latest_left["image_rect"]
#cam_msg_left.features = # Not implemented here
# create chain msgs
# ----------------------
transformations = []
print self.transformations
for (key, links) in self.transformations.iteritems():
chain_msg = ChainMeasurement()
chain_msg.chain_id = key
while not rospy.is_shutdown():
try:
(
trans, rot) = self.listener.lookupTransform(links[0], links[1],
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "error looking up transformation from ", links[0], " to ", links[1]
rospy.sleep(0.5)
chain_msg.translation = trans
chain_msg.rotation = rot
transformations.append(chain_msg)
# DEBUG publish pic
# -----------------
self._image_pub_left.publish(latest_left["image"])
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
robot_msg.M_cam = [cam_msg_left]
robot_msg.M_chain = transformations
self._robot_measurement_pub.publish(robot_msg)
return True
class Detection():
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split(
"x")[0]), int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
topic_name = '/stereo/left/'
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber(topic_name + 'camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(
topic_name + 'image_color', Image, self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
self.frame = self.camera_info.header.frame_id
# initialize torso for movement
def __camera_info_callback__(self, data):
'''
executed on new message in /cam3d/rgb/camera_info
stores received data
'''
self.camera_info = data
self.camera_info_received = True
def __image_raw_callback__(self, data):
'''
executed on new message in /cam3d/rgb/image_raw
stores newest image
'''
self.latest_image = data
def get_position(self):
'''
returns the Y value in subpixel accuracy for the center of the recognized checkerboard
'''
r = rospy.Rate(10) # Hz
while rospy.Time.now() - self.latest_image.header.stamp > rospy.Duration(1) and not rospy.is_shutdown():
print 'no image received'
print rospy.Time.now(), self.latest_image.header.stamp
r.sleep()
cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8")
image_raw = cv2util.cvmat2np(cvImage)
#image_processed = cv2.undistort(image_raw, self.cm, self.dc)
#points=self.detector.detect_image_points(image_processed,True)
(rmat, tvec) = self.detector.calculate_object_pose(image_raw, self.cm, self.dc, True)
#print tvec[1]
return (rmat, tvec), self.latest_image.header.frame_id
def run_stereo_calibration(self):
'''
Runs the calibration
'''
# set up Checkerboard, CheckerboardDetector and MonoCalibrator
board = Checkerboard(self.pattern_size, self.pattern["square_size"])
detector = CheckerboardDetector(board)
attributes2update = {}
output = ""
camera_ref_saved = False;
for image_prefix_dep,\
camera_name_dep,\
frame_id_dep,\
output_file_dep,\
baseline_prop_prefix,\
is_part_of_stereo_system in zip(self.image_prefixes_dep,
self.camera_names_dep,
self.frame_ids_dep,
self.output_files_dep,
self.baseline_prop_prefixes_dep,
self.is_part_of_stereo_system):
if image_prefix_dep is not None:
print "Calibrating %s: \n\t Frame: %s \n\t Output File: %s \n\t Baseline Prefix: %s"%(camera_name_dep,frame_id_dep,output_file_dep, baseline_prop_prefix)
output += self.camera_name_ref +" -> " + camera_name_dep + ": \n"
calibrator = StereoCalibrator(board, detector, self.folder,
self.image_prefix_ref, image_prefix_dep)
# run calibration for stereo pair
(
(rms_ref, rms_dep, rms_stereo),
camera_matrix_ref, dist_coeffs_ref, rectification_matrix_ref, projection_matrix_ref,
camera_matrix_dep, dist_coeffs_dep, rectification_matrix_dep, projection_matrix_dep,
((h_ref, w_ref), (h_dep, w_dep)), R, T) = calibrator.calibrate_stereo_camera(self.alpha)
output += "==> successfully calibrated, stereo reprojection RMS (in pixels): "
output += str(rms_stereo)
output += "\n\n"
# create CalibrationData object with results
camera_info_ref = CalibrationData(
self.camera_name_ref, self.frame_id_ref, w_ref, h_ref)
camera_info_ref.camera_matrix = camera_matrix_ref
camera_info_ref.rectification_matrix = rectification_matrix_ref
camera_info_ref.projection_matrix = projection_matrix_ref
camera_info_ref.distortion_coefficients = dist_coeffs_ref
camera_info_dep = CalibrationData(
camera_name_dep, frame_id_dep, w_dep, h_dep)
camera_info_dep.camera_matrix = camera_matrix_dep
camera_info_dep.rectification_matrix = rectification_matrix_dep
camera_info_dep.projection_matrix = projection_matrix_dep
camera_info_dep.distortion_coefficients = dist_coeffs_dep
if output_file_dep is not None:
if not is_part_of_stereo_system:
# TODO: compute new projectionmatrix
(projection_matrix, _) = cv2.getOptimalNewCameraMatrix(
camera_matrix_dep, dist_coeffs_dep, (w_dep, h_dep),0 )
camera_info_dep.projection_matrix = np.array(np.hstack((
projection_matrix, np.matrix([0, 0, 0]).reshape(3, 1))))
camera_info_dep.rectification_matrix=[1,0,0, 0,1,0, 0,0,1]
# save results
if not camera_ref_saved:
camera_info_ref.save_camera_yaml_file(self.output_file_ref)
print "==> saved left results to:", self.output_file_ref
camera_ref_saved = True
#output += "==> saved left results to:", self.output_file_ref
camera_info_dep.save_camera_yaml_file(output_file_dep)
print "==> saved " + camera_name_dep + " results to:", output_file_dep
#output += "==> saved " + camera_name_dep + " results to:", output_file_dep
# convert baseline (invert transfrom as T and R bring right frame into left
# and we need transform from left to right for urdf!)
M = np.matrix(np.vstack((np.hstack(
(R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix
# TODO: tests with real hardware samples
# compute transformation from reference to parent frame of camera
# k_u = kinematic_utils()
# k_u.get_parent(frame_id_dep)
# M_parent = np.matrix(k_u.get_tf_to_parent(self.frame_id_ref))
# # resulting transformation
M_inv = M.I
# M_result = M_inv * M_parent
# # urdf specifies origin -> inverse
# M_inv = M_result.I
T_inv = np.array(
M_inv[:3, 3]).flatten().tolist() # T as list (x, y, z)
R_inv = list(tf.transformations.euler_from_matrix(
M_inv[:3, :3])) # convert R to (roll, pitch, yaw)
baseline_prop_prefix = 'offset_' + baseline_prop_prefix
attributes2update[baseline_prop_prefix + 'x'] = T_inv[0]
attributes2update[baseline_prop_prefix + 'y'] = T_inv[1]
attributes2update[baseline_prop_prefix + 'z'] = T_inv[2]
attributes2update[baseline_prop_prefix + 'roll'] = R_inv[0]
attributes2update[baseline_prop_prefix + 'pitch'] = R_inv[1]
attributes2update[baseline_prop_prefix + 'yaw'] = R_inv[2]
#verbose mode
if self.verbose:
print "--> results:"
np.set_printoptions(suppress=1)
print "left\n----"
print "rms left monocular calibration:", rms_ref
print "camera matrix:\n", camera_matrix_ref
print "distortion coefficients:\n", dist_coeffs_ref
print "rectification matrix:\n", rectification_matrix_ref
print "projection matrix:\n", projection_matrix_ref
print
print "right\n-----"
print "rms right monocular calibration:", rms_dep
print "camera matrix:\n", camera_matrix_dep
print "distortion coefficients:\n", dist_coeffs_dep
print "rectification matrix:\n", rectification_matrix_dep
print "projection matrix:\n", projection_matrix_dep
print
print "baseline (transform from left to right camera)\n--------"
print "R (in rpy):\n", R_inv
print "T (in xyz):\n", T_inv
# save baseline
if (self.calibration_offset_urdf != "" and self.calibration_default_urdf != ""):
for p in attributes2update.iteritems():
print "%s: %s"%p
urdf_updater = CalibrationUrdfUpdater(self.calibration_offset_urdf,
self.calibration_offset_urdf,
debug = self.verbose,
urdf_default=self.calibration_default_urdf)
urdf_updater.update(attributes2update)
print "==> updated baseline in:", self.calibration_offset_urdf
else:
print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..."
print output
class TorsoCalibration():
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split("x")[0]),
int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
self.ts = TorsoState()
def __camera_info_callback__(self, data):
'''
executed on new message in /cam3d/rgb/camera_info
stores received data
'''
self.camera_info = data
self.camera_info_received = True
def __image_raw_callback__(self, data):
'''
executed on new message in /cam3d/rgb/image_raw
stores newest image
'''
self.latest_image = data
def run(self):
'''
Runs the calibration
'''
print "==> starting torso pitch/height calibration"
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber('/cam3d/rgb/camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber('/cam3d/rgb/image_color', Image,
self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
# initialize torso for movement
self.sss.init("torso")
# get initial state of Torso (Upper Tilt Joint and Head Axis Joint horizontal)
state = list(self.ts.calc_references())
print state
# start minimization for getting the upright position of the torso
start_time = rospy.Time.now()
solution = self.minimize_yvalue(state)
if self.verbose:
print 'Elapsed time: ', (rospy.Time.now() - start_time).to_sec()
print 'Upright forward Position found at %s' % solution
print 'Seed was ', state
if self.save_result:
system(
'roslaunch cob_torso_calibration update_torso_calibration_urdf.launch'
)
#
def get_position(self):
'''
returns the Y value in subpixel accuracy for the center of the recognized checkerboard
'''
r = rospy.Rate(10) # Hz
while rospy.Time.now(
) - self.latest_image.header.stamp > rospy.Duration(
1) and not rospy.is_shutdown():
print 'no image received'
print rospy.Time.now(), self.latest_image.header.stamp
r.sleep()
cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8")
image_raw = cv2util.cvmat2np(cvImage)
#image_processed = cv2.undistort(image_raw, self.cm, self.dc)
#points=self.detector.detect_image_points(image_processed,True)
(rmat, tvec,
image_points) = self.detector.calculate_object_pose_ransac(
image_raw, self.cm, self.dc, True)
#print tvec[1]
return tvec[0][0], tvec[1][0], tvec[2][0]
def get_average_position(self, nsec, frequency=15):
'''
returns the average Y value for the last nsec seconds
@param nsec: time for samples to be taken
@type nsec: integer
@param frequency: update frequency for image topic
@type frequency: integer
'''
R = rospy.Rate(frequency)
position_list = []
for i in range(frequency * nsec):
position_list.append(list(self.get_position()))
R.sleep()
#if self.verbose:
#prettyprint(position_list)
#print np.average(position_list,0)
return np.average(position_list, 0)
def minimize_yvalue(self, initial_state, eps=0.005):
'''
Executive function. Calculates and applies forward rotation.
Moves torso until the upright position is found
@param initial_state: initial joint state of torso (usually "Home" position) with leveled head joint
@type initial_state: list [joint state lower neck, joint state tilt, joint state upper neck]
@param eps: minimum accuracy for upright position
@type eps: float
'''
initial_state = np.asarray(initial_state)
states = []
step = 0.1
step_factor = [1, -1]
state = initial_state
self.sss.move("torso", [state.tolist()])
rospy.sleep(3)
# calculate rotational reference and rotate to center
states.append([self.get_average_position(10)[1], state])
while step > eps:
for factor in step_factor:
state = initial_state + np.asarray(
[factor * step, 0, factor * -step])
self.sss.move("torso", [state.tolist()])
rospy.sleep(2.5)
states.append([self.get_average_position(10)[1], state])
if self.verbose:
prettyprint(states)
states = sorted(states)
prettyprint(states)
step /= 2
states = [states[0]]
initial_state = states[0][1] # initial state for next iteration
if self.verbose:
print '*' * 20
print(states)
print initial_state
self.sss.move("torso", [state.tolist()])
rospy.sleep(2)
for i in range(3):
position = self.get_average_position(10)
rotational_error = math.atan(position[0] / position[2])
state -= np.asarray([0, rotational_error, 0])
self.sss.move("torso", [state.tolist()])
return state
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size,
square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
# --------------------
# receive images
# -----------------
for v in self._images_received:
self._images_received[v] = False
for v in self.transformations_received:
self.transformations_received[v] = False
print self._images_received
start_time = rospy.Time.now()
while (not all(self._images_received.values())
or not all(self.transformations.values())):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still
# missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
for name, v in self._images_received.iteritems():
if not v:
print "--> still waiting for sample from %s" % name
for name, v in self._transformations_received.iteritems():
if not v:
print "--> still waiting for sample from %s" % name
start_time = rospy.Time.now()
print "got sample"
#latest_left = self._left
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb
# --------------
for name, image in self._images.iteritems():
image = image["image"]
#print image.header
cvImage = self.bridge.imgmsg_to_cv(image, "mono8")
imagecv = cv2util.cvmat2np(cvImage)
try:
corners = checkerboard_detector.detect_image_points(
imagecv, is_grayscale=True)
except:
# cb not found
rospy.logwarn("No calibration pattern found for: '%s'" % name)
return False
else:
print "cb found: %s" % name
img_points = []
for (x, y) in corners.reshape(-1, 2):
img_points.append(ImagePoint(x, y))
# create camera msg left
# ----------------------
cam_msg = CameraMeasurement()
cam_msg.camera_id = name
#print "crash before"
cam_msg.header.stamp = image.header.stamp
#print "crash after"
#cam_msg.cam_info = image["camera_info"]
cam_msg.image_points = img_points
cam_msg.verbose = False
robot_msg.M_cam.append(cam_msg)
cam_msg.image = image
# print cam_msg.camera_id
#print cam_msg.header
# cam_msg.image_rect = latest_left["image_rect"]
# cam_msg.features = # Not implemented here
#----------------------
#DEBUG publish pic
#-----------------
#self._image_pub_left.publish(latest_left["image"])
#----------------------
# Fill robot_msg
#----------------------
robot_msg.M_chain = self.transformations.values()
self._robot_measurement_pub.publish(robot_msg)
return True
class TorsoCalibration():
def __init__(self):
'''
Configures the calibration node
Reads configuration from parameter server or uses default values
'''
rospy.init_node(NODE)
print "==> started " + NODE
# get parameter from parameter server or set defaults
self.pattern_size = rospy.get_param('~pattern_size', "9x6")
self.square_size = rospy.get_param('~square_size', 0.03)
self.alpha = rospy.get_param('~alpha', 0.0)
self.verbose = rospy.get_param('~verbose', True)
self.save_result = rospy.get_param('~save_result', False)
# split pattern_size string into tuple, e.g '9x6' -> tuple(9,6)
self.pattern_size = tuple((int(self.pattern_size.split(
"x")[0]), int(self.pattern_size.split("x")[1])))
self.camera_info = CameraInfo()
self.camera_info_received = False
self.latest_image = Image()
self.bridge = CvBridge()
# set up Checkerboard and CheckerboardDetector
self.board = Checkerboard(self.pattern_size, self.square_size)
self.detector = CheckerboardDetector(self.board)
self.sss = simple_script_server()
self.ts = TorsoState()
def __camera_info_callback__(self, data):
'''
executed on new message in /cam3d/rgb/camera_info
stores received data
'''
self.camera_info = data
self.camera_info_received = True
def __image_raw_callback__(self, data):
'''
executed on new message in /cam3d/rgb/image_raw
stores newest image
'''
self.latest_image = data
def run(self):
'''
Runs the calibration
'''
print "==> starting torso pitch/height calibration"
# subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients
rospy.Subscriber('/cam3d/rgb/camera_info', CameraInfo,
self.__camera_info_callback__)
# subscribe to /cam3d/rgb/image_raw for image data
rospy.Subscriber(
'/cam3d/rgb/image_color', Image, self.__image_raw_callback__)
# wait until camera informations are recieved.
start_time = rospy.Time.now()
while not (self.camera_info_received or rospy.is_shutdown()):
rospy.sleep(0.05)
if start_time + rospy.Duration(2.0) < rospy.Time.now():
# print warning every 2 seconds if the message is stil missing
print "--> still waiting for /cam3d/rgb/camera_info"
start_time = rospy.Time.now()
# convert camera matrix an distortion coefficients and store them
camera_matrix = self.camera_info.K
cm = np.asarray(camera_matrix)
self.cm = np.reshape(cm, (3, 3))
dist_coeffs = self.camera_info.D
self.dc = np.asarray(dist_coeffs)
# initialize torso for movement
self.sss.init("torso")
# get initial state of Torso (Upper Tilt Joint and Head Axis Joint horizontal)
state = list(self.ts.calc_references())
print state
# start minimization for getting the upright position of the torso
start_time = rospy.Time.now()
solution = self.minimize_yvalue(state)
if self.verbose:
print 'Elapsed time: ', (rospy.Time.now() - start_time).to_sec()
print 'Upright forward Position found at %s' % solution
print 'Seed was ', state
if self.save_result:
system('roslaunch cob_torso_calibration update_torso_calibration_urdf.launch')
#
def get_position(self):
'''
returns the Y value in subpixel accuracy for the center of the recognized checkerboard
'''
r = rospy.Rate(10) # Hz
while rospy.Time.now() - self.latest_image.header.stamp > rospy.Duration(1) and not rospy.is_shutdown():
print 'no image received'
print rospy.Time.now(), self.latest_image.header.stamp
r.sleep()
cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8")
image_raw = cv2util.cvmat2np(cvImage)
#image_processed = cv2.undistort(image_raw, self.cm, self.dc)
#points=self.detector.detect_image_points(image_processed,True)
(rmat, tvec, image_points) = self.detector.calculate_object_pose_ransac(image_raw, self.cm, self.dc, True)
#print tvec[1]
return tvec[0][0], tvec[1][0], tvec[2][0]
def get_average_position(self, nsec, frequency=15):
'''
returns the average Y value for the last nsec seconds
@param nsec: time for samples to be taken
@type nsec: integer
@param frequency: update frequency for image topic
@type frequency: integer
'''
R = rospy.Rate(frequency)
position_list = []
for i in range(frequency * nsec):
position_list.append(list(self.get_position()))
R.sleep()
#if self.verbose:
#prettyprint(position_list)
#print np.average(position_list,0)
return np.average(position_list, 0)
def minimize_yvalue(self, initial_state, eps=0.005):
'''
Executive function. Calculates and applies forward rotation.
Moves torso until the upright position is found
@param initial_state: initial joint state of torso (usually "Home" position) with leveled head joint
@type initial_state: list [joint state lower neck, joint state tilt, joint state upper neck]
@param eps: minimum accuracy for upright position
@type eps: float
'''
initial_state = np.asarray(initial_state)
states = []
step = 0.1
step_factor = [1, -1]
state = initial_state
self.sss.move("torso", [state.tolist()])
rospy.sleep(3)
# calculate rotational reference and rotate to center
states.append([self.get_average_position(10)[1], state])
while step > eps:
for factor in step_factor:
state = initial_state + np.asarray(
[factor * step, 0, factor * -step])
self.sss.move("torso", [state.tolist()])
rospy.sleep(2.5)
states.append([self.get_average_position(10)[1], state])
if self.verbose:
prettyprint(states)
states = sorted(states)
prettyprint(states)
step /= 2
states = [states[0]]
initial_state = states[0][1] # initial state for next iteration
if self.verbose:
print '*' * 20
print(states)
print initial_state
self.sss.move("torso", [state.tolist()])
rospy.sleep(2)
for i in range(3):
position = self.get_average_position(10)
rotational_error = math.atan(position[0] / position[2])
state -= np.asarray([0, rotational_error, 0])
self.sss.move("torso", [state.tolist()])
return state