def check_reprojection(img, marker_pos, marker_img_pos, rvec, tvec, cam_mtx,
dist_coeffs):
"""Project marker positions from 3D to the image plane and compare with user indicated positions."""
marker_img_pos_proj, _ = cv2.projectPoints(marker_pos, rvec, tvec, cam_mtx,
dist_coeffs)
marker_img_pos_proj = marker_img_pos_proj.squeeze()
# Check accuracy by reprojecting calibration markers
error = np.linalg.norm(marker_img_pos -
marker_img_pos_proj) / len(marker_img_pos)
print(f"RMS error: {error}")
for p in marker_img_pos:
draw_marker(*p, img, color=(0, 0, 255))
for p in marker_img_pos_proj:
draw_marker(*p, img, color=(0, 255, 0))
# Draw world axis
cv2.drawFrameAxes(img, cam_mtx, dist_coeffs, rvec, tvec, 1)
win_name = 'Reprojection'
cv2.namedWindow(win_name, cv2.WINDOW_GUI_NORMAL)
cv2.resizeWindow(win_name, 960, 540)
cv2.imshow(win_name, img)
print("Press any key to continue.")
cv2.waitKey(0)
cv2.destroyAllWindows()
def draw_debug_bgr(self, cam, img_cam=None):
cnt_color, cntmax_color, thickness = (255,0,0), (0, 255,0), 3
if self.img is None:
return np.zeros((cam.h, cam.w, 3), dtype=np.uint8)
else:
if self.display_mode == 1: # input
debug_img = self.img.copy()
elif self.display_mode == 2: # aruco
debug_img = cv2.cvtColor(self.img, cv2.COLOR_GRAY2BGR)
#debug_img = cv2.cvtColor(self.threshold1, cv2.COLOR_GRAY2BGR)
#cv2.drawContours(debug_img, self.cnt_max, -1, cntmax_color, 3)
#cv2.rectangle(debug_img, tuple(self.tl), tuple(self.br), (0,0,255), 1)
if self.corners is not None:
aruco.drawDetectedMarkers(image=debug_img, corners=self.corners)
if self.rvec is not None:
cv2.drawFrameAxes(debug_img, cam.K, cam.D, self.rvec, self.tvec, 0.1)
self._draw_path(cam, debug_img)
elif self.display_mode == 3: # bird eye
if self.bird_eye is None :
debug_img = self.img.copy()
else:
chroma_blue, chroma_green = (187, 71, 0), (64, 177, 0)
debug_img = self.bird_eye.undist_unwarp_img(cv2.cvtColor(self.img, cv2.COLOR_GRAY2BGR), cam, fill_bg=chroma_green)
ui = cv_be.UnwarpedImage()
ui.draw_grid(debug_img, self.bird_eye, cam, 0.1)
ui.draw_path(debug_img, self.bird_eye, cam, self.track_path.points)
self.draw_timing(debug_img)
return debug_img
def _updateFeatures(self, gate_centre, rvec, tvec, contour, bounding_box):
if gate_centre is not None:
'''Updates all the features on the frame.'''
# Plot the centre of the bounding box (on frame, using centre coords,
# radius, color, filled)
# cv.circle(self.frame, gate_centre, 5, (255, 255, 255), -1)
# Draw contours/box on frame, biggest one, all of them, color, thickness
cv.drawContours(self.frame, contour, -1, (0, 255, 0), 6)
# cv.drawContours(self.frame, [bounding_box], -1, (0, 255, 255), 2)
# Draw the axes on frame using the calibration data and PnP results
cv.drawFrameAxes(self.frame, config.CAMERA_MATRIX,
config.DISTORTION, rvec, tvec, 25)
def draw_axes(image, right_camera):
"""
Draw axes on the chosen camera background
:param image:
:param right_camera:
:return: nothing
"""
extrinsic_camera_matrix, extrinsic_distortion_vector, extrinsic_rotation_vector, extrinsic_translation_vector \
= get_extrinsic_parameters(right_camera)
cv.drawFrameAxes(image, extrinsic_camera_matrix, extrinsic_distortion_vector, extrinsic_rotation_vector,
extrinsic_translation_vector, 1)
def plot_marker_axes(image_path, vectors, factor):
"""
Used to plot multiple individual markers
"""
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
dist_coeffs = vectors[str(factor)]['dist_coeffs']
cam_matrix = vectors[str(factor)]['cam_matrix']
marker_size = 0.025 # 25mm
between_markers = 0.005 # 5mm
length_of_axis = marker_size / 2
img = cv2.imread(image_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict)
frame_markers = aruco.drawDetectedMarkers(img.copy(), corners, ids)
rvecs, tvecs, objPoints = aruco.estimatePoseSingleMarkers(
corners, marker_size, cam_matrix, dist_coeffs)
for i in range(len(tvecs)):
frame_markers = cv2.drawFrameAxes(frame_markers,
cam_matrix,
dist_coeffs,
rvecs[i],
tvecs[i],
length_of_axis,
thickness=2)
plt.figure(figsize=(12, 6))
plt.imshow(frame_markers, interpolation="nearest")
return rvecs, tvecs
def get_position_lib(self, bbox, ints, source_name):
"""Compute transformation of 2d pixel coordinates to 3d camera coordinates."""
camera = self.make_cam_mat(ints)
best_bbox = (None, None, source_name)
closest_dist = 1000
for i in range(len(bbox)):
obj_points, img_points = self.bbox_to_img_obj_pts(bbox[i])
if self._cam_to_ext_guess[source_name][0]:
# initialize the position estimate with the previous extrinsics solution
# then iteratively solve for new position
old_rvec, old_tvec = self._cam_to_ext_guess[source_name][1]
_, rvec, tvec = cv2.solvePnP(obj_points, img_points, camera,
np.zeros(
(5, 1)), old_rvec, old_tvec,
True, cv2.SOLVEPNP_ITERATIVE)
else:
# Determine current extrinsic solution for the tag
_, rvec, tvec = cv2.solvePnP(obj_points, img_points, camera,
np.zeros((5, 1)))
#Save extrinsics results to help speed up next attempts to locate bounding box
self._cam_to_ext_guess[source_name] = (True, (rvec, tvec))
if self._debug:
self.back_prop_error(obj_points, img_points, rvec, tvec,
camera)
cv2.drawFrameAxes(self._image[source_name], camera,
np.zeros((5, 1)), rvec, tvec, 100)
if not self._display_images:
cv2.imshow('frame', self._image[source_name])
cv2.waitKey()
cv2.destroyAllWindows()
dist = (float(tvec[0][0])**2 + float(tvec[1][0])**2 +
float(tvec[2][0])**2)**(.5) / 1000.0
if dist < closest_dist:
closest_dist = dist
best_bbox = (tvec, rvec, source_name)
# Flag indicating if the best april tag been found/located
self._tag_not_located = best_bbox[0] is None and best_bbox[1] is None
return best_bbox
def draw_reprojection(color, objPoints, imgPoints, cameraMatrix, distCoeffs, patternSize, squaresize, folder, i):
""" Pour une image, reprojeter des points et les axes"""
# Vérification de la calibration de la caméra en reprojection:
# Montrer axes
ret, rvecs, tvecs = cv.solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs)
img =cv.drawFrameAxes(color.copy(), cameraMatrix, distCoeffs, rvecs, tvecs, 3*squaresize, 5)
cv.imwrite('{}reprojection_axes_{}.jpg'.format(folder, i), img) #Z est en bleu, Y en vert, X en rouge
pts, jac = cv.projectPoints(objPoints, rvecs, tvecs, cameraMatrix, distCoeffs)
img = cv.drawChessboardCorners(color.copy(), patternSize, pts, 1)
cv.imwrite('{}reprojection_points_{}.jpg'.format(folder, i), img)
return img
def find_extrinsic_parameters(img, obj_points, img_points, camera_matrix, dist_coeffs, show=True, save=False,
prefix=''):
if save:
if prefix == '':
return print('Veuillez saisir un prefix')
np.savetxt('matrices/points/calibration_image_points/' + prefix + '_img_points', img_points)
np.savetxt('matrices/points/calibration_object_points/' + prefix + '_obj_points', obj_points)
obj_points = np.array(obj_points, 'float32')
img_points = np.array(img_points, 'float32')
size = img.shape[:2]
cali_flag = cv2.CALIB_FIX_INTRINSIC | cv2.CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_USE_INTRINSIC_GUESS
retval, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera([obj_points], [img_points], size,
camera_matrix,
dist_coeffs, flags=cali_flag)
(rotation_matrix, _) = cv2.Rodrigues(rvecs[0])
if show:
mean_error = 0
# dessiner les axes
img = cv2.drawFrameAxes(img, camera_matrix, dist_coeffs, rvecs[0], tvecs[0], 5)
# redessiner les points source et calculer le pourcentage d'erreur comis
img_points2, jacobian = cv2.projectPoints(obj_points, rvecs[0], tvecs[0], camera_matrix, dist_coeffs)
for px in img_points2:
cv2.circle(img, (px[0][0], px[0][1]), 10, (255, 255, 255), 5)
cv2.namedWindow("errors", cv2.WINDOW_NORMAL)
cv2.imshow('errors', img)
cv2.waitKey(0)
for i in range(0, len(img_points2)):
mean_error += cv2.norm(img_points[i], img_points2[i][0], cv2.NORM_L2) / len(img_points2)
print('erreur moyenne', mean_error)
if save:
np.savetxt('matrices/camera_matrices/extrinsic/' + prefix + '_camera_matrix', camera_matrix)
np.savetxt('matrices/vectors/distortion/extrinsic/' + prefix + '_distortion_vector', dist_coeffs)
np.savetxt('matrices/vectors/rotation/' + prefix + '_rotation_vector', rvecs[0])
np.savetxt('matrices/vectors/translation/' + prefix + '_translation_vector', tvecs[0])
np.savetxt('matrices/rotation_matrix/' + prefix + '_rotation_matrix', rotation_matrix)
return camera_matrix, dist_coeffs, rotation_matrix, rvecs[0], tvecs[0]
def draw_axes(self, img, R, t):
img = cv2.drawFrameAxes(img, self.camera_matrix, self.dist_coefs, R, t, 30)
def draw(self,
image,
color,
thickness=1,
view_matrix=None,
camera_matrix=None,
dist_coeffs=None):
"""Draws the track"""
if self.is_confirmed():
track_color = (0, 200, 0, 0)
text_color = (50, 50, 50)
else:
if self.is_occluded():
track_color = (0, 0, 200, 0.3)
text_color = (250, 250, 250)
else:
track_color = (200, 0, 0, 0.3)
text_color = (250, 250, 250)
if self.is_confirmed():
if self.is_located() and self.is_confirmed():
if view_matrix is not None and \
camera_matrix is not None and \
dist_coeffs is not None:
sensor_pose = Vector6D().from_transform(
np.dot(np.linalg.inv(view_matrix),
self.pose.transform()))
rot = sensor_pose.rotation().to_array()
depth = sensor_pose.position().z
# for opencv convention
R = euler_matrix(rot[0][0], rot[1][0], rot[2][0], "rxyz")
rvec = cv2.Rodrigues(R[:3, :3])[0]
cv2.putText(image, "{0:.3}m".format(depth),
(self.bbox.xmin + 5, self.bbox.ymin - 5),
cv2.FONT_HERSHEY_SIMPLEX, .6, (255, 255, 255),
2)
cv2.drawFrameAxes(image, camera_matrix, dist_coeffs, rvec,
sensor_pose.position().to_array(), 0.1)
cv2.rectangle(image, (self.bbox.xmin, self.bbox.ymax - 26),
(self.bbox.xmax, self.bbox.ymax), (200, 200, 200),
-1)
# if self.mask is not None:
# mask = np.full((image.shape[0], image.shape[1], 1), 255)
# xmin = int(self.bbox.xmin)
# ymin = int(self.bbox.ymin)
# xmax = int(self.bbox.xmax)
# ymax = int(self.bbox.ymax)
# mask[ymin:ymax, xmin:xmax] = self.mask
# contours, hierarchy = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# cv2.drawContours(image, contours, 0, track_color, thickness)
self.bbox.draw(image, track_color, 2)
self.bbox.draw(image, text_color, 1)
cv2.putText(image, "{}".format(self.id[:6]),
(self.bbox.xmax - 60, self.bbox.ymax - 8),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1)
cv2.putText(image, self.label,
(self.bbox.xmin + 5, self.bbox.ymax - 8),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, text_color, 1)
if "facial_landmarks" in self.features:
self.features["facial_landmarks"].draw(image, track_color,
thickness)
else:
self.bbox.draw(image, track_color, 1)
def draw(self, image, color, thickness=1, view_pose=None, camera=None):
"""Draws the track
"""
if self.is_confirmed() or self.is_occluded():
track_color = (0, 200, 0)
text_color = (50, 50, 50)
mask_color = (0, 100, 0)
if self.is_static():
track_color = (0, 200, 0)
text_color = (50, 50, 50)
mask_color = (0, 100, 0)
else:
if self.is_lost():
track_color = (0, 0, 200)
text_color = (250, 250, 250)
mask_color = (0, 0, 100)
else:
track_color = (200, 0, 0)
text_color = (250, 250, 250)
mask_color = (100, 0, 0)
if self.is_confirmed():
if self.is_located():
if view_pose is not None and camera is not None:
view_matrix = view_pose.transform()
camera_matrix = camera.camera_matrix()
dist_coeffs = camera.dist_coeffs
sensor_pose = Vector6D().from_transform(np.dot(np.linalg.inv(view_matrix), self.pose.transform()))
rot = sensor_pose.rotation().to_array()
depth = sensor_pose.position().z
# for opencv convention
R = euler_matrix(rot[0][0], rot[1][0], rot[2][0], "rxyz")
rvec = cv2.Rodrigues(R[:3, :3])[0]
cv2.putText(image, "{:0.3}m".format(depth),
(self.bbox.xmin+5, self.bbox.ymin-5),
cv2.FONT_HERSHEY_SIMPLEX,
.6,
(255, 255, 255),
2)
cv2.drawFrameAxes(image, camera_matrix, dist_coeffs,
rvec,
sensor_pose.position().to_array(), 0.1)
else:
depth = self.bbox.depth
cv2.putText(image, "{0:.2}m".format(depth),
(self.bbox.xmin+5, self.bbox.ymin-5),
cv2.FONT_HERSHEY_SIMPLEX,
.6,
(255, 255, 255),
2)
if self.mask is not None:
contours, hierarchy = cv2.findContours(self.mask.astype("uint8"), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
roi = image[int(self.bbox.ymin):int(self.bbox.ymax), int(self.bbox.xmin):int(self.bbox.xmax)]
overlay = roi.copy()
cv2.drawContours(overlay, contours, -1, mask_color, 2, cv2.LINE_8, hierarchy, 100)
alpha = 0.6
roi = cv2.addWeighted(overlay, alpha, roi, 1 - alpha, 0, roi)
cv2.rectangle(image, (self.bbox.xmin, self.bbox.ymax-26),
(self.bbox.xmax, self.bbox.ymax),
(200, 200, 200), -1)
self.bbox.draw(image, track_color, 2)
self.bbox.draw(image, text_color, 1)
cv2.putText(image,
"{}".format(self.id[:6]),
(self.bbox.xmax-75, self.bbox.ymax-8),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
text_color,
1)
if "facial_landmarks" in self.features:
self.features["facial_landmarks"].draw(image,
track_color,
thickness)
else:
self.bbox.draw(image, track_color, 1)
cap = cv.VideoCapture(0)
cap.set(cv.CAP_PROP_FRAME_WIDTH, imsize[0])
cap.set(cv.CAP_PROP_FRAME_HEIGHT, imsize[1])
rot, trans = None, None
while cv.waitKey(1) != 27:
img = cap.read()[1]
# detection with aruco
if rot is None:
corners, ids = cv.aruco.detectMarkers(img, adict)[:2]
if ids is not None:
rvecs, tvecs = cv.aruco.estimatePoseSingleMarkers(corners, marker_len, K, None)[:2]
rot, trans = rvecs[0].ravel(), tvecs[0].ravel()
# tracking and refinement with rapid
if rot is not None:
for i in range(5): # multiple iterations
ratio, rot, trans = cv.rapid.rapid(img, 40, line_len, obj_points, tris, K, rot, trans)
if ratio < 0.8:
# bad quality, force re-detect
rot, trans = None, None
break
# drawing
cv.putText(img, "detecting" if rot is None else "tracking", (0, 20), cv.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255))
if rot is not None:
cv.drawFrameAxes(img, K, None, rot, trans, marker_len)
cv.imshow("tracking", img)
def draw_axes(self, img, r, t):
cv2.drawFrameAxes(img, self.camera_matrix, self.dist_coefficients, r,
t, 30)
def plot_gridboard_axes(image_path, vectors, factor, gridboards=None):
"""
Used to plot multiple gridboards, with one large set of axes per gridboard
"""
dist_coeffs = vectors[str(factor)]['dist_coeffs']
cam_matrix = vectors[str(factor)]['cam_matrix']
nx = 2
ny = 2
marker_size = 0.025 # 25mm
between_markers = 0.005 # 5mm
markers_per_board = nx * ny
length_of_axis = (marker_size + between_markers) * nx - between_markers
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
offset = 0
if not gridboards:
gridboards = [
aruco.GridBoard_create(nx,
ny,
marker_size,
between_markers,
aruco_dict,
firstMarker=offset)
]
print(image_path)
frame = cv2.imread(image_path)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict)
all_rvecs = []
all_tvecs = []
frame_with_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids)
# print(f"ids={ids}")
for i, board in enumerate(gridboards):
# Go through all the corners / ids found in the image, and just select the
# ones relevant to the current board
ids_subset = ids.copy()
corners_subset = corners.copy()
for j, item in enumerate(ids):
if item[0] not in [val[0] for val in board.ids]:
np.delete(ids_subset, j)
np.delete(corners_subset, j)
# Reset the vectors
rvec = np.ones(shape=(3, 1), dtype=float)
tvec = np.ones(shape=(3, 1), dtype=float)
# Estimate the Board's pose
ret, rvec, tvec = aruco.estimatePoseBoard(corners_subset, ids_subset,
board, cam_matrix,
dist_coeffs, rvec, tvec)
if ret == 0: # if no gridboard was found
print("Failed to get BoardPose")
rvecs, tvecs, objPoints = aruco.estimatePoseSingleMarkers(
corners, marker_size, cam_matrix, dist_coeffs)
for i in range(len(tvecs)):
frame_with_markers = cv2.drawFrameAxes(frame_with_markers,
cam_matrix,
dist_coeffs,
rvecs[i],
tvecs[i],
length_of_axis,
thickness=2)
else:
frame_with_markers = cv2.drawFrameAxes(frame_with_markers,
cam_matrix,
dist_coeffs,
rvec,
tvec,
length_of_axis,
thickness=2)
all_rvecs.append(rvec)
all_tvecs.append(tvec)
plt.figure(figsize=(12, 6))
plt.imshow(frame_with_markers, interpolation="nearest")
return all_rvecs, all_tvecs
contours,
-1, (0, 255, 0),
thickness=3)
#cv2.imshow("contours", contours_img)
center = np.array([
[0, 0, 0],
], dtype=np.float32)
if rvecs is not None:
imagePoints, jacobian = cv2.projectPoints(
center, rvecs, tvecs,
vision_pipeline.calibration_info.camera_matrix,
vision_pipeline.calibration_info.dist_coeffs)
imagePoints = imagePoints.reshape(-1, 2)
image = cv2.drawFrameAxes(
image, vision_pipeline.calibration_info.camera_matrix,
vision_pipeline.calibration_info.dist_coeffs, rvecs, tvecs, 1)
corner_img = cv2.circle(image,
tuple(imagePoints[0].astype(np.int32)),
3, (66, 244, 113),
thickness=3)
#if dist > 10:
cv2.imshow("corner_img", corner_img)
# print(euler_angles) g
#print("dist: {0:0.2f} | angle (degrees): {1:0.2f}".format(dist, euler_angles[1]*180/math.pi))
cv2.waitKey(1000 // 30)
def observation_callback(self, rgb_image_msg):
if self.camera_info is not None:
bgr_image = self.bridge.imgmsg_to_cv2(rgb_image_msg)
rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB)
viz_frame = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)
timer1 = cv2.getTickCount()
detections = []
if self.frame_count % self.n_frame == 0:
detections = self.face_detector.detect(rgb_image)
if self.frame_count % self.n_frame == 1:
detections = self.detector.detect(rgb_image)
self.frame_count += 1
human_detections = [
d for d in detections if d.class_label in self.body_parts
]
object_detections = [
d for d in detections if d.class_label not in self.body_parts
]
object_tracks = self.object_tracker.update(rgb_image,
object_detections)
human_tracks = self.human_tracker.update(rgb_image,
human_detections,
self.camera_matrix,
self.dist_coeffs)
fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer1)
detection_fps = "Detection and track fps : %0.4fhz" % fps
#print(detection_fps)
cv2.putText(viz_frame, detection_fps, (5, 25),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2)
for track in self.human_tracker.tracks:
#if track.is_confirmed():
tl_corner = (int(track.bbox.left()), int(track.bbox.top()))
br_corner = (int(track.bbox.right()), int(track.bbox.bottom()))
# if track.class_label == "face":
# shape = self.landmark_estimator.estimate(rgb_image, track)
# for (x, y) in shape:
# cv2.circle(viz_frame, (x, y), 1, (0, 255, 0), -1)
if track.class_label == "face":
rot = track.rotation
trans = track.translation
#rot, trans = track.get_head_pose()
if rot is not None and trans is not None:
transform = geometry_msgs.msg.TransformStamped()
transform.header.stamp = rospy.Time.now()
transform.header.frame_id = self.camera_frame_id
transform.child_frame_id = "gaze"
transform.transform.translation.x = trans[0]
transform.transform.translation.y = trans[1]
transform.transform.translation.z = trans[2]
q_rot = quaternion_from_euler(rot[0], rot[1], rot[2],
"rxyz")
transform.transform.rotation.x = q_rot[0]
transform.transform.rotation.y = q_rot[1]
transform.transform.rotation.z = q_rot[2]
transform.transform.rotation.w = q_rot[3]
self.tf_broadcaster.sendTransform(transform)
if track.uuid not in self.internal_simulator:
self.internal_simulator.load_urdf(
track.uuid, "face.urdf", trans, q_rot)
self.internal_simulator.update_entity(
track.uuid, trans, q_rot)
self.internal_simulator.get_human_visibilities(
trans, q_rot)
cv2.drawFrameAxes(viz_frame, self.camera_matrix,
self.dist_coeffs,
np.array(rot).reshape((3, 1)),
np.array(trans).reshape(3, 1), 0.03)
cv2.putText(viz_frame, track.uuid[:6],
(tl_corner[0] + 5, tl_corner[1] + 25),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (240, 0, 0), 2)
cv2.putText(viz_frame, track.class_label,
(tl_corner[0] + 5, tl_corner[1] + 45),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (240, 0, 0), 2)
cv2.rectangle(viz_frame, tl_corner, br_corner, (255, 255, 0),
2)
viz_img_msg = self.bridge.cv2_to_imgmsg(viz_frame)
self.visualization_publisher.publish(viz_img_msg)
def drawAxes(self, img, R, t):
img = cv2.drawFrameAxes(img, self.cameraMatrix,
self.distCoeefs, R, t, 30)
def draw_axes(self, img, R, t):
img = np.array(img, dtype=np.float64)
img = cv2.drawFrameAxes(img, self.camera_matrix, self.dist_coeefs, R,
t, 30)
return img
# Compute Depths
objectPose.center_glassdoor_XYZ = objectPose.depthCompute(center_glassdoor)
objectPose.center_knob_XYZ = objectPose.depthCompute(center_knob)
# To fix origin wrt rgb_optial_frame to knob center
objectPose.T_washOrigin_CRF[0,3] = objectPose.center_knob_XYZ[0][0]
objectPose.T_washOrigin_CRF[1,3] = objectPose.center_knob_XYZ[0][1]
objectPose.T_washOrigin_CRF[2,3] = objectPose.center_knob_XYZ[0][2]
# Apply transformation from depth_optical_frame to base_link
objectPose.T_wrt_mobilebase = objectPose.applyTransformation('base','camera_rgb_optical_frame',objectPose.T_washOrigin_CRF)
# Transformation from base_link to optical frame in order to show axes
objectPose.T_washOrigin_CRF = objectPose.applyTransformation('camera_rgb_optical_frame','base',objectPose.T_wrt_mobilebase)
# Show coordinate frame from result of last transformation
rot_vec,_ = cv.Rodrigues(objectPose.T_washOrigin_CRF[:3,:3])
tr_vec = objectPose.T_washOrigin_CRF[:3,3]
cv.drawFrameAxes(rgbImg, camera_info_A, camera_info_D, rot_vec, tr_vec, 0.1)
# Transformation from rgb_optical_frame to depth_optical_frame
objectPose.T_washOrigin_Depth = objectPose.applyTransformation('camera_depth_optical_frame', 'camera_rgb_optical_frame', objectPose.T_washOrigin_CRF)
# Transformation of origin from WASH MACHINE ORIGIN located in knob center to the down part of the wash machine - necessary for pcl_registration
objectPose.T_knob_wrt_machine_corner[0,3] = 0
objectPose.T_knob_wrt_machine_corner[1,3] = 0
objectPose.T_knob_wrt_machine_corner[2,3] = 0
objectPose.T_knob_wrt_machine_corner[:3,:3] = np.identity(3,dtype=np.float32)
objectPose.T_knob_wrt_machine_corner[3,:] = [0,0,0,1]
# Multiplication matrices
objectPose.T_washOrigin_Depth = np.dot(objectPose.T_knob_wrt_machine_corner,objectPose.T_washOrigin_Depth)
##### Publishers ###########################################################
# Structuring message for transformation matrix between wash machine origin and base link
