Проблемы при попытке использовать KLT с OpenCV в моделировании ROS

Question

Я пытаюсь создать несколько проектов Visual Odometry с ROS + Gazebo с алгоритмом KLT из OpenCV.

Мой код работает не так хорошо. Он отлично работал с использованием видеофайла в качестве источника, как в этом руководстве , но когда я пытаюсь использовать темы / изображения ROS в качестве входных данных, это не удается. Даже если я могу заставить точки функции отслеживать, вид застывает (мне нужно несколько раз нажать esc, чтобы обновить) и не рисует путь точек по мере его движения.

Я также попытался изменить смоделированную частоту кадров камеры, на случай, если это было слишком быстро для времени обработки, но безуспешно.

Я думаю, что делаю какую-то основную ошибку, связанную с обратным вызовом или событием, связанным с отношением «старый кадр» к «новому кадру», но не знаю, как ее решить.

Я был бы очень признателен, если бы кто-то мог оказать поддержку в этом вопросе.

Мой код ниже:

      #!/usr/bin/env python

import roslib
import rospy
import sys
import cv2
import numpy as np

from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError


class OpticalFlow(object):



    def __init__(self):

        #Setting up the bridge and the subscriber
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/realsense/color/image_raw", Image,self.image_callback)
        
        # Parameters for Good Features Detection
        self.feature_params = dict( maxCorners = 100,
                                qualityLevel = 0.3,
                                minDistance = 7,
                                blockSize = 7 )

        # Parameters for Lucas-Kanade optical flow
        self.lk_params = dict( winSize  = (150,150),
                                maxLevel = 2,
                                criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

        # Create some random colors for the tracking points
        self.color = np.random.randint(0,255,(100,3))



    def image_callback(self,ros_image):

        # Using cv_bridge() to convert the ROS image to OpenCV format
        try:
            cv_image = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")#bgr8
            hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
            gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
            
        except CvBridgeError as e:
            print(e)
    

        cv2.imshow("Robot Image", cv_image) # The actual (non-processed image from the simulation/robot)
        #cv2.imshow('Image HSV', hsv_image)
        #cv2.imshow('Image Gray', gray)
        frame = np.array(cv_image, dtype=np.uint8)


        # Calling the Optical Flow Function
        display = self.optical_flow(frame,self.feature_params,self.lk_params,self.color)


    
    def optical_flow(self,frame,feature_params,lk_params,color):
        
        
        old_frame = frame
        old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
        p0 = cv2.goodFeaturesToTrack(old_gray, mask = None, **feature_params)
        mask = np.zeros_like(old_frame)
        while(1):
            
            newframe = frame
            frame_gray = cv2.cvtColor(newframe, cv2.COLOR_BGR2GRAY)

            # Calculating the optical flow
            p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)
            
            # Select good points
            good_new = p1[st==1]
            good_old = p0[st==1]

            # Draw the tracks
            for i,(new,old) in enumerate(zip(good_new, good_old)):
                a,b = new.ravel()
                c,d = old.ravel()
                mask = cv2.line(mask, (a,b),(c,d), color[i].tolist(), 2)
                frame = cv2.circle(frame,(a,b),5,color[i].tolist(),-1)
            img = cv2.add(frame,mask)
            
            cv2.imshow('LK_Flow',img)                    
            k = cv2.waitKey(30) & 0xff
            if k == 27:
                break

            # Now update the previous frame and previous points
            old_gray = frame_gray.copy()
            p0 = good_new.reshape(-1,1,2)
            


def main():

    optical_flow_object = OpticalFlow()
    rospy.init_node('KLT_Node', anonymous=True)
    rospy.loginfo("\nWaiting for image topics...\n...")
    try:
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo("\nShutting Down...\n...")
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()