Issues with Aligning Infrared and RGB Images from Intel RealSense D435i in ROS

Question

I am working on a project to align infrared and RGB images from an Intel RealSense D435i camera using ROS. My goal is to subscribe to the RGB and infrared image topics and publish the aligned infrared image to match the RGB image frame. However, I am facing an issue where the aligned images do not completely overlap.

Here is the code I am using:

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

class ImageAligner:
    def __init__(self):
        self.bridge = CvBridge()
        self.tf_listener = tf.TransformListener()
        
        self.rgb_image_sub = message_filters.Subscriber("/camera/color/image_raw", Image)
        self.infra_image_sub = message_filters.Subscriber("/camera/infra1/image_rect_raw", Image)
        self.rgb_info_sub = message_filters.Subscriber("/camera/color/camera_info", CameraInfo)
        self.infra_info_sub = message_filters.Subscriber("/camera/infra1/camera_info", CameraInfo)
        
        self.ts = message_filters.ApproximateTimeSynchronizer([self.rgb_image_sub, self.infra_image_sub, self.rgb_info_sub, self.infra_info_sub], 10, 0.1)
        self.ts.registerCallback(self.align_image)
        
        self.aligned_infra_pub = rospy.Publisher("/camera/aligned_infra1_to_color/image_raw", Image, queue_size=10)
        self.aligned_infra_info_pub = rospy.Publisher('camera/aligned_infra1_to_color/camera_info', CameraInfo, queue_size=10)
    
    def align_image(self, rgb_image, infra_image, rgb_info, infra_info):
        rospy.loginfo("Callback triggered.")
        try:
            rgb_cv_image = self.bridge.imgmsg_to_cv2(rgb_image, "bgr8")
            infra_cv_image = self.bridge.imgmsg_to_cv2(infra_image, "mono8")
            rospy.loginfo("Converted images to OpenCV format. RGB size: {}, Infra size: {}".format(rgb_cv_image.shape, infra_cv_image.shape))
            
            K_rgb = np.array(rgb_info.K).reshape((3, 3)).astype(np.float32)
            K_infra = np.array(infra_info.K).reshape((3, 3)).astype(np.float32)
            rospy.loginfo("Obtained camera matrices.")
            
            try:
                self.tf_listener.waitForTransform("camera_color_optical_frame", "camera_infra1_optical_frame", rospy.Time(0), rospy.Duration(1.0))
                (trans, rot) = self.tf_listener.lookupTransform("camera_color_optical_frame", "camera_infra1_optical_frame", rospy.Time(0))
                rospy.loginfo("Transformation from infrared to RGB camera obtained.")
            except (tf.Exception, tf.LookupException, tf.ConnectivityException) as e:
                rospy.logerr("Failed to get transformation from infrared to RGB camera: {}".format(e))
                return
            
            rot_matrix = tf.transformations.quaternion_matrix(rot)[:3, :3].astype(np.float32)
            trans_matrix = np.array(trans).reshape((3, 1)).astype(np.float32)
            rospy.loginfo("Rotation matrix: {}".format(rot_matrix))
            rospy.loginfo("Translation matrix: {}".format(trans_matrix))  
            
            n = rot_matrix[:, 2]
            d = np.linalg.norm(n)
            rospy.loginfo("Normal vector: {}".format(n))
            rospy.loginfo("Distance from camera: {}".format(d))
            
            h_matrix = np.dot(K_rgb, np.dot((rot_matrix-trans_matrix*np.transpose(n)/d), np.linalg.inv(K_infra)))
            rospy.loginfo("Computed homography matrix.")
            
            infra_aligned = cv2.warpPerspective(infra_cv_image, h_matrix, (rgb_cv_image.shape[1], rgb_cv_image.shape[0]))
            rospy.loginfo("Warped infrared image to RGB perspective. Size: {}".format(infra_aligned.shape))
            
            if infra_aligned is not None and infra_aligned.size > 0:
                infra_aligned_msg = self.bridge.cv2_to_imgmsg(infra_aligned, "mono8")
                rospy.loginfo("Converted aligned infrared image to ROS format.")
                self.aligned_infra_pub.publish(infra_aligned_msg)
                rospy.loginfo("Published aligned infrared image.")
            else:
                rospy.logwarn("Aligned infrared image is empty. Skipping publish.")
            
            self.aligned_camera_info(infra_info)
                
        except CvBridgeError as e:
            rospy.logerr("CvBridge Error: {0}".format(e))
        except Exception as e:
            rospy.logerr("Error in aligning images: {0}".format(e))
            
    def aligned_camera_info(self, infra_info):
        if infra_info is None:
            return

        aligned_camera_info = infra_info
        aligned_camera_info.header.frame_id = 'camera_color_optical_frame'
        
        self.aligned_infra_info_pub.publish(aligned_camera_info)

if __name__ == '__main__':
    rospy.init_node('image_aligner', anonymous=True)
    ImageAligner()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo("Shutting down image aligner node.")

The images are not completely overlapping after alignment. I suspect the issue might be with the homography matrix calculation or the transformation between the camera frames.

RGB image

Infrared image (Original)

Aligned infrared image

Details:

• I am using the /camera/color/image_raw topic for RGB images and the /camera/infra1/image_rect_raw topic for infrared images.
• The homography matrix is computed using the camera matrices and the transformation between the infrared and RGB cameras.

Questions:

1. Is there an error in the homography matrix calculation?
2. Are there any additional steps needed to ensure the images completely overlap?
3. Is there a better approach to align these images using ROS and OpenCV?

Any guidance or suggestions to resolve this issue would be greatly appreciated.

Answer 1

in order to align the images acquired by 2 different cameras you need the following information:

• rgb camera intrinsic matrix (3x3)
• ir camera intrinsic matrix (3x3)
• extrinsic matrix representing the transformation between the rgb and the ir camera (4x4)
• rgb image size

This python code will align the ir image, returnig an image overlappable to the rgb image:

import numpy as np

def align(
    intrinsics_ir, intrinsics_rgb, extrinsics_rgb2ir, ir_image, rgb_image_shape
) -> np.ndarray:
    width, height = rgb_image_shape
    out = np.zeros((height, width)) / 0
    y, x = np.meshgrid(
        np.arange(ir_image.shape[0]), np.arange(ir_image.shape[1]), indexing="ij"
    )
    x = x.reshape(1, -1)
    y = y.reshape(1, -1)
    z = ir_image.reshape(1, -1)
    x = (x - intrinsics_ir[0, 2]) / intrinsics_ir[0, 0]
    y = (y - intrinsics_ir[1, 2]) / intrinsics_ir[1, 1]
    pts = np.vstack((x * z, y * z, z))
    pts = extrinsics_rgb2ir[:3, :3] @ pts + extrinsics_rgb2ir[:3, 3:]
    pts = intrinsics_rgb @ pts
    px = np.round(pts[0, :] / pts[2, :])
    py = np.round(pts[1, :] / pts[2, :])
    mask = (px >= 0) * (py >= 0) * (px < width) * (py < height)
    out[py[mask].astype(int), px[mask].astype(int)] = pts[2, mask]
    return out