Go to the documentation of this file.00001
00002 import rospy
00003 import tf
00004
00005 from image_geometry import PinholeCameraModel
00006 from geometry_msgs.msg import TransformStamped, PointStamped
00007 from cmvision.msg import Blobs, Blob
00008 from cmvision_3d.msg import Blob3d
00009
00010 import numpy as np
00011 from math import isnan
00012
00013
00014
00015 class color_model():
00016 def __init__(self, blob, camera_info, parent_frame, depth_image, cam_model, listener, broadcaster):
00017
00018
00019 self.blob = blob
00020
00021
00022 self.parent_frame = parent_frame
00023
00024
00025 self.depth_image = depth_image
00026
00027
00028 self.camera_frame = camera_info.header.frame_id
00029
00030
00031 self.cam_model = cam_model
00032
00033
00034 self.listener = listener
00035
00036
00037 self.broadcaster = broadcaster
00038
00039
00040 def validate(self):
00041 return ( self._validateDepthAt(self.blob.x, self.blob.y) and
00042 self._validateDepthAt(self.blob.left, self.blob.top) and
00043 self._validateDepthAt(self.blob.right, self.blob.bottom) )
00044
00045 return False
00046
00047
00048 def publish(self):
00049 transform = self._toTransform(self.blob.x, self.blob.y)
00050 pos = (transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z)
00051 rot = (transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w)
00052
00053 self.broadcaster.sendTransform(pos, rot, rospy.Time.now(), transform.child_frame_id, transform.header.frame_id)
00054
00055 return transform
00056 def toBlob3d(self):
00057 blob3d = Blob3d()
00058
00059 blob3d.name = self.blob.name
00060 blob3d.red = self.blob.red
00061 blob3d.green = self.blob.green
00062 blob3d.blue = self.blob.blue
00063 blob3d.area = self.blob.area
00064
00065 transform = self._toTransform(self.blob.x, self.blob.y)
00066 blob3d.center.x = transform.transform.translation.x
00067 blob3d.center.y = transform.transform.translation.y
00068 blob3d.center.z = transform.transform.translation.z
00069
00070 transform = self._toTransform(self.blob.left, self.blob.top)
00071 blob3d.top_left.x = transform.transform.translation.x
00072 blob3d.top_left.y = transform.transform.translation.y
00073 blob3d.top_left.z = transform.transform.translation.z
00074
00075 transform = self._toTransform(self.blob.right, self.blob.bottom)
00076 blob3d.bottom_right.x = transform.transform.translation.x
00077 blob3d.bottom_right.y = transform.transform.translation.y
00078 blob3d.bottom_right.z = transform.transform.translation.z
00079
00080 return blob3d
00081
00082
00083
00084
00085
00086 def _toTransform(self, my_x, my_y):
00087 transform = TransformStamped()
00088 transform.header.stamp = rospy.Time.now()
00089 transform.header.frame_id = self.camera_frame
00090 transform.child_frame_id = self.blob.name
00091
00092 (x,y,z) = self._projectTo3d(my_x, my_y)
00093 transform.transform.translation.x = x
00094 transform.transform.translation.y = y
00095 transform.transform.translation.z = z
00096
00097 transform.transform.rotation.w = 1.0
00098
00099
00100 if self.parent_frame != self.camera_frame:
00101 point = PointStamped()
00102 point.header.frame_id = self.camera_frame
00103 point.header.stamp = rospy.Time(0)
00104 point.point.x = transform.transform.translation.x
00105 point.point.y = transform.transform.translation.y
00106 point.point.z = transform.transform.translation.z
00107
00108
00109 point_transformed = self.listener.transformPoint(self.parent_frame, point)
00110
00111 transform.header.frame_id = self.parent_frame
00112 transform.transform.translation.x = point_transformed.point.x
00113 transform.transform.translation.y = point_transformed.point.y
00114 transform.transform.translation.z = point_transformed.point.z
00115
00116 return transform
00117
00118 def _projectTo3d(self, x, y):
00119 [vx,vy,vz] = self.cam_model.projectPixelTo3dRay((x,y))
00120 blob_z = self._getDepthAt(x,y)
00121 blob_x = vx * blob_z
00122 blob_y = vy * blob_z
00123
00124 return (blob_x, blob_y, blob_z)
00125
00126 def _getDepthAt(self, x,y):
00127 return self.depth_image[y][x]/1000
00128
00129 def _validateDepthAt(self, x, y):
00130 depth = self._getDepthAt(x, y)
00131 if isnan(depth) or depth == 0:
00132 return False
00133 if self.blob.area == 0:
00134 return False
00135 return True
00136