wxchen/maintain
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

modified: src/maintain/scripts/test.py

Browse Source 
modified:   src/yolov5_ros/src/yolov5/best.pt
This commit is contained in:
wxchen
2023-05-09 21:32:02 +08:00
parent f5d37a5f11
commit 4f14636768
2 changed files with 12 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/maintain/scripts/test.py
View File

@@ -23,8 +23,8 @@ bridge = CvBridge()
annulus_width = 10 annulus_width = 10
# 2d to 3d # 2d to 3d
def computer_2d_3d(x, y, depth_roi, color_intrinsics): def computer_2d_3d(x, y, depth_array, color_intrinsics):
pz = np.mean(depth_roi) * 0.001 pz = depth_array[int(y), int(x)] / 1000
px = (x - color_intrinsics[2]) * pz / color_intrinsics[0] px = (x - color_intrinsics[2]) * pz / color_intrinsics[0]
py = (y - color_intrinsics[5]) * pz / color_intrinsics[4] py = (y - color_intrinsics[5]) * pz / color_intrinsics[4]
return px, py, pz return px, py, pz
@@ -69,26 +69,24 @@ def box_callback(box, depth, color_info):
# print(screw_x,screw_y) # print(screw_x,screw_y)
depth_array = np.array(depth_image, dtype=np.float32) depth_array = np.array(depth_image, dtype=np.float32)
depth_roi = depth_array[boundingBox.ymin:boundingBox.ymax, boundingBox.xmin:boundingBox.xmax] # depth_roi = depth_array[boundingBox.ymin:boundingBox.ymax, boundingBox.xmin:boundingBox.xmax]
x, y, z = computer_2d_3d(screw_x, screw_y, depth_roi, color_intrinsics) x, y, z = computer_2d_3d(screw_x, screw_y, depth_array, color_intrinsics)
# rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z) # rospy.loginfo("screw pose: x: %f, y: %f, z: %f", x, y, z)
# calculate normal direction of screw area # calculate normal direction of screw area
p1x, p1y, p1z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymin-annulus_width, depth_roi, color_intrinsics) p1x, p1y, p1z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymin-annulus_width, depth_array, color_intrinsics)
p2x, p2y, p2z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymin-annulus_width, depth_roi, color_intrinsics) p2x, p2y, p2z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymin-annulus_width, depth_array, color_intrinsics)
p3x, p3y, p3z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymax+annulus_width, depth_roi, color_intrinsics) p3x, p3y, p3z = computer_2d_3d(boundingBox.xmax+annulus_width, boundingBox.ymax+annulus_width, depth_array, color_intrinsics)
p4x, p4y, p4z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymax+annulus_width, depth_roi, color_intrinsics) p4x, p4y, p4z = computer_2d_3d(boundingBox.xmin-annulus_width, boundingBox.ymax+annulus_width, depth_array, color_intrinsics)
p1 = [p1x, p1y, p1z] p1 = [p1x, p1y, p1z]
p2 = [p2x, p2y, p2z] p2 = [p2x, p2y, p2z]
p3 = [p3x, p3y, p3z] p3 = [p3x, p3y, p3z]
p4 = [p4x, p4y, p4z] p4 = [p4x, p4y, p4z]
normal = compute_normal_vector(p1, p2, p3, p4) normal = compute_normal_vector(p1, p2, p3, p4)
# annulus_roi = depth_array[boundingBox.ymin-annulus_width:boundingBox.ymax+annulus_width, boundingBox.xmin-annulus_width:boundingBox.xmax+annulus_width] # annulus_roi = depth_array[boundingBox.ymin-annulus_width:boundingBox.ymax+annulus_width, boundingBox.xmin-annulus_width:boundingBox.xmax+annulus_width]
# normal = calculate_image_edge_plane_normal(annulus_roi) # normal = calculate_image_edge_plane_normal(annulus_roi)
# print(normal) # print(normal)
# normal vector to quaternion # normal vector to quaternion
screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0) screw_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
screw_quat[0] = normal[0] screw_quat[0] = normal[0]
@@ -116,10 +114,10 @@ def box_callback(box, depth, color_info):
screw_tf.transform.translation.x = x screw_tf.transform.translation.x = x
screw_tf.transform.translation.y = y screw_tf.transform.translation.y = y
screw_tf.transform.translation.z = z screw_tf.transform.translation.z = z
screw_tf.transform.rotation.x = screw_quat_filtered[0] screw_tf.transform.rotation.x = screw_quat[0]
screw_tf.transform.rotation.y = screw_quat_filtered[1] screw_tf.transform.rotation.y = screw_quat[1]
screw_tf.transform.rotation.z = screw_quat_filtered[2] screw_tf.transform.rotation.z = screw_quat[2]
screw_tf.transform.rotation.w = screw_quat_filtered[3] screw_tf.transform.rotation.w = screw_quat[3]
tf_broadcaster.sendTransform(screw_tf) tf_broadcaster.sendTransform(screw_tf)

BIN
src/yolov5_ros/src/yolov5/best.pt
View File

Binary file not shown.