3 from jsk_recognition_msgs.msg
import Accuracy
4 from jsk_recognition_msgs.msg
import BoundingBox
5 import jsk_recognition_utils
6 from jsk_topic_tools
import ConnectionBasedTransport
8 from visualization_msgs.msg
import MarkerArray
14 super(EvaluateVoxelSegmentationByGTBox, self).
__init__()
16 self.
marker_ns = rospy.get_param(
'~marker_ns',
None)
19 self.
pub = self.advertise(
'~output', Accuracy, queue_size=1)
28 self.sub_voxels.unregister()
35 rospy.logwarn(
'GT. box is not set, skipping.')
40 for marker
in markers_msg.markers:
43 dim_x = marker.scale.x
44 dim_y = marker.scale.y
45 dim_z = marker.scale.z
46 v_voxel = dim_x * dim_y * dim_z
47 for point
in marker.points:
51 x1 = center_x - dim_x / 2.
52 x2 = center_x + dim_x / 2.
53 y1 = center_y - dim_y / 2.
54 y2 = center_y + dim_y / 2.
55 z1 = center_z - dim_z / 2.
56 z2 = center_z + dim_z / 2.
57 voxel = (x1, y1, z1, x2, y2, z2)
59 self.
box_gt, voxel, return_volumes=
True)
63 x1, y1, z1, x2, y2, z2 = self.
box_gt 64 v_box = (x2 - x1) * (y2 - y1) * (z2 - z1)
66 v_union = v_voxels + v_box - v_intersect
68 iu = 1. * v_intersect / v_union
69 out_msg = Accuracy(header=markers_msg.markers[0].header, accuracy=iu)
70 self.pub.publish(out_msg)
73 if __name__ ==
'__main__':
74 rospy.init_node(
'evaluate_voxel_segmentation_by_gt_box')
def _cb_box_gt(self, box_msg)
def _cb_markers(self, markers_msg)