Go to the documentation of this file.00001
00002
00003 from jsk_recognition_msgs.msg import Accuracy
00004 from jsk_recognition_msgs.msg import BoundingBox
00005 import jsk_recognition_utils
00006 from jsk_topic_tools import ConnectionBasedTransport
00007 import rospy
00008 from visualization_msgs.msg import MarkerArray
00009
00010
00011 class EvaluateVoxelSegmentationByGTBox(ConnectionBasedTransport):
00012
00013 def __init__(self):
00014 super(EvaluateVoxelSegmentationByGTBox, self).__init__()
00015 self.box_gt = None
00016 self.marker_ns = rospy.get_param('~marker_ns', None)
00017 if self.marker_ns is not None:
00018 self.marker_ns = str(self.marker_ns)
00019 self.pub = self.advertise('~output', Accuracy, queue_size=1)
00020 self.sub_box_gt = rospy.Subscriber(
00021 '~input/box_gt', BoundingBox, self._cb_box_gt)
00022
00023 def subscribe(self):
00024 self.sub_voxels = rospy.Subscriber(
00025 '~input/markers', MarkerArray, self._cb_markers)
00026
00027 def unsubscribe(self):
00028 self.sub_voxels.unregister()
00029
00030 def _cb_box_gt(self, box_msg):
00031 self.box_gt = jsk_recognition_utils.bounding_box_msg_to_aabb(box_msg)
00032
00033 def _cb_markers(self, markers_msg):
00034 if self.box_gt is None:
00035 rospy.logwarn('GT. box is not set, skipping.')
00036 return
00037
00038 v_intersect = 0
00039 v_voxels = 0
00040 for marker in markers_msg.markers:
00041 if self.marker_ns and self.marker_ns != marker.ns:
00042 continue
00043 dim_x = marker.scale.x
00044 dim_y = marker.scale.y
00045 dim_z = marker.scale.z
00046 v_voxel = dim_x * dim_y * dim_z
00047 for point in marker.points:
00048 center_x = point.x
00049 center_y = point.y
00050 center_z = point.z
00051 x1 = center_x - dim_x / 2.
00052 x2 = center_x + dim_x / 2.
00053 y1 = center_y - dim_y / 2.
00054 y2 = center_y + dim_y / 2.
00055 z1 = center_z - dim_z / 2.
00056 z2 = center_z + dim_z / 2.
00057 voxel = (x1, y1, z1, x2, y2, z2)
00058 _, v_tp, _ = jsk_recognition_utils.get_overlap_of_aabb(
00059 self.box_gt, voxel, return_volumes=True)
00060 v_intersect += v_tp
00061 v_voxels += v_voxel
00062
00063 x1, y1, z1, x2, y2, z2 = self.box_gt
00064 v_box = (x2 - x1) * (y2 - y1) * (z2 - z1)
00065
00066 v_union = v_voxels + v_box - v_intersect
00067
00068 iu = 1. * v_intersect / v_union
00069 out_msg = Accuracy(header=markers_msg.markers[0].header, accuracy=iu)
00070 self.pub.publish(out_msg)
00071
00072
00073 if __name__ == '__main__':
00074 rospy.init_node('evaluate_voxel_segmentation_by_gt_box')
00075 EvaluateVoxelSegmentationByGTBox()
00076 rospy.spin()