REMODE+ORBSLAM运行配置(2) REMODE和编译后的ORB ros工程利用节点实现通讯
2017-01-03 09:16
706 查看
将ORB编译成ROS工程后,就可以利用ROS的节点通信实现ORB和REMODE的数据传输。
步骤:
1 仿照SVO-REMODE的编译方式。安装googletest 和rpg_open_remode至~/catkin_ws/src 目录下。
2 修改 ORB的主函数,以便发送数据。
2.1 加入头文件
以下修改在main函数中
开启ros节点,vk::getParam才能找到launch文件的参数
1 frame的id
2 左图
3 四元数格式的位姿
4 当前场景的最大最小深度。
opencv的cv::Mat 和ros中的sensor_msgs/Image 需要cv_bridge::CvImage 做个转换。
用法:http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
3 仿照
至此,运行脚本就可以运行REMODE+ORBSLAM工程了。
步骤:
1 仿照SVO-REMODE的编译方式。安装googletest 和rpg_open_remode至~/catkin_ws/src 目录下。
2 修改 ORB的主函数,以便发送数据。
2.1 加入头文件
#include<vikit/file_reader.h> #include<vikit/params_helper.h> #include<vikit/camera_loader.h> //以上几个头文件是从程序外从参数用的,需要下载vikit源码 #include<cv_bridge/cv_bridge.h> #include <sensor_msgs/image_encodings.h> #include <image_transport/image_transport.h> //以上3个头文件是发送图片用的 #include<ros/ros.h> #include<ros/package.h> #include"std_msgs/String.h" #include"sstream" #include"DenseInput.h"//这个头文件 说明了传输数据的结构,需要从svo工程svo_msgs里的拷出来。 #include <Converter.h>//位姿矩阵转四元素 #include <pangolin/pangolin.h> #include <iomanip>2.2 在main函数中完成通信
以下修改在main函数中
开启ros节点,vk::getParam才能找到launch文件的参数
ros::init(argc,argv,"ORB_SLAM"); ros::NodeHandle SendMessage; // 发布 ros::Publisher pub_dense_=SendMessage.advertise<svo_msgs::DenseInput>("dense_input",10); //利用vikit读取参数 const string dataset_path(vk::getParam<std::string>("ORB_SLAM/Img_Path")); const string CamPara_Path(vk::getParam<std::string>("ORB_SLAM/CamPara_Path")); const string BOW_Path(vk::getParam<std::string>("ORB_SLAM/BOW_Path"));发送数据:每执行一次slam,发送一次数据。数据包括
1 frame的id
2 左图
3 四元数格式的位姿
4 当前场景的最大最小深度。
/////////////////////////////////////////////////////////与REMODE通信////////////////////////////// svo_msgs::DenseInput msg; msg.header.stamp=ros::Time(20);//svo中设置20可以正常运行 msg.header.frame_id="/world"; msg.frame_id=framecnt; cv_bridge::CvImage img_msg; img_msg.header.stamp=msg.header.stamp; img_msg.header.frame_id="camera"; img_msg.image=imRGB; img_msg.encoding = sensor_msgs::image_encodings::MONO8; msg.image = *img_msg.toImageMsg(); double min_z = std::numeric_limits<double>::max(); double max_z = std::numeric_limits<double>::min(); SLAM.mpTracker->getSceneDepth(SLAM.mpTracker->mCurrentFrame,max_z,min_z); cout<<"min_z----------------------------------------- "<<min_z<<endl; cout<<"max_z------------------------------------------- "<<max_z<<endl; msg.min_depth=(float)min_z; msg.max_depth=(float)max_z; cv::Mat TWC=SLAM.mpTracker->mCurrentFrame.mTcw.inv(); cv::Mat RWC=TWC.rowRange(0,3).colRange(0,3); cv::Mat tWC=TWC.rowRange(0,3).col(3); vector<float> q=ORB_SLAM2::Converter::toQuaternion(RWC); msg.pose.position.x = tWC.at<float>(0,0); msg.pose.position.y = tWC.at<float>(1,0); msg.pose.position.z = tWC.at<float>(2,0); msg.pose.orientation.w = q[3];//q.w(); msg.pose.orientation.x = q[0];//q.x(); msg.pose.orientation.y = q[1];//q.y(); msg.pose.orientation.z = q[2];//q.z(); pub_dense_.publish(msg); ////////////////////////////////////////////////////////////////////////////////////////////////////////
opencv的cv::Mat 和ros中的sensor_msgs/Image 需要cv_bridge::CvImage 做个转换。
用法:http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
3 仿照
ubuntu下通过命令打开多个终端并在相应终端执指令
写一个脚本,目的是在运行工程的时候方便,快捷。脚本内容为gnome-terminal -x bash -c "roscore" gnome-terminal -x bash -c "rosrun rviz rviz -d /home/baohua/project/SLAM/REMODE_ORB/catkin_ws/src/rpg_open_remode/open_remode.rviz " gnome-terminal -x bash -c " source '/home/baohua/project/SLAM/REMODE_ORB/catkin_ws/devel/setup.sh';roslaunch ORB_SLAM MH01.launch " gnome-terminal -x bash -c " source '/home/baohua/project/SLAM/REMODE_ORB/catkin_ws/devel/setup.sh';roslaunch rpg_open_remode remode_EuRoC.launch "
至此,运行脚本就可以运行REMODE+ORBSLAM工程了。
相关文章推荐
- REMODE+ORBSLAM运行配置(1) 把ORB编译成ROS工程
- 编译ORBSLAM2 build_ros.sh,实现kinect2在ROS环境下运行ORBSLAM2
- ORB_SLAM运行详细过程(ubuntu14.04系统和ROS Indigo环境搭建,配置及测试运行)
- 编译安装及运行单目ORBSLAM2在Ubuntu14.04
- 利用条件编译实现工程定制版本的自动输出
- ros 节点实现简易超声雷达串口通讯 模拟出激光雷达消息
- Ubuntu 16 ORB_SLAM2使用KinectV2在ROS上运行总结
- 在ROS indigo下运行ORB-SLAM2
- 利用VS编辑器(cl)配置Notepad++环境以编译/运行C++
- 小强ROS机器人教程(17)___利用ORB_SLAM2建立环境三维模型
- ubuntu下Eclipse修改配置使得可以成功编译运行C11工程(另加解决调试时warning: breakpoint installation failed)
- 从编译运行orbslam2说起
- ZED 相机 && ORB-SLAM2安装环境配置与ROS下的调试
- 利用邻接表求解所有节点的最短路径 java实现 可运行
- [ROS]编译orbslam出错,ros路径问题
- Ubuntu 16.04+ROS+ORB-SLAM2配置以及相关问题
- 如何配置android studio环境实现ionic的编译,运行与打包
- Sublime Java 环境配置-实现Java程序编译、运行
- 编译安装及运行单目ORBSLAM2在Ubuntu14.04
- [置顶] ubuntu14.04 + ROS下编译Pangolin和ORB_SLAM2死机的解决办法