Install libopencv on Starling 2

Judoor 0 · 25 Jul 2024, 08:32

@Alex-Kushleyev @wilkinsaf Thanks for your answers! Actually, I've done something similar with docker, but I still have a few problems with the data format that mpa_to_ros sends. To address this, I did the following:

#!/usr/bin/env python
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import yaml

class CamImageConverter:
    def __init__(self):
        self.bridge = CvBridge()
        
        # Subscribers
        rospy.Subscriber("/drone/tof_depth/image_raw", Image, self.depth_callback)
        rospy.Subscriber("/drone/hires/image_raw", Image, self.rgb_callback)
        rospy.Subscriber("/drone/tracking_front/image_raw", Image, self.tracking_callback)
        
        # Publishers
        self.depth_pub = rospy.Publisher("/drone/tof_depth/image", Image, queue_size=1000)
        
        self.rgb_pub = rospy.Publisher("/drone/hires/image", Image, queue_size=1000)
        self.rgb_info_pub = rospy.Publisher("/drone/hires/camera_info", CameraInfo, queue_size=1000)
        self.tracking_pub = rospy.Publisher("/drone/tracking_front/image", Image, queue_size=1000)
        self.tracking_info_pub = rospy.Publisher("/drone/tracking_front/camera_info", CameraInfo, queue_size=1000)

        self.camera_info_msg = self.load_camera_info("/root/ros_ws/src/cam_converter/calibration/ost.yaml")

        rospy.Timer(rospy.Duration(1.0 / 30), self.publish_tracking_info)
        rospy.Timer(rospy.Duration(1.0 / 30), self.publish_rgb_info)

    def load_camera_info(self, filename):
        with open(filename, "r") as file:
            calib_data = yaml.safe_load(file)
            camera_info_msg = CameraInfo()
            camera_info_msg.height = calib_data["image_height"]
            camera_info_msg.width = calib_data["image_width"]
            camera_info_msg.distortion_model = calib_data["distortion_model"]
            camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
            camera_info_msg.K = calib_data["camera_matrix"]["data"]
            camera_info_msg.R = calib_data["rectification_matrix"]["data"]
            camera_info_msg.P = calib_data["projection_matrix"]["data"]
            return camera_info_msg

    def depth_callback(self, msg):
        try:
            # Convertir le message ROS en une image OpenCV
            raw_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="mono8")

            # Normalisation de l'image pour améliorer la qualité
            depth_image = cv2.normalize(raw_image, None, 0, 65535, cv2.NORM_MINMAX).astype(np.uint16)

            # Publier l'image convertie
            converted_msg = self.bridge.cv2_to_imgmsg(depth_image, encoding="16UC1")
            converted_msg.header.frame_id = 'tof_link'
            converted_msg.header.stamp = rospy.Time.now()
            self.depth_pub.publish(converted_msg)

        except CvBridgeError as e:
            rospy.logerr("CvBridge Error: {0}".format(e))

    def rgb_callback(self, msg):
        try:
            # Convertir le message ROS en une image OpenCV
            raw_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")

            # Conversion YUV422 ou NV12 en RGB8
            if msg.encoding == "yuv422":
                rgb_image = cv2.cvtColor(raw_image, cv2.COLOR_YUV2RGB_Y422)
            elif msg.encoding == "nv12":
                yuv_image = np.frombuffer(msg.data, dtype=np.uint8).reshape((msg.height * 3 // 2, msg.width))
                rgb_image = cv2.cvtColor(yuv_image, cv2.COLOR_YUV2RGB_NV12)
            else:
                rospy.logerr("Unsupported encoding: {}".format(msg.encoding))
                return

            # Redimensionner l'image RGB pour qu'elle ait la même taille que l'image ToF (240x180)
            resized_rgb_image = cv2.resize(rgb_image, (180, 240), interpolation=cv2.INTER_LINEAR)

            # Publier l'image convertie et redimensionnée
            converted_msg = self.bridge.cv2_to_imgmsg(resized_rgb_image, encoding="rgb8")
            converted_msg.header.frame_id = 'hires_link'
            converted_msg.header.stamp = rospy.Time.now()
            self.rgb_pub.publish(converted_msg)

        except CvBridgeError as e:
            rospy.logerr("CvBridge Error: {0}".format(e))

    def tracking_callback(self, msg):
        try:
            # Convertir le message ROS en une image OpenCV
            raw_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")

            # Redimensionner l'image RGB pour qu'elle ait la même taille que l'image ToF (240x180)
            resized_image = cv2.resize(raw_image, (180, 240), interpolation=cv2.INTER_LINEAR)

            # Publier l'image convertie et redimensionnée
            converted_msg = self.bridge.cv2_to_imgmsg(resized_image)
            converted_msg.header.frame_id = 'tracking_front_link'
            converted_msg.header.stamp = rospy.Time.now()
            self.tracking_pub.publish(converted_msg)

        except CvBridgeError as e:
            rospy.logerr("CvBridge Error: {0}".format(e))

    def publish_rgb_info(self, event):
        self.camera_info_msg.header.stamp = rospy.Time.now()
        self.camera_info_msg.header.frame_id = 'hires_link'
        self.rgb_info_pub.publish(self.camera_info_msg)

    def publish_tracking_info(self, event):
        camera_info_msg = CameraInfo()
        camera_info_msg.header.frame_id = 'tracking_front_link'
        camera_info_msg.header.stamp = rospy.Time.now()

        camera_info_msg.height = 240
        camera_info_msg.width = 180
        camera_info_msg.distortion_model = 'plumb_bob'
        camera_info_msg.D = [5.0234834483837899e-02, 3.2967878240805777e-02, -1.6468916528340826e-02, 1.8155367951008903e-03]
        camera_info_msg.K = [4.6177826259848081e+02, 0., 6.1505488982380564e+02, 0., 4.6160870283061041e+02, 4.1065345867690684e+02, 0., 0., 1.]
        camera_info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
        camera_info_msg.P = [4.6177826259848081e+02, 0., 6.1505488982380564e+02, 0., 0., 4.6160870283061041e+02, 4.1065345867690684e+02, 0., 0., 0., 1., 0.]

        self.tracking_info_pub.publish(camera_info_msg)
   
    
def main(): 
    rospy.init_node('cam_image_converter', anonymous=True)
    CamImageConverter()
    rospy.spin()

if __name__ == '__main__':
    main()

I don't know if this is the right way to do it, but it seems to work. Do you have any suggestions for implementing this directly on the Voxl2?

Also, I have a few problems with camera calibrations and the drone model. Do you have any files that contain all the data for the cameras (ToF, tracking, and high-resolution) and a model of the drone that I can use in RViz and Gazebo? This could really help me.

For more context, I'm trying to implement this project for Autonomous SLAM with ROS. Has anyone already used RTAB-Map on this drone? Do you have any advice to solve this problem?

Thank you for your help

wilkinsaf · 26 Jul 2024, 15:11

@Judoor-0

I do not have an suggestions for implementing this directly on voxl2. as mentioned the voxl-opencv might conflict with standard opencv. I have also seen other packages conflict and python versions conflict depending on the ROS version you are using.
Far easier and cleaner to use a docker image, and creates less headache in the long run.

when you say data for cameras, what do you mean? I do not believe Modal has a DIRECT model of the drone in RViz or Gazebo. But you can follow this guide to get HITL working in Gazebo with the voxl2 hardware: https://docs.modalai.com/voxl2-PX4-hitl/

wilkinsaf · 26 Jul 2024, 15:11

and video: https://www.youtube.com/watch?v=ysvpJdXFWaM

Judoor 0 · 29 Jul 2024, 08:15

@wilkinsaf Okay, thanks for your reply. Sorry, that was not clear. What I mean by data for cameras is data that I can send to RTAB-Map as a sensor_msgs/CameraInfo message. There is some data in the extrinsics files for tracking, but not for other sensors.

wilkinsaf · 29 Jul 2024, 15:19

@Judoor-0 Gotcha. Let me know if this doesnt answer your question.

you can use voxl mpa to ros2: https://gitlab.com/voxl-public/voxl-sdk/utilities/voxl-mpa-to-ros2
and this will allow you to access camera frame information

Judoor 0 · 30 Jul 2024, 07:26

@wilkinsaf Does mpa to ros do the same thing? Because I currently use ROS1. And for ROS1, mpa to ros only sends camera images or point cloud, not camera_infos.

Prabhav Gupta · 30 Jul 2024, 20:10

@Judoor-0 @wilkinsaf

I am working on something very similar as well and am facing the same issue.

I need to run RTAB-MAP for SLAM using the drone's sensors (Video and IMU). But RTAB-MAP expects camera_info (the camera matrix and other variables, as explained by @Judoor-0 above) along with video data.

One way to provide this information to RTAB-MAP is by creating a seperate publisher and publish this data over a ROS topic as done in one of the posts above. But I suspect it would be better if the voxl-mpa-to-ros package could provide the exact camera matrix and other variables (as specified here - https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/CameraInfo.html) as a ROS topic directly.

Kindly let us know if there is any way to implement this.

@Judoor-0, were you able to run SLAM using the stereo data from the drone? Or did you use depth data from a depth camera? The RB5 flight drone that I am using does not provide depth data directly, and hence I was thinking of using the Stereo data package within RTAB-MAP.

Thanks!

Alex Kushleyev · 30 Jul 2024, 20:52

@Prabhav-Gupta,

The easiest way to do this, perhaps, is to create a python script that reads the camera calibration json and publishes a latched message via rospy. Similarly it could be done in C++ with a little more effort.. I don't think this currently exists.

Alex

Prabhav Gupta · 30 Jul 2024, 21:00

@Alex-Kushleyev

Yes, that is similar to what I had in mind. I will try this out.

Thanks!

Judoor 0 · 31 Jul 2024, 07:21

@Prabhav-Gupta I didn't use the stereo data because I don't have one on my drone. I used the point cloud returned by the TOF. But I know that with RTAB-Map you can easily use stereo without depth. There are some examples that may help you in the rtabmap_ros website.

Prabhav Gupta · 31 Jul 2024, 17:42

@Judoor-0 Thanks a lot, I will look into it.

Prabhav Gupta · 14 Aug 2024, 21:05

@Alex-Kushleyev

I was trying to create this script -

@Alex-Kushleyev said in Install libopencv on Starling 2:

The easiest way to do this, perhaps, is to create a python script that reads the camera calibration json and publishes a latched message via rospy. Similarly it could be done in C++ with a little more effort.. I don't think this currently exists.

and need some help.

I am working with the RB5 drone and found this in the /data/modalai/ folder in the drone:

In opencv_stereo_front_extrinsics.yml:

rb5:/data/modalai$ cat opencv_stereo_front_extrinsics.yml                                                                                                                                                            
%YAML:1.0                                                                                                                                                                                                            
---                                                                                                                                                                                                                  
R: !!opencv-matrix                                                                                                                                                                                                   
   rows: 3                                                                                                                                                                                                           
   cols: 3                                                                                                                                                                                                           
   dt: d                                                                                                                                                                                                             
   data: [ 9.9995382093423246e-01, -9.1903872613942166e-03,
       -2.8094093711296423e-03, 9.2509466314868449e-03,
       9.9970714036476482e-01, 2.2361875818584419e-02,
       2.6030723098620125e-03, -2.2386832864208624e-02,
       9.9974599460505953e-01 ]
T: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [ -7.9639069991921108e-02, -9.0666624431155548e-05,
       -1.0799460870486485e-03 ]
reprojection_error: 2.7322523335815047e-01
calibration_time: "2022-04-08 23:15:23"

and in opencv_stereo_front_intrinsics.yml:

rb5:/data/modalai$ cat opencv_stereo_front_intrinsics.yml
%YAML:1.0
---
M1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 5.0159907997903338e+02, 0., 2.9377341710319376e+02, 0.,
       5.0083699409439379e+02, 2.3544444409742863e+02, 0., 0., 1. ]
D1: !!opencv-matrix
   rows: 5
   cols: 1
   dt: d
   data: [ -1.6571748643440132e-01, 6.3134583515870882e-02,
       2.4908601395800438e-03, 6.9258577723375913e-04, 0. ]
reprojection_error1: 1.8877343672847582e-01
M2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 5.0289492433892644e+02, 0., 3.1156572782508289e+02, 0.,
       5.0234014337071841e+02, 2.4962793784523797e+02, 0., 0., 1. ]
D2: !!opencv-matrix
   rows: 5
   cols: 1
   dt: d
   data: [ -1.6640627389329365e-01, 6.4800083011513396e-02,
       1.1988146735987267e-04, -6.3680006718804514e-04, 0. ]
reprojection_error2: 1.8906708149286014e-01
width: 640
height: 480
distortion_model: plumb_bob
calibration_time: "2022-04-08 23:15:23"

To create the script that publishes CameraInfo, I need to figure out the information within these two files that correspond to the variables within the CameraInfo class as given here: https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/CameraInfo.html

sensor_msgs/CameraInfo:

std_msgs/Header header
uint32 height
uint32 width
string distortion_model
float64[] D
float64[9] K
float64[9] R
float64[12] P
uint32 binning_x
uint32 binning_y
sensor_msgs/RegionOfInterest roi

Some of the variable mappings are clear, such as height, width, distortion model, R and D.

How do I map K and P required by CameraInfo from the Calibration files? And anything else that I should potentially look out for while doing this mapping?

Thanks!

Alex Kushleyev · 14 Aug 2024, 21:16

@Prabhav-Gupta ,

K is your M1 and M2 matrices for both cameras (3x3 Intrinsic camera matrix)

The link you shared has information how to compute the P matrix. I am not sure if it would be used by your application though.

Alex

Prabhav Gupta · 14 Aug 2024, 21:21

@Alex-Kushleyev,

Cool, Thanks a lot!