運行ORB-SLAM筆記_使用篇（二）

1. 編譯完成之后就可以使用了，按照說明我們可以知道，首先開啟roscore

再打開一個命令窗口使用命令：rosrun ORB_SLAM ORB_SLAM PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

其中ORB_SLAM PATH_TO_VOCABULARY：是一種樹型數據結構模型，ORB-SLAM里面主要用來做回訪(loop-closure)檢 測，對於不同數據集嚴格來說需要離線單獨處理生成，但一般成像條件都差不多所以對於不同圖像數據集可以使用相同的詞匯數據文件（相當於一個數據庫文件，方 便快速保存和查詢視覺特征信息）。雖然是TXT文件，打開就是許多數字而已。（https://answers.cosrobotics.org/question/184/guan-yu-orb_slamyun-xing-yi-ji-rgb-d-datasetde-xiang-ji-can-shu-de-wen-ti/）

PATH_TO_SETTINGS_FILE：這個很容易理解就是相機的內參；

然后我們輸入命令如圖所示：

我們查看ORB_SLAM節點的topic

salm@salm:~$ rtopic  list -v
Published topics:
 * /ORB_SLAM/Map [visualization_msgs/Marker] 1 publisher
 * /ORB_SLAM/Frame [sensor_msgs/Image] 1 publisher
 * /rosout [rosgraph_msgs/Log] 1 publisher
 * /tf [tf2_msgs/TFMessage] 1 publisher
 * /rosout_agg [rosgraph_msgs/Log] 1 publisher

Subscribed topics:
 * /camera/image_raw [sensor_msgs/Image] 1 subscriber
 * /rosout [rosgraph_msgs/Log] 1 subscriber

（1）知道該節點是訂閱/camera/image_raw這個topic,然后被 ORB_SLALM 節點處理后的圖像幀被發布到話題 ORB_SLAM/Frame 中，可以通過使用 image_view 功能包來查看

rosrun image_view image_view image:=/ORB_SLAM/Frame _autosize:=true

（2）ORB_SLAM 節點處理得到的地圖被發布到話題 /ORB_SLAM/Map 中，攝像機當前位姿和地圖全局坐標原點通過 /tf 功能包分別發布到話題 /ORB_SLAM/Camera 和話題 /ORB_SLAM/World 中，通過運行 rviz 功能包來查看地圖：

rosrun rviz rviz -d Data/rviz.rviz

這都是我們在訂閱了/camera/image_raw才能看到的試驗結果，那么我們從那里得到/camera/image_raw這個節點呢？

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

 那么我就嘗試首先發布一個經典的圖片topic 給ORB_SLAM節點:

（1）首先進入自己的工作空間catkin_ws,然后進入src,使用catkin_pkg_create命令創建我的功能包

    參考           http://wiki.ros.org/image_transport/Tutorials   

                     http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

             $ cd catkin_ws/              $ cd src/              $ catkin_create_pkg learning_image_transport image_transport cv_bridge
             $ cd ..
             $ catkin_make
             $ cd src
             $ vi my_publisher.cpp 
輸入：

#include <ros/ros.h>                       
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>    //非常有用的功能包，實現ROS與OPENCV圖像的轉換

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("camera/image_raw", 1);  //這里我們發布的主題要與ORB_SLAM節點訂閱的話題一致

  //cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
  cv::Mat image = cv::imread("/home/salm/myopencv/lena.jpg",1);  //寫入自己要載入的圖像位置
  cv::waitKey(30);
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();    //這一句就是把圖像讀入並轉為opencv可處理的圖像

  ros::Rate loop_rate(5);
  while (nh.ok()) {
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
}

之后在CMakefile.txt文件添加

add_executable(my_publisher src/my_publisher.cpp)
target_link_libraries(my_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

進行catkin_make即可我們運行查看一下實現的結果：

                                                       $ rosrun learning_image_transport  my_publisher

　　　　　　　　　　　　　　　  $  rosrun ORB_SLAM ORB_SLAM Data/ORBvoc.txt Data/Settings.yaml

                                                       $  rosrun image_view image_view image:=/ORB_SLAM/Frame _autosize:=true

                                                                  

就是這個結果，因為我也是一邊學習，一邊實現，我自己也不知道里面使用的算法，具體為什么會這樣，這也是不斷學習的原因

（2）那現在我再發布一個動態的topic 用攝像頭捕捉的話題發布給ORB_SLAM,看看實現結果：

 與之前的一樣，仍然在src 文件下     $ vi  image_converter.cpp

輸入：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp> //include the headers for OPENCV's image processing and GUI module 
#include <opencv2/highgui/highgui.hpp>  //

static const std::string OPENCV_WINDOW = "Image window";   //define show image gui

class ImageConverter
{
  ros::NodeHandle nh_;                    //define Nodehandle
  image_transport::ImageTransport it_;    //use this to create a publisher or subscriber
  image_transport::Subscriber image_sub_; //
  image_transport::Publisher image_pub_;
  
public:
  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, 
      &ImageConverter::imageCb, this);
    //image_pub_ = it_.advertise("/image_converter/output_video", 1);
    image_pub_ = it_.advertise("/camera/image_raw", 1);
    cv::namedWindow(OPENCV_WINDOW);    //Opencv HighGUI calls to create/destroy a display window on start-up / shutdon
  }

  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    cv::waitKey(3);
    
    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}

 同理 添加

                           add_executable(image_converter src/image_converter.cpp)
                           target_link_libraries(image_converter ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

當然前提是電腦有攝像頭，並且下載了USB攝像頭的驅動：https://github.com/bosch-ros-pkg/usb_cam

輸入以下命令：                             

                                                       $  roslaunch usb_cam usb_cam-test.launch

（在啟動攝像頭可能會遇到一些問題吧，比如出現  Webcam: expected picture but didn't get it...就要修改launch文件下的 "pixel_format"，比如有mjpeg ,yuyv等）

                                                       $  rosrun learning_image_transport image_converter

　　　　　　　　　　　　　　　  $  rosrun ORB_SLAM ORB_SLAM Data/ORBvoc.txt Data/Settings.yaml

                                                       $  rosrun image_view image_view image:=/ORB_SLAM/Frame _autosize:=true

 

版權所有，轉載請注明出處 http://www.cnblogs.com/li-yao7758258/p/5912663.html