def test_checkerboard_object_points_4x4(self):
c = Checkerboard((4,4), 1)
points = c.get_pattern_points()
points_ = [[0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0],
[0, 1, 0], [1, 1, 0], [2, 1, 0], [3, 1, 0],
[0, 2, 0], [1, 2, 0], [2, 2, 0], [3, 2, 0],
[0, 3, 0], [1, 3, 0], [2, 3, 0], [3, 3, 0]]
self.assertTrue(np.allclose(points, points_))
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 __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 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):
'''
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_checkerboard_object_points_4x4(self):
c = Checkerboard((4, 4), 1)
points = c.get_pattern_points()
points_ = [
[0, 0, 0],
[1, 0, 0],
[2, 0, 0],
[3, 0, 0],
[0, 1, 0],
[1, 1, 0],
[2, 1, 0],
[3, 1, 0],
[0, 2, 0],
[1, 2, 0],
[2, 2, 0],
[3, 2, 0],
[0, 3, 0],
[1, 3, 0],
[2, 3, 0],
[3, 3, 0],
]
self.assertTrue(np.allclose(points, points_))
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 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
def test_checkerboard_object_points_2x2_scale(self):
c = Checkerboard((2, 2), 1.5)
points = c.get_pattern_points()
points_ = [[0, 0, 0], [1.5, 0, 0], [0, 1.5, 0], [1.5, 1.5, 0]]
self.assertTrue(np.allclose(points, points_))
def test_checkerboard_object_points_2x2_scale(self):
c = Checkerboard((2,2), 1.5)
points = c.get_pattern_points()
points_ = [[0, 0, 0], [1.5, 0, 0], [0, 1.5, 0], [1.5, 1.5, 0]]
self.assertTrue(np.allclose(points, points_))
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
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
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
