# point_cloud2 becomes inaccurate after a couple metres distance

I'm trying to using YOLO to identify objects from the robot's camera feed, then use point cloud data to extract the 3D world coordinates from this.

I have a node with the following code:

        model = PinholeCameraModel()
model.fromCameraInfo(self.cam_info)
xyz = model.projectPixelTo3dRay((obj_coords[0][0],obj_coords[0][1]))  # Has potential to return multiple objects, so use [0] element

stampedPoint = PointStamped()
stampedPoint.point.x=xyz[0]
stampedPoint.point.y=0
stampedPoint.point.z=xyz[1]

rospy.wait_for_service('get_3d_position')
get_3d_points =rospy.ServiceProxy('get_3d_position',LocalizePoint)
resp = get_3d_points(stampedPoint)


And the get_3d_position service uses the following function to determine the 3D position from the 2D image coordinate, using point_cloud2 data:

def _inRange(self,data,error,targetX,targetY):
xTotal = 0
yTotal =0
zTotal = 0
counter = 0
for value in data: #data = pc2.read_points(.........)
if (self._approx_equal(value[0],targetX) and self._approx_equal(value[1],targetY)):
if value[2]<=0:
print("ignoring depth = 0")
else:
xTotal+=value[0]
yTotal+=value[1]
zTotal+=value[2]
counter = counter + 1

value = [xTotal/counter,yTotal/counter,zTotal/counter]
print "x : %f  y: %f  z: %f, (sampled from %f coordinates" %(value[0],value[1],value[2],counter)