軟件篇-02-基於ZED 2和ORB_SLAM2的SLAM實踐

時隔兩周，我又回來了。

​

本期內容如題，ZED 2的SDK功能還是挺多的，包括軌跡跟蹤，實時建圖等等。雖然由於是商業產品，我看不到他們的源代碼，但是根據使用情況來看，ZED 2內部是采用了IMU和光學信息融合的算法，並且IMU的數據在決策權重上占了更大的比例，至於為什么我會在后文講到。關於ORB_SALM2，它給我們提供了目前來說效果比較好的實時定位和稀疏地圖構建功能，同時支持單目、雙目和RGB-D攝像頭的SLAM算法，但是目前來說大多數SLAM工程應用都是用來做導航避障，單單的稀疏地圖並不能滿足我們的要求，於是我決定充分利用兩者的信息，以達到最佳的效果。

 

目錄

• 安裝ZED 2 SDK和ORB_SLAM2
• 使用Qt Creator開發ROS工程
• 適配ORB_SALM2於雙目相機ZED 2
• 創建功能包用於保存SLAM地圖和三維點雲圖
• RVIZ、URDF配置

---

一、安裝ZED 2 SDK和ORB_SLAM2

 

• 安裝ZED 2 SDK和ZED 2 ROS功能包

ZED SDK 3.1 - Download

https://www.stereolabs.com/docs/installation/jetson/

https://github.com/stereolabs/zed-ros-wrapper

• 安裝ORB_SLAM2

如果你只在ROS環境下使用ORB_SLAM2的話，僅需要下載以下功能包

https://github.com/appliedAI-Initiative/orb_slam_2_ros

否則，下載同時下載編譯ORB_SLAM2源碼

https://github.com/raulmur/ORB_SLAM2

 

二、使用Qt Creator開發ROS工程

 

由於ros_qtc_plugin不支持arm64系統，故直接用Qt-Creator開發

這個時候你已經完成下載以上兩個功能包，將功能包放入已經創建好的ROS工作空間后編譯

$ rosdep install --from-paths src --ignore-src -r -y 
$ catkin_make -DCMAKE_BUILD_TYPE=Release 
$ source ./devel/setup.bash 
# 后續你可能還需要以下Cmake參數 -DCATKIN_DEVEL_PREFIX=../devel -DCMAKE_INSTALL_PREFIX=../install

默認你已經安裝配置好了Qt Creator，參考以下文檔來完成

http://wiki.ros.org/IDEs#QtCreator

 

1、修改Qt Creator.Desktop文件

$ sudo vim /usr/share/applications/org.qt-project.qtcreator.desktop

 

2、更改CMakeLists.txt，在文件頭添加以下內容，目的是為了在Qt工程樹中顯示所有文件

$ sudo vim catkin_qi/src/CMakeLists.txt 
project(catkin_qi) 
#Add custom (non compiling) 
targets so launch scripts and python files show up in QT Creator's project view. file(GLOB_RECURSE EXTRA_FILES */*) 
# 以下的‘ROS_Packages’是我自定義的名字，到時所有功能包都會顯示在該目錄下 add_custom_target(ROS_Packages ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})

 

3、打開Qt-Creator，打開CMakeLists.txt，接下來等待軟件Cmake，完成后工程樹就會自動展示出來了，嘗試'Build'來驗證是否配置成功。

 

Qt-ROS工程樹

 

三、適配ORB_SALM2於雙目相機ZED 2

 

orb_slam_2_ros目前只是支持了RealSense等系列相機，於是我們需要自己更改源碼來適配ZED2，要更改的文件不多，我列舉一下：

在'lanuch'目錄內添加一個自己的lanuch的文件，內容先復制其它'stereo'模板，最后修改的文件如下：

<?xml version="1.0"?> 
<launch> <!-- launch the zed_ros_wrapper node--> 
<include file="$(find zed_wrapper)/launch/zed2.launch"> </include> 

<node name="orb_slam2_stereo" pkg="orb_slam2_ros" type="orb_slam2_ros_stereo" output="screen">
 <remap from="image_left/image_color_rect" to="/zed2/zed_node/left/image_rect_color" /> 
 <remap from="image_right/image_color_rect" to="/zed2/zed_node/right/image_rect_color" /> <param name="publish_pointcloud" type="bool" value="true" /> <param name="publish_pose" type="bool" value="true" /> <param name="localize_only" type="bool" value="false" /> <param name="reset_map" type="bool" value="false"/><!-- static parameters --><paramname="load_map"type="bool"value="true"/><paramname="map_file"type="string"value="/home/qi/catkin_qi/src/tx2_slam/map/bin/zed2_slam_Map.bin"/><paramname="settings_file"type="string"value="$(find tx2_slam)/config/Zed2Stereo.yaml"/><paramname="voc_file"type="string"value="$(find tx2_slam)/Vocabulary/ORBvoc.txt"/><paramname="pointcloud_frame_id"type="string"value="map"/><paramname="camera_frame_id"type="string"value="camera_link"/><paramname="min_num_kf_in_map"type="int"value="5"/></node><!-- Position respect to base frame (i.e. "base_link) --><argname="cam_pos_x"default="0.0"/><argname="cam_pos_y"default="0.0"/><argname="cam_pos_z"default="0.0"/><!-- Orientation respect to base frame (i.e. "base_link) --><argname="cam_roll"default="0.0"/><argname="cam_pitch"default="0.0"/><argname="cam_yaw"default="0.0"/><!-- ROS URDF description of the ZED --><groupif="true"><paramname="zed2_description"command="$(find xacro)/xacro '$(find tx2_slam)/urdf/zed_descr.urdf.xacro' camera_name:=tx2zed2 camera_model:=zed2 base_frame:=zed2_base_link cam_pos_x:=$(arg cam_pos_x) cam_pos_y:=$(arg cam_pos_y) cam_pos_z:=$(arg cam_pos_z) cam_roll:=$(arg cam_roll) cam_pitch:=$(arg cam_pitch) cam_yaw:=$(arg cam_yaw)"/><nodename="zed2_state_publisher"pkg="robot_state_publisher"type="robot_state_publisher"output="screen"required="true"><remapfrom="robot_description"to="zed2_description"/></node></group><!-- Launch rivz display --><nodename="rviz"pkg="rviz"type="rviz"args="-d $(find tx2_slam)/config/rviz_config.rviz"output="screen"/><nodename="MapBuild"pkg="tx2_slam"type="MapBuild"output="screen"><paramname="prefix"type="string"value="$(find tx2_slam)/map/pcd_files/"/></node></launch>

 

其中你需要更改的有：添加ZED2的lanuch文件、remap ros-topic names（這樣orb_ros就能收到zed2節點的左右圖片信息了），以及其它節點。

然后就是配置屬於ZED2自己的.yaml文件了,你可以參考以下內容來配置：

其中fx、fy、cx、cy、k1、k2、k3、p1，p2是相機的內參和畸變矯正參數，你可以在你安裝ZED SDK的目錄下找到存儲有相關參數的文件。至於圖片的分辨率和幀率看情況來定，不過要和ZED2 的設置保持一致（關於如何設置ZED2 輸出分辨率下文有說明）。

當然，每個相機出廠參數不可能一模一樣，條件允許的話最好還是用camera_calibration標定一下相機。

 

%YAML:1.0 
# Camera calibration and distortion parameters (OpenCV) Camera.fx: 527.2025 Camera.fy: 527.245 Camera.cx: 618.31 Camera.cy: 367.1445 # get the rectified image from zed-ros-wrapper, so set all zero Camera.k1: 0.0 Camera.k2: 0.0 Camera.p1: 0.0 Camera.p2: 0.0 Camera.k3: 0.0 # more to see https://www.stereolabs.com/docs/video/camera-controls/ Camera.width: 2560 Camera.height: 720 Camera.fps: 30.0 # stereo baseline(m) times fx Camera.bf: 63.1965 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1 # Close/Far threshold. Baseline times. # set it according to the actual effect ThDepth: 35 # the depth in program equal to the real depth times the DepthMapFactor #DepthMapFactor: 1000.0 #-------------------------------------------------------------------------------------------- # Stereo Rectification. Only if you need to pre-rectify the images. # Camera.fx, .fy, etc must be the same as in LEFT.P #-------------------------------------------------------------------------------------------- LEFT.height: 720 LEFT.width: 2560 LEFT.D: !!opencv-matrix rows: 1 cols: 5 dt: d data: [0.0, 0.0, 0.0, 0.0, 0.0] LEFT.K: !!opencv-matrix rows: 3 cols: 3 dt: d data: [527.2025, 0.0, 618.31, 0.0, 527.245, 367.1445, 0.0, 0.0,1.0]LEFT.R:!!opencv-matrixrows:3cols:3dt: d data:[1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]LEFT.P:!!opencv-matrixrows:3cols:4dt: d data:[527.2025,0.0,618.31,0.0,0.0,527.245,367.1445,0.0,0.0,0.0,1.0,0.0]RIGHT.height:720RIGHT.width:2560RIGHT.D:!!opencv-matrixrows:1cols:5dt: d data:[0.0,0.0,0.0,0.0,0.0]RIGHT.K:!!opencv-matrixrows:3cols:3dt: d data:[527.2025,0.0,618.31,0.0,527.245,367.1445,0.0,0.0,1.0]RIGHT.R:!!opencv-matrixrows:3cols:3dt: d data:[1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0]RIGHT.P:!!opencv-matrixrows:3cols:4dt: d data:[527.2025,0.0,618.31,0.0,0.0,527.245,367.1445,0.0,0.0,0.0,1.0,0.0]#--------------------------------------------------------------------------------------------# ORB Parameters#--------------------------------------------------------------------------------------------# ORB Extractor: Number of features per imageORBextractor.nFeatures:1200# ORB Extractor: Scale factor between levels in the scale pyramidORBextractor.scaleFactor:1.2# ORB Extractor: Number of levels in the scale pyramidORBextractor.nLevels:8# ORB Extractor: Fast threshold# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST# You can lower these values if your images have low contrastORBextractor.iniThFAST:20ORBextractor.minThFAST:7

 

設置ZED 2輸出的分辨率和幀率，一般來說分辨率越高效果越好，但是需要同時考慮負載情況，不過我們總得相信我們的TX2不是/狗頭。

在zed-ros-wraper功能包的zed wrapper_nodelet.cpp里添加以下兩行代碼來設置。

//set the Zed2 ouput 
mZedParams.camera_resolution = sl::RESOLUTION::HD720; 
mZedParams.camera_fps = 30; 
mConnStatus = mZed.open(mZedParams);

 

四、創建功能包用於保存SLAM地圖和三維點雲圖

• 保存三維點雲圖

這里我是直接將ZED2節點的點雲存為pcd文件供下次使用的，新建一個功能包創建以下節點：注意這里訂閱的是'/zed2/zed_node/mapping/fused_cloud'話題，運行節點，取最后一幀pcd文件作為最終的點雲圖。下次直接讀取pcd文件然后發布相關數據到rviz中即可。

以下為使用'pcl_viewer'查看的效果。

 

房間內部分點雲圖

#pragma once #include <ros/ros.h> #include <sensor_msgs/PointCloud2.h> #include <pcl_conversions/pcl_conversions.h> #include <pcl_ros/point_cloud.h> #include <tf2_ros/buffer.h> #include <tf2_ros/transform_listener.h> #include <tf2_eigen/tf2_eigen.h> #include <pcl/io/pcd_io.h> class MapBuild { public: ~MapBuild(); MapBuild(); private: }; class PointCloudToPCD { protected: ros::NodeHandle nh_; private: std::string prefix_; std::string filename_; bool binary_; bool compressed_; std::string fixed_frame_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; public: std::string cloud_topic_; ros::Subscriber sub_; //////////////////////////////////////////////////////////////////////////////// // Callback void cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud) { if ((cloud->width * cloud->height) == 0) return; ROS_INFO ("Received %d data points in frame %s with the following fields: %s", (int)cloud->width * cloud->height, cloud->header.frame_id.c_str (), pcl::getFieldsList(*cloud).c_str()); Eigen::Vector4f v = Eigen::Vector4f::Zero(); Eigen::Quaternionf q = Eigen::Quaternionf::Identity();if(!fixed_frame_.empty()){if(!tf_buffer_.canTransform(fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL(cloud->header.stamp), ros::Duration(3.0))){ROS_WARN("Could not get transform!");return;} Eigen::Affine3d transform; transform = tf2::transformToEigen(tf_buffer_.lookupTransform(fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL(cloud->header.stamp))); v = Eigen::Vector4f::Zero(); v.head<3>()= transform.translation().cast<float>(); q = transform.rotation().cast<float>();} std::stringstream ss;if(filename_ !=""){ ss << filename_ <<".pcd";}else{ ss << prefix_ << cloud->header.stamp <<".pcd";}ROS_INFO("Data saved to %s", ss.str().c_str()); pcl::PCDWriter writer;if(binary_){if(compressed_){ writer.writeBinaryCompressed(ss.str(),*cloud, v, q);}else{ writer.writeBinary(ss.str(),*cloud, v, q);}}else{ writer.writeASCII(ss.str(),*cloud, v, q,8);}}////////////////////////////////////////////////////////////////////////////////PointCloudToPCD():binary_(false),compressed_(false),tf_listener_(tf_buffer_){// Check if a prefix parameter is defined for output file names. ros::NodeHandle priv_nh("~");if(priv_nh.getParam("prefix", prefix_)){ROS_INFO_STREAM("PCD file prefix is: "<< prefix_);}elseif(nh_.getParam("prefix", prefix_)){ROS_WARN_STREAM("Non-private PCD prefix parameter is DEPRECATED: "<< prefix_);} priv_nh.getParam("fixed_frame", fixed_frame_); priv_nh.getParam("binary", binary_); priv_nh.getParam("compressed", compressed_); priv_nh.getParam("filename", filename_);if(binary_){if(compressed_){ROS_INFO_STREAM("Saving as binary compressed PCD");}else{ROS_INFO_STREAM("Saving as binary uncompressed PCD");}}else{ROS_INFO_STREAM("Saving as ASCII PCD");}if(filename_ !=""){ROS_INFO_STREAM("Saving to fixed filename: "<< filename_);} cloud_topic_ ="/zed2/zed_node/mapping/fused_cloud"; sub_ = nh_.subscribe(cloud_topic_,1,&PointCloudToPCD::cloud_cb,this);ROS_INFO("Listening for incoming data on topic %s", nh_.resolveName(cloud_topic_).c_str());}};

• 保存ORB_SLAM2地圖

orbslam2_ros功能包為我們提供了地圖保存和加載功能，稍微看下它的源碼就會發現，你可以創建一個ROS server client請求位於orb_slam2_ros的服務：

如此以來，下次在相同環境下就可以直接加載先前建好的地圖了（記得修改lanuch文件里的加載地圖參數）。

ros::init ( argc, argv, "MapBuild", ros::init_options::AnonymousName); 
　　ros::NodeHandle n; ros::ServiceClient client = n.serviceClient<orb_slam2_ros::SaveMap>("/orb_slam2_stereo/save_map"); orb_slam2_ros::SaveMap srv; srv.request.name = "/home/qi/catkin_qi/src/tx2_slam/map/bin/zed2_slam_Map.bin"; if (client.call(srv)) { ROS_INFO("Ready to create Map"); } else { ROS_ERROR("Failed to call service SaveMap"); return 1; }

 

五、RVIZ、URDF配置

 

URDF參考上面lanuch文件里的配置，記得在節點里發布tf關聯讓攝像頭模型隨着位置變化一起動起來。

RVIZ配置，可以根據自己的情況設置添加的插件和訂閱的話題，因為內容太多我就只放圖片形式的配置了：

 

下期再見。