point_cloud2 becomes inaccurate after a couple metres distance

asked 2021-03-25 12:18:06 -0500

chulme99 gravatar image

updated 2021-03-26 03:35:36 -0500

Ranjit Kathiriya gravatar image

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:

image description

1 metre distance:

image description

I'm really stumped at how to resolve this issue so any pointers would be much appreciated.

edit retag flag offensive close merge delete