在 ROS 模拟中使用 OpenCV 尝试 KLT 时出现的问题

问题描述

我正在尝试使用 ROS + Gazebo 和 OpenCV 的 KLT 算法制作一些视觉里程计项目。

我的代码运行得不太好。使用视频文件作为源 as in this tutorial 工作正常，但是当我尝试使用 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,5,-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()

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）