def _calibrate_camera_extrinsic(intrinsic_calib, image, board,
camera_local_device_name):
# TODO: verify calibration data
mtx = intrinsic_calib.K
dist_rr = intrinsic_calib.distortion_info.data
dist = np.array(
[dist_rr.k1, dist_rr.k2, dist_rr.p1, dist_rr.p2, dist_rr.k3],
dtype=np.float64)
image_util = ImageUtil()
frame = image_util.compressed_image_to_array(image)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if board == "chessboard":
width = 7
height = 6
square_size = 0.03
else:
raise RR.InvalidOperationException(
f"Invalid calibration board {board}")
ret, corners = cv2.findChessboardCorners(gray, (width, height), None)
assert ret, "Could not find calibration target"
objp = np.zeros((height * width, 3), np.float32)
objp[:, :2] = np.mgrid[0:width, 0:height].T.reshape(-1, 2)
objp = objp * square_size
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
ret, rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)
R = cv2.Rodrigues(rvecs.flatten())[0]
R_landmark = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]], dtype=np.float64)
R_cam1 = R.transpose()
p_cam1 = -R.transpose() @ tvecs
R_cam = R_landmark.transpose() @ R_cam1
p_cam = R_landmark.transpose() @ p_cam1
cv_image2 = cv2.aruco.drawAxis(frame, mtx, dist,
cv2.Rodrigues(R_cam.transpose())[0],
-R_cam.transpose() @ p_cam, 0.1)
T = rox.Transform(R_cam, p_cam, "world", camera_local_device_name)
geom_util = GeometryUtil()
cov = np.eye(6) * 1e-5
return geom_util.rox_transform_to_named_pose(
T), cov, image_util.array_to_compressed_image_jpg(cv_image2), 0.0
class VisionTemplateMatching_impl(object):
def __init__(self, device_manager, device_info = None, node: RR.RobotRaconteurNode = None):
if node is None:
self._node = RR.RobotRaconteurNode.s
else:
self._node = node
self.device_info = device_info
self.service_path = None
self.ctx = None
self._matched_template_2d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.MatchedTemplate2D")
self._template_matching_result_2d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.TemplateMatchingResult2D")
self._template_matching_result_3d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.TemplateMatchingResult3D")
self._matched_template_3d_type = self._node.GetStructureType("tech.pyri.vision.template_matching.MatchedTemplate3D")
self._named_pose_with_covariance_type = self._node.GetStructureType("com.robotraconteur.geometry.NamedPoseWithCovariance")
self._pose2d_dtype = self._node.GetNamedArrayDType("com.robotraconteur.geometry.Pose2D")
self._image_util = ImageUtil(node=self._node)
self._geom_util = GeometryUtil(node=self._node)
self.device_manager = device_manager
self.device_manager.connect_device_type("tech.pyri.variable_storage.VariableStorage")
self.device_manager.connect_device_type("com.robotraconteur.imaging.Camera")
self.device_manager.device_added += self._device_added
self.device_manager.device_removed += self._device_removed
self.device_manager.refresh_devices(5)
def RRServiceObjectInit(self, ctx, service_path):
self.service_path = service_path
self.ctx = ctx
def _device_added(self, local_device_name):
pass
def _device_removed(self, local_device_name):
pass
def match_template_stored_image(self, image_global_name, template_global_name, roi):
var_storage = self.device_manager.get_device_client("variable_storage",1)
image_var = var_storage.getf_variable_value("globals", image_global_name)
image = self._image_util.compressed_image_to_array(image_var.data)
template_var = var_storage.getf_variable_value("globals", template_global_name)
template = self._image_util.compressed_image_to_array(template_var.data)
return self._do_template_match(template, image, roi)
def match_template_camera_capture(self, camera_local_device_name, template_global_name, roi):
var_storage = self.device_manager.get_device_client("variable_storage",1)
template_var = var_storage.getf_variable_value("globals", template_global_name)
template = self._image_util.compressed_image_to_array(template_var.data)
camera_device = self.device_manager.get_device_client(camera_local_device_name)
image_compressed = camera_device.capture_frame_compressed()
image = self._image_util.compressed_image_to_array(image_compressed)
return self._do_template_match(template, image, roi)
def _do_template_match(self, template, image, roi):
matcher_roi = None
if roi is not None:
roi_x = roi.center.pose[0]["position"]["x"]
roi_y = roi.center.pose[0]["position"]["y"]
roi_theta = roi.center.pose[0]["orientation"]
roi_w = roi.size[0]["width"]
roi_h = roi.size[0]["height"]
matcher_roi = (roi_x, roi_y, roi_w, roi_h, -roi_theta)
# execute the image detection using opencv
matcher = TemplateMatchingMultiAngleWithROI(template,image,matcher_roi)
# return_result_image = True
match_center, template_wh, match_angle, detection_result_img = matcher.detect_object(True)
# return the pose of the object
print("the object is found..:")
print("center coordinates in img frame(x,y): " + str(match_center))
print("(w,h): " + str(template_wh))
print("angle: " + str(match_angle))
match_result = self._template_matching_result_2d_type()
matched_template_result = self._matched_template_2d_type()
centroid = np.zeros((1,),dtype=self._pose2d_dtype)
centroid[0]["position"]["x"] = match_center[0]
centroid[0]["position"]["y"] = match_center[1]
centroid[0]["orientation"] = match_angle
matched_template_result.match_centroid = centroid
matched_template_result.template_size = self._geom_util.wh_to_size2d(template_wh,dtype=np.int32)
matched_template_result.confidence = 0
match_result.template_matches =[matched_template_result]
match_result.display_image = self._image_util.array_to_compressed_image_jpg(detection_result_img, 70)
return match_result
def _do_match_with_pose(self,image, template, intrinsic_calib, extrinsic_calib, object_z, roi):
matcher_roi = None
if roi is not None:
roi_x = roi.center.pose[0]["position"]["x"]
roi_y = roi.center.pose[0]["position"]["y"]
roi_theta = roi.center.pose[0]["orientation"]
roi_w = roi.size[0]["width"]
roi_h = roi.size[0]["height"]
matcher_roi = (roi_x, roi_y, roi_w, roi_h, -roi_theta)
# execute the image detection using opencv
matcher = TemplateMatchingMultiAngleWithROI(template,image,matcher_roi)
# return_result_image = True
match_center, template_wh, match_angle, detection_result_img = matcher.detect_object(True)
# detection_result.width
# detection_result.height
x = match_center[0]
y = match_center[1]
theta = match_angle
src = np.asarray([x,y], dtype=np.float32)
src = np.reshape(src,(-1,1,2)) # Rehsape as opencv requires (N,1,2)
# print(src)
# Now the detection results in image frame is found,
# Hence, find the detected pose in camera frame with given z distance to camera
# To do that first we need to get the camera parameters
# Load the camera matrix and distortion coefficients from the calibration result.
mtx = intrinsic_calib.K
d = intrinsic_calib.distortion_info.data
dist = np.array([d.k1,d.k2,d.p1,d.p2,d.k3])
T_cam = self._geom_util.named_pose_to_rox_transform(extrinsic_calib.pose)
#TODO: Figure out a better value for this
object_z_cam_dist = abs(T_cam.p[2]) - object_z
# Find the corresponding world pose of the detected pose in camera frame
dst = cv2.undistortPoints(src,mtx,dist) # dst is Xc/Zc and Yc/Zc in the same shape of src
dst = dst * float(object_z_cam_dist) * 1000.0 # Multiply by given Zc distance to find all cordinates, multiply by 1000 is because of Zc is given in meters but others are in millimeters
dst = np.squeeze(dst) * 0.001 # Xc and Yc as vector
# Finally the translation between the detected object center and the camera frame represented in camera frame is T = [Xc,Yc,Zc]
Xc = dst[0]
Yc = dst[1]
Zc = float(object_z_cam_dist)
T = np.asarray([Xc,Yc,Zc])
# Now lets find the orientation of the detected object with respect to camera
# We are assuming +z axis is looking towards the camera and xy axes of the both object and camera are parallel planes
# So the rotation matrix would be
theta = np.deg2rad(theta) #convert theta from degrees to radian
R_co = np.asarray([[math.cos(theta),-math.sin(theta),0],[-math.sin(theta),-math.cos(theta),0],[0,0,-1]])
T_obj_cam_frame = rox.Transform(R_co, T, "camera", "object")
T_obj = T_cam * T_obj_cam_frame
# TODO: Better adjustment of Z height?
T_obj.p[2] = object_z
ret1 = self._matched_template_3d_type()
ret1.pose = self._named_pose_with_covariance_type()
ret1.pose.pose = self._geom_util.rox_transform_to_named_pose(T_obj)
ret1.pose.covariance= np.zeros((6,6))
ret1.confidence = 0
ret = self._template_matching_result_3d_type()
ret.template_matches = [ret1]
ret.display_image = self._image_util.array_to_compressed_image_jpg(detection_result_img,70)
return ret
def _do_match_template_world_pose(self, image, template_global_name,
camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi, var_storage):
template_var = var_storage.getf_variable_value("globals", template_global_name)
template = self._image_util.compressed_image_to_array(template_var.data)
intrinsic_calib = var_storage.getf_variable_value("globals", camera_calibration_intrinsic).data
extrinsic_calib = var_storage.getf_variable_value("globals", camera_calibration_extrinsic).data
return self._do_match_with_pose(image,template,intrinsic_calib,extrinsic_calib,object_z_height,roi)
def match_template_world_pose_stored_image(self, image_global_name, template_global_name,
camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi):
var_storage = self.device_manager.get_device_client("variable_storage",1)
image_var = var_storage.getf_variable_value("globals", image_global_name)
image = self._image_util.compressed_image_to_array(image_var.data)
return self._do_match_template_world_pose(image, template_global_name, camera_calibration_intrinsic,
camera_calibration_extrinsic, object_z_height, roi, var_storage)
def match_template_world_pose_camera_capture(self, camera_local_device_name, template_global_name,
camera_calibration_intrinsic, camera_calibration_extrinsic, object_z_height, roi):
var_storage = self.device_manager.get_device_client("variable_storage",1)
camera_device = self.device_manager.get_device_client(camera_local_device_name,1)
img_rr = camera_device.capture_frame_compressed()
image = self._image_util.compressed_image_to_array(img_rr)
return self._do_match_template_world_pose(image, template_global_name, camera_calibration_intrinsic,
camera_calibration_extrinsic, object_z_height, roi, var_storage)
def calibrate(images, joint_poses, aruco_dict, aruco_id, aruco_markersize,
flange_to_marker, mtx, dist, cam_pose, rox_robot,
robot_local_device_name):
""" Apply extrinsic camera calibration operation for images in the given directory path
using opencv aruco marker detection, the extrinsic marker poses given in a json file,
and the given intrinsic camera parameters."""
assert aruco_dict.startswith("DICT_"), "Invalid aruco dictionary name"
aruco_dict = getattr(aruco, aruco_dict) # convert string to python
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict)
aruco_params = cv2.aruco.DetectorParameters_create()
i = 0
imgs_out = []
geom_util = GeometryUtil()
image_util = ImageUtil()
object_points = []
image_points = []
for img, joints in zip(images, joint_poses):
# Find the aruco tag corners
# corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=aruco_params,cameraMatrix=mtx, distCoeff=dist)
corners, ids, rejected = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=aruco_params)
# #debug
# print(str(type(corners))) # <class 'list'>
# print(str(corners)) # list of numpy arrays of corners
# print(str(type(ids))) # <class 'numpy.ndarray'>
# print(str(ids))
if len(corners) > 0:
# Only find the id that we desire
indexes = np.flatnonzero(ids.flatten() == aruco_id).tolist()
corners = [corners[index] for index in indexes]
ids = np.asarray([ids[index] for index in indexes])
# #debug
# print(str(type(corners))) # <class 'list'>
# print(str(corners)) # list of numpy arrays of corners
# print(str(type(ids))) # <class 'numpy.ndarray'>
# print(str(ids))
if len(ids) > 0: # if there exist at least one id that we desire
# Select the first detected one, discard the others
corners = corners[0] # now corners is 1 by 4
# # extract the marker corners (which are always returned
# # in top-left, top-right, bottom-right, and bottom-left
# # order)
# corners = corners.reshape((4, 2))
# (topLeft, topRight, bottomRight, bottomLeft) = corners
# Estimate the pose of the detected marker in camera frame
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(
corners, aruco_markersize, mtx, dist)
# # Debug: Show the detected tag and axis in the image
# # # cv2.aruco.drawDetectedMarkers(img, corners) # Draw A square around the markers (Does not work)
img1 = img.copy()
img_out = cv2.aruco.drawAxis(img1, mtx, dist, rvec, tvec,
aruco_markersize *
0.75) # Draw Axis
imgs_out.append(img_out)
transform_base_2_flange = rox.fwdkin(rox_robot, joints)
transform_flange_2_marker = geom_util.pose_to_rox_transform(
flange_to_marker)
transform_base_2_marker = transform_base_2_flange * transform_flange_2_marker
transform_base_2_marker_corners = _marker_corner_poses(
transform_base_2_marker, aruco_markersize)
# Structure of this disctionary is "filename":[[R_base2marker],[T_base2marker],[R_cam2marker],[T_cam2marker]]
for j in range(4):
object_points.append(transform_base_2_marker_corners[j].p)
image_points.append(corners[0, j])
#pose_pairs_dict[i] = (transform_base_2_marker_corners, corners)
i += 1
object_points_np = np.array(object_points, dtype=np.float64)
image_points_np = np.array(image_points, dtype=np.float32)
# Finally execute the calibration
retval, rvec, tvec = cv2.solvePnP(object_points_np, image_points_np, mtx,
dist)
R_cam2base = cv2.Rodrigues(rvec)[0]
T_cam2base = tvec
# Add another display image of marker at robot base
img_out = cv2.aruco.drawAxis(img, mtx, dist,
cv2.Rodrigues(R_cam2base)[0], T_cam2base,
aruco_markersize * 0.75) # Draw Axis
imgs_out.append(img_out)
rox_transform_cam2base = rox.Transform(R_cam2base, T_cam2base,
cam_pose.parent_frame_id,
robot_local_device_name)
rox_transform_world2base = cam_pose * rox_transform_cam2base
#R_base2cam = R_cam2base.T
#T_base2cam = - R_base2cam @ T_cam2base
R_base2cam = rox_transform_world2base.inv().R
T_base2cam = rox_transform_world2base.inv().p
#debug
print("FINAL RESULTS: ")
print("str(R_cam2base)")
# print(str(type(R_cam2base)))
print(str(R_cam2base))
print("str(T_cam2base)")
# print(str(type(T_cam2base.flatten())))
print(str(T_cam2base))
print("str(R_base2cam)")
# print(str(type(R_base2cam)))
print(str(R_base2cam))
print("str(T_base2cam)")
# print(str(type(T_base2cam.flatten())))
print(str(T_base2cam))
pose_res = geom_util.rox_transform_to_named_pose(rox_transform_world2base)
cov = np.eye(6) * 1e-5
imgs_out2 = [
image_util.array_to_compressed_image_jpg(i, 70) for i in imgs_out
]
return pose_res, cov, imgs_out2, 0.0
class VisionArucoDetection_impl(object):
def __init__(self,
device_manager,
device_info=None,
node: RR.RobotRaconteurNode = None):
if node is None:
self._node = RR.RobotRaconteurNode.s
else:
self._node = node
self.device_info = device_info
self.service_path = None
self.ctx = None
self._detected_marker = self._node.GetStructureType(
"tech.pyri.vision.aruco_detection.DetectedMarker")
self._aruco_detection_result = self._node.GetStructureType(
"tech.pyri.vision.aruco_detection.ArucoDetectionResult")
self._pose2d_dtype = self._node.GetNamedArrayDType(
"com.robotraconteur.geometry.Pose2D")
self._point2d_dtype = self._node.GetNamedArrayDType(
"com.robotraconteur.geometry.Point2D")
self._image_util = ImageUtil(node=self._node)
self._geom_util = GeometryUtil(node=self._node)
self.device_manager = device_manager
self.device_manager.connect_device_type(
"tech.pyri.variable_storage.VariableStorage")
self.device_manager.connect_device_type(
"com.robotraconteur.imaging.Camera")
self.device_manager.device_added += self._device_added
self.device_manager.device_removed += self._device_removed
self.device_manager.refresh_devices(5)
def RRServiceObjectInit(self, ctx, service_path):
self.service_path = service_path
self.ctx = ctx
def _device_added(self, local_device_name):
pass
def _device_removed(self, local_device_name):
pass
def _do_aruco_detection(self, img, intrinsic_global_name,
extrinsic_global_name, aruco_dict_str, aruco_id,
aruco_markersize, roi):
intrinsic_calib = None
extrinsic_calib = None
if intrinsic_global_name is not None and extrinsic_global_name is not None:
var_storage = self.device_manager.get_device_client(
"variable_storage", 0.1)
intrinsic_calib = var_storage.getf_variable_value(
"globals", intrinsic_global_name).data
extrinsic_calib = var_storage.getf_variable_value(
"globals", extrinsic_global_name).data
display_img = img.copy()
assert aruco_dict_str.startswith(
"DICT_"), "Invalid aruco dictionary name"
aruco_dict_i = getattr(aruco,
aruco_dict_str) # convert string to python
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_i)
aruco_params = cv2.aruco.DetectorParameters_create()
aruco_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
corners1, ids1, rejected = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=aruco_params)
if corners1 is None:
corners1 = []
if ids1 is None:
ids1 = []
if aruco_id < 0:
corners2 = corners1
ids2 = ids1
else:
corners2 = []
ids2 = []
for id2, corner2 in zip(ids1, corners1):
if id2 == aruco_id:
corners2.append(corner2)
ids2.append(id2)
ids2 = np.array(ids2)
if roi is None:
corners = corners2
ids = ids2
else:
roi_x = roi.center.pose[0]["position"]["x"]
roi_y = roi.center.pose[0]["position"]["y"]
roi_theta = roi.center.pose[0]["orientation"]
roi_w = roi.size[0]["width"]
roi_h = roi.size[0]["height"]
geom_roi1 = shapely.geometry.box(-roi_w / 2.,
-roi_h / 2.,
roi_w / 2.,
roi_h / 2.,
ccw=True)
geom_roi2 = shapely.affinity.translate(geom_roi1,
xoff=roi_x,
yoff=roi_y)
geom_roi = shapely.affinity.rotate(geom_roi2,
roi_theta,
origin='centroid',
use_radians=True)
corners = []
ids = []
for id3, corner3 in zip(ids2, corners2):
centroid = shapely.geometry.Polygon([
shapely.geometry.Point(corner3[0, i, 0], corner3[0, i, 1])
for i in range(4)
])
if geom_roi.contains(centroid):
corners.append(corner3)
ids.append(id3)
ids = np.array(ids)
roi_outline = np.array([geom_roi.exterior.coords], dtype=np.int32)
display_img = cv2.polylines(display_img,
roi_outline,
True,
color=(255, 255, 0))
if len(ids) > 0:
display_img = aruco.drawDetectedMarkers(display_img, corners, ids)
poses = None
if intrinsic_calib is not None and extrinsic_calib is not None:
poses = []
mtx = intrinsic_calib.K
d = intrinsic_calib.distortion_info.data
dist = np.array([d.k1, d.k2, d.p1, d.p2, d.k3])
T_cam = self._geom_util.named_pose_to_rox_transform(
extrinsic_calib.pose)
for id4, corner4 in zip(ids, corners):
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(
corner4, aruco_markersize, mtx, dist)
display_img = cv2.aruco.drawAxis(display_img, mtx, dist, rvec,
tvec, 0.05)
# R_marker1 = cv2.Rodrigues(rvec.flatten())[0]
# TODO: 3D pose estimation from rvec is very innaccurate. Use a simple trigonometry
# to estimate the Z rotation of the tag
# compute vectors from opposite corners
v1 = corner4[0, 2, :] - corner4[0, 0, :]
v2 = corner4[0, 3, :] - corner4[0, 1, :]
# Use atan2 on each vector and average
theta1 = (np.arctan2(v1[1], v1[0]) - np.deg2rad(45)) % (2. *
np.pi)
theta2 = (np.arctan2(v2[1], v2[0]) - np.deg2rad(135)) % (2. *
np.pi)
theta = (theta1 + theta2) / 2.
R_marker1 = rox.rot([1., 0., 0.], np.pi) @ rox.rot(
[0., 0., -1.], theta)
p_marker1 = tvec.flatten()
T_marker1 = rox.Transform(R_marker1, p_marker1, "camera",
f"aruco{int(id4)}")
T_marker = T_cam * T_marker1
rr_marker_pose = self._geom_util.rox_transform_to_named_pose(
T_marker)
poses.append(rr_marker_pose)
ret_markers = []
if poses is None:
poses = [None] * len(ids)
for id_, corner, pose in zip(ids, corners, poses):
m = self._detected_marker()
m.marker_id = int(id_)
m.marker_corners = np.zeros((4, ), dtype=self._point2d_dtype)
for i in range(4):
m.marker_corners[i] = self._geom_util.xy_to_point2d(
corner[0, i, :])
m.marker_pose = pose
ret_markers.append(m)
ret = self._aruco_detection_result()
ret.detected_markers = ret_markers
ret.display_image = self._image_util.array_to_compressed_image_jpg(
display_img, 70)
return ret
def detect_aruco_stored_image(self, image_global_name,
camera_calibration_intrinsic,
camera_calibration_extrinsic, aruco_dict,
aruco_id, aruco_markersize, roi):
if len(camera_calibration_intrinsic) == 0:
camera_calibration_intrinsic = None
if len(camera_calibration_extrinsic) == 0:
camera_calibration_extrinsic = None
var_storage = self.device_manager.get_device_client(
"variable_storage", 0.1)
img_rr = var_storage.getf_variable_value("globals", image_global_name)
img = self._image_util.compressed_image_to_array(img_rr.data)
return self._do_aruco_detection(img, camera_calibration_intrinsic,
camera_calibration_extrinsic,
aruco_dict, aruco_id, aruco_markersize,
roi)
def detect_aruco_camera_capture(self, camera_local_device_name,
camera_calibration_intrinsic,
camera_calibration_extrinsic, aruco_dict,
aruco_id, aruco_markersize, roi):
if len(camera_calibration_intrinsic) == 0:
camera_calibration_intrinsic = None
if len(camera_calibration_extrinsic) == 0:
camera_calibration_extrinsic = None
camera_device = self.device_manager.get_device_client(
camera_local_device_name, 1)
img_rr = camera_device.capture_frame_compressed()
img = self._image_util.compressed_image_to_array(img_rr)
return self._do_aruco_detection(img, camera_calibration_intrinsic,
camera_calibration_extrinsic,
aruco_dict, aruco_id, aruco_markersize,
roi)