【代码相关-ROS】利用小觅采集的rosbag，制作双目图像集，跑orbslam2

环境

Ubuntu16.04+小觅标准彩色双目+orbslam2
做这个的目的，是想用slam的定位算法解决我三维重建的定位，但是结果来看（至少orbslam2）不符合预期。既然做了，就记录下来。

小觅发布图像话题

因为我要做三维重建，希望图片分辨率大点。但是小觅ros发布的是640*400的分辨率。

1. 我先改/home/…/MYNT-EYE-S-SDK/wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml文件。修改里面的

#改为6，原来是2 
standard2/request_index: 6

重新编译

make ros

roscore
roslaunch mynt_eye_ros_wrapper mynteye.launch 
#用来看当前图像，我录制的矫正后的1280*800图像。
rosrun image_view image_view image:=/mynteye/left_rect/image_rect
#保存话题
rosbag record -O shiyanshi.bag /mynteye/left_rect/image_rect /mynteye/right_rect/image_rect

3. ctrl+c停止rosbag record的程序
4. 至此获得了小觅双目的1280*800的矫正后的左右话题图像ros包

从rosbag中保存时间戳一致的左右图像

1.在catkin_ws/src下创建一个工作小包（我命名为catchImgFromTopic），main.cpp如下

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include "sensor_msgs/Image.h"
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <boost/thread/thread.hpp>
#include <iostream>
#include <fstream>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <stdio.h>
#include <string.h>

using std::cout;
using std::endl;
using std::stringstream;
using std::string;

int countl=0;
int countr=0;
int count=0;



void multi_callback(const sensor_msgs::ImageConstPtr& msgl, const  sensor_msgs::ImageConstPtr& msgr)
{
   std::cout << "同步完成！" << std::endl;
       char l_name[30];
       char r_name[30];

       snprintf(l_name, sizeof(l_name), "./image_0/%06d.png", count);
       snprintf(r_name, sizeof(r_name), "./image_1/%06d.png", count);

       cv::imwrite(l_name, cv_bridge::toCvShare(msgl,"bgr8")->image);
       cv::imwrite(r_name, cv_bridge::toCvShare(msgr,"bgr8")->image);

// 	f<
      //追加写入,在原来基础上加了ios::app 
   std::stringstream ss;
   std::string str;
   std::fstream f;
   f.open("./times.txt",std::ios::out|std::ios::app);
   ss << std::setw(6) << std::setfill('0') << count ;
   ss >> str; 
   f<<"0.0"<<str<<std::endl;
   f.close();
       std::cout << "Saved " << l_name << " " << r_name << " " << " to current directory" << std::endl;
   count=count+1;
}

int main(int argc, char **argv)
{

   ros::init(argc, argv, "multi_receiver");
   ros::NodeHandle n;

   message_filters::Subscriber<sensor_msgs::Image> subscriber_l(n,"/mynteye/left_rect/image_rect",1000,ros::TransportHints().tcpNoDelay());
   message_filters::Subscriber<sensor_msgs::Image> subscriber_r(n,"/mynteye/right_rect/image_rect",1000,ros::TransportHints().tcpNoDelay());
   
   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> syncPolicy;
   //message_filters::TimeSynchronizer sync(subscriber_laser, subscriber_pose, 10);
   message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), subscriber_l, subscriber_r);  
   sync.registerCallback(boost::bind(&multi_callback, _1, _2));

   std::cout << "hahah" << std::endl;
   ros::spin();  
   return 0;

}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(catchImgFromTopic)

##Compile as C++11, supported in ROS Kinetic and newer
#add_compile_options(-std=c++11)

##Find catkin macros and libraries
##if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
##is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
 cv_bridge
 image_transport
 roscpp
 rosbag
 sensor_msgs
 std_msgs
)
find_package(OpenCV)
include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
add_executable(exportMynt /home/ymb/catkin_ws/src/catchImgFromTopic/src/main.cpp)
target_link_libraries(exportMynt ${OpenCV_LIBS})
target_link_libraries(exportMynt ${catkin_LIBRARIES})

2.编译
只编译一个包

catkin_make -DCATKIN_WHITELIST_PACKAGES="catchImgFromTopic"

保存图片

1.先在一个文件夹下创建两个文件夹，命名为image_0，image_1
2.再运行roscore
3.该文夹下打开终端，运行

~/catkin_ws/build/catchImgFromTopic/exportMynt
#新开终端，播放rosbag
rosbag play shiyanshi.bag

结果

至此，图像集都做好了。
回去跑orbslam2的KITTI双目样例吧，别忘了也做一个跟KITTI数据集一样的相机参数.yaml。
结束》。。。