Projecting 3D points into pixel using image_geometry::Pinholecameramodel

edit

Currently, I have this set of problems where I am trying to simulate bounding boxes in the simulation, and the general idea that I have drawn out is basically:

1. Subscribe to ground truth which helps me to detect the object I am trying to find its bounding boxes inside gazebo
2. Currently, the position of the object as subscribed from the ground truth is with respect to the base_link. So, I want to find the transform between the base_link and the camera_link, which I could simply use a lookupTransform to find its translation.
3. Then, I would have to do some maths so that the position of the object is now with respect to the camera_link
4. From here, I am able to determine the object's 3d position with respect to the camera (camera is now the origin)
5. I could then project the 3d coordinates into pixel simply by using image_geometry::PinholeCameraModel::project3dToPixel

However, the problem that I am facing is that the projection from 3d coordinates into pixel coordinate is way off from the expected position (as in it is not in the frame of my camera altho the cone is right there). I cant seem to find where I did it wrongly. Here is a really simple code for me to test in my simulation

import rclpy
import time
import cv2

from cv_bridge import CvBridge, CvBridgeError
from rclpy.qos import qos_profile_sensor_data

from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from image_geometry import PinholeCameraModel

class BoundingBox(Node):

     def __init__(self):
        super().__init__('boundary_box_node')

        self.model = PinholeCameraModel()

        # self.cone_position = (6.029119415283203, 2.77589750289917, 0.76)
        self.cone_position = (12.79, 3.012697458267212, 0.76)
        # self.cone_position = (0,0,1)
        self.need_info = True
        self.cam_info = self.create_subscription(CameraInfo, "zed/right/camera_info", self.info_callback, 
                              qos_profile_sensor_data)


        # while self.need_info:
        #      time.sleep(0.01)

        self.get_logger().info(f'Got info from right!')

        self.bridge = CvBridge()
        self.sub_image = self.create_subscription(Image, 'zed/right/image_rect_color', self.image_callback, 
                                  qos_profile_sensor_data)
        # self.pub_image = self.create_publisher('/image_out', Image, 10)

    def info_callback(self, info):

        if self.need_info:
            self.get_logger().info(f'Inside info callback')
            self.model.fromCameraInfo(info)
            self.need_info = False

    def image_callback(self, image):

        translated_cone_pixel = []

        u, v = self.model.project3dToPixel(self.cone_position)

        trannslated_cone_pixel.append((int(u), int(v)))

        try:
            cv_image = self.bridge.imgmsg_to_cv2(image, desired_encoding="bgr8")
            cv2.circle(cv_image, translated_cone_pixel[0], 10, (255,0,0))
           #cv2.rectangle(cv_image, corners[0],corners[1],(0,255,0),3)

            cv2.imshow('test', cv_image)
            cv2.waitKey(1)

        except CvBridgeError as e:
            print(e)


def main(args=None):
    rclpy.init(args=args)

    bounding_box = BoundingBox()

    try:
        while rclpy.ok():
            rclpy.spin_once(bounding_box)

    except KeyboardInterrupt:
        pass

    bounding_box.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
     main()

In this code, I have extracted the cone's position myself and transform it manually, and I was just testing out only for the 3d projection part!

Note: For confidential reason, I had to omit my model in the first image, and change it to a cube.

The position of the cube inside gazebo is (-12.3647,10.2,0) and the cone ... (more)