# 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,obj_coords))  # Has potential to return multiple objects, so use  element

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

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,targetX) and self._approx_equal(value,targetY)):
if value<=0:
print("ignoring depth = 0")
else:
xTotal+=value
yTotal+=value
zTotal+=value
counter = counter + 1

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


While the code performs sufficiently in close range, after a 1-2 metre distance, the plotted points strays far off from the target:

Close: 1 metre distance: I'm really stumped at how to resolve this issue so any pointers would be much appreciated.

edit retag close merge delete