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.header=msg.header
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)
transformed_point = self._transform_pose(value,"head_rgbd_sensor_rgb_frame", "map")
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.