def _find_rotated_enclosing_rect(self, image, object_id):
"""Find the rectangle with arbitrary orientation that encloses the
segmented object in the given image with minimum area.
Note: The passed image is being modified and published on the
visualization image topic!
:param image: An image (numpy array) of shape (height, width, 3).
:param object_id: The object identifier.
:return: The rotated rectangle enclosing the segmented object,
given by ((cx, cy), (w, h), alpha).
:raise: ValueError if the given object could not be segmented.
"""
# first, detect object in image
if object_id == 'hand':
det = self._detection.detect_best(image=image, threshold=0.5)
else:
det = self._detection.detect_object(image=image,
object_id=object_id,
threshold=0.5)
img_copy = np.copy(image)
draw_detection(image=img_copy, detections=det)
self._pub_vis.publish(img_to_imgmsg(img=img_copy))
rospy.sleep(self._tsleep)
# second, segment object within bounding box
if det['box'] is not None:
xul, yul, xlr, ylr = [int(round(x)) for x in det['box']]
# selection = np.copy(image[yul:ylr, xul:xlr])
# self._pub_vis.publish(img_to_imgmsg(img=selection))
# rospy.sleep(self._tsleep)
seg = self._segmentation.detect_best(image=image[yul:ylr, xul:xlr],
threshold=0.8)
# self._pub_vis.publish(img_to_imgmsg(img=seg['mask']))
# rospy.sleep(self._tsleep)
handstring = ' in hand' if object_id == 'hand' else ''
if seg['mask'] is not None:
seg['id'] = det['id']
seg['score'] = det['score']
self._logger.debug("Segmented {}{}.".format(
seg['id'], handstring))
# place segmentation in appropriate place in image
seg['box'] += np.array([xul, yul, xul, yul])
mask = np.zeros(shape=image.shape[:2], dtype=np.uint8)
h, w = seg['mask'].shape[:2]
mask[yul:yul + h, xul:xul + w] = seg['mask']
seg['mask'] = mask
draw_detection(image=image, detections=seg)
rroi = mask_to_rroi(mask=seg['mask'])
else:
raise ValueError("Segmentation of {}{} failed!".format(
seg['id'], handstring))
else:
raise ValueError("Detection of {} failed!".format(object_id))
draw_rroi(image=image, rroi=rroi)
self._pub_vis.publish(img_to_imgmsg(img=image))
rospy.sleep(self._tsleep)
return rroi, det['id']
def publish_vis(self, image):
"""Publish an image to the ROS topic defined in
settings.debug.topic_img4.
:param image: The image (numpy array) to publish.
:return:
"""
self._pub_vis.publish(img_to_imgmsg(img=image))
def publish(self):
"""Publish self.img to the ROS topic defined in
settings.debug.topic_visualization.
"""
if topic_visualization == '/robot/xdisplay':
xsize = (1024, 600)
if any([(a > b) for a, b in zip(self.img.shape[:2], xsize)]):
self.img = cv2.resize(src=self.img, dsize=xsize)
self.pub.publish(img_to_imgmsg(img=self.img))
def visual_test(self, tto, btc):
"""Visualize the result of the calibration.
Compute the robot coordinates of the calibration pattern. Then project
them to camera space and compute corresponding pixel coordinates. Draw
them onto the image to see if the markings are reasonable close to the
marks on the calibration pattern.
:param tto: The homogeneous transform between TCP and the origin of
the calibration pattern.
:param btc: The homogeneous transform between Baxter's base frame and
the camera frame.
:return:
"""
hom_pattern = np.concatenate(
[self._pattern,
np.ones((1, self._pattern.shape[1]))], axis=0)
tcp_pattern = np.dot(tto, hom_pattern)
btc_inv = inv_trafo_matrix(trafo=btc)
self.logger.info("Detect pattern and compare it with estimate.")
patternfound = 0
while patternfound == 0 and not rospy.is_shutdown():
if self._move():
rospy.sleep(1.0)
btt = self._robot.hom_gripper_to_robot(arm=self._arm)
color, _, _ = self._kinect.collect_data(color=True)
self._pub_vis.publish(img_to_imgmsg(img=color))
patternfound, centers = cv2.findCirclesGridDefault(
image=color,
patternSize=self._patternsize,
flags=cv2.CALIB_CB_ASYMMETRIC_GRID)
if patternfound == 0:
self.logger.debug("No pattern found!")
continue
fname = os.path.join(self._sink, "3_vis")
cv2.imwrite(fname + ".jpg", color)
cv2.drawChessboardCorners(image=color,
patternSize=self._patternsize,
corners=centers,
patternWasFound=patternfound)
self._pub_vis.publish(img_to_imgmsg(img=color))
cv2.imwrite(fname + "_det.jpg", color)
centers = centers[:, 0, :]
rob_pattern = np.dot(btt, tcp_pattern)
cam_pattern = np.dot(btc_inv, rob_pattern)
pixels = np.zeros_like(centers)
for i in xrange(cam_pattern.shape[1]):
coord = list(cam_pattern[:-1, i])
print i, coord,
pixels[i] = self._kinect.color.projection_camera_to_pixel(
position=coord)
# flip in y direction
pixels[i, 0] = color.shape[1] - pixels[i, 0]
print centers[i], pixels[i]
cv2.circle(color, tuple(int(x) for x in pixels[i]), 3,
[255, 0, 0] if i == 0 else [0, 255, 0], 2)
self._pub_vis.publish(img_to_imgmsg(img=color))
cv2.imwrite(fname + "_est.jpg", color)
delta = centers - pixels
self.logger.info("Offset in detected and estimated pixel "
"coordinates:")
self.logger.info("Mean: {}".format(delta.mean(axis=0)))
self.logger.info("Std: {}".format(delta.std(axis=0)))
self.logger.info("Median: {}".format(np.median(delta, axis=0)))
def estimate_cam_trafo(self, n, tto):
"""Estimate the transformation between Baxter's base frame and the
camera frame using singular value decomposition.
:param n: The number of absolute end effector poses to use.
:param tto: The homogeneous transform between TCP and the origin of
the calibration pattern.
:return: The homogeneous transform between Baxter's base frame and
the camera frame.
"""
bto = list()
cto = list()
self.logger.info("Record %d absolute point cloud pairs." % n)
pattern = self._pattern.T[:, np.newaxis, :]
hom_pattern = np.concatenate(
[self._pattern,
np.ones((1, self._pattern.shape[1]))], axis=0)
while len(bto) < n and not rospy.is_shutdown():
if self._move():
rospy.sleep(1.0)
bttn = self._robot.hom_gripper_to_robot(arm=self._arm)
color, _, _ = self._kinect.collect_data(color=True)
self._pub_vis.publish(img_to_imgmsg(img=color))
patternfound, centers = cv2.findCirclesGridDefault(
image=color,
patternSize=self._patternsize,
flags=cv2.CALIB_CB_ASYMMETRIC_GRID)
if patternfound == 0:
self.logger.debug("No pattern found!")
continue
rot, trans, _ = cv2.solvePnPRansac(
objectPoints=pattern,
imagePoints=centers,
cameraMatrix=self._kinect.color.camera_matrix,
distCoeffs=self._kinect.color.distortion_coeff,
flags=cv2.CV_ITERATIVE)
rot, _ = cv2.Rodrigues(rot)
cton = np.eye(4)
cton[:-1, :-1] = rot
cton[:-1, -1] = np.squeeze(trans)
fname = os.path.join(self._sink, "2_tto_%d" % (len(bto) + 1))
cv2.imwrite(fname + ".jpg", color)
cv2.drawChessboardCorners(image=color,
patternSize=self._patternsize,
corners=centers,
patternWasFound=patternfound)
self._pub_vis.publish(img_to_imgmsg(img=color))
cv2.imwrite(fname + "_det.jpg", color)
bto.append(np.dot(np.dot(bttn, tto), hom_pattern))
fname = os.path.join(self._sink, "2_bto_%d.npz" % len(bto))
np.savez(fname, bttn=bto[-1])
cto.append(np.dot(cton, hom_pattern))
fname = os.path.join(self._sink, "2_cto_%d.npz" % len(cto))
np.savez(fname, cton=cto[-1])
self.logger.info("Compute affine transformation from 3D point "
"correspondences.")
# see http://stackoverflow.com/questions/15963960/opencv-2-4-estimateaffine3d-in-python
# data of shape (Nx3)
cto = np.array(cto).reshape((4, -1)).T[:, :-1]
bto = np.array(bto).reshape((4, -1)).T[:, :-1]
cto_mean = cto.mean(axis=0)
bto_mean = bto.mean(axis=0)
# compute covariance
cov = np.dot((cto - cto_mean).T, bto - bto_mean)
u, s, v = np.linalg.svd(cov)
rot = np.dot(u, v.T)
if np.linalg.det(rot) < 0:
v[:, 2] = -v[:, 2]
rot = np.dot(u, v.T)
trans = bto_mean - np.dot(rot, cto_mean)
trafo = np.eye(4)
trafo[:-1, :-1] = rot
trafo[:-1, -1] = trans
fname = os.path.join(self._sink, "2_btc.npz")
np.savez(fname, btc=trafo)
return trafo
def estimate_hand_trafo(self, n):
"""Estimate the transformation between Baxter's end effector and the
origin of the calibration pattern mounted on it using the algorithm
by Tsai and Lenz.
:param n: The number of absolute end effector poses to use.
:return: The homogeneous transform between TCP and the origin of the
calibration pattern.
"""
if n < 3:
raise ValueError("At least 3 poses are needed to compute the "
"transformation!")
btt = list()
cto = list()
pattern = self._pattern.T[:, np.newaxis, :]
self.logger.info("Record %d absolute pose and pattern pairs." % n)
while len(btt) < n and not rospy.is_shutdown():
if self._move():
rospy.sleep(1.0)
bttn = self._robot.hom_gripper_to_robot(arm=self._arm)
color, _, _ = self._kinect.collect_data(color=True)
self._pub_vis.publish(img_to_imgmsg(img=color))
patternfound, centers = cv2.findCirclesGridDefault(
image=color,
patternSize=self._patternsize,
flags=cv2.CALIB_CB_ASYMMETRIC_GRID)
if patternfound == 0:
self.logger.debug("No pattern found!")
continue
rot, trans, _ = cv2.solvePnPRansac(
objectPoints=pattern,
imagePoints=centers,
cameraMatrix=self._kinect.color.camera_matrix,
distCoeffs=self._kinect.color.distortion_coeff,
flags=cv2.CV_ITERATIVE)
rot, _ = cv2.Rodrigues(rot)
cton = np.eye(4)
cton[:-1, :-1] = rot
cton[:-1, -1] = np.squeeze(trans)
fname = os.path.join(self._sink, "1_tto_%d" % (len(btt) + 1))
cv2.imwrite(fname + ".jpg", color)
cv2.drawChessboardCorners(image=color,
patternSize=self._patternsize,
corners=centers,
patternWasFound=patternfound)
self._pub_vis.publish(img_to_imgmsg(img=color))
cv2.imwrite(fname + "_det.jpg", color)
btt.append(bttn)
fname = os.path.join(self._sink, "1_btt_%d.npz" % len(btt))
np.savez(fname, bttn=btt[-1])
cto.append(cton)
fname = os.path.join(self._sink, "1_cto_%d.npz" % len(cto))
np.savez(fname, cton=cto[-1])
self.logger.info("Compute %d relative transforms." % ((n**2 - n) / 2))
bttij = list()
ctoij = list()
for i in range(n):
for j in range(i + 1, n):
bttij.append(np.dot(inv_trafo_matrix(btt[j]), btt[i]))
ctoij.append(np.dot(cto[j], inv_trafo_matrix(cto[i])))
self.logger.info('Apply algorithm by Tsai and Lenz (1989).')
tto = tsai_lenz_89(a=bttij, b=ctoij)
fname = os.path.join(self._sink, "1_tto.npz")
np.savez(fname, tto=tto)
return tto