本文涉及很多代码及文字,排版、文字错误请见谅。
阅读时间预计30分钟
本文涉及图像、数据均由INDEMIND双目视觉惯性模组采集
一:环境介绍
系统:Ubuntu 16.04 ROS
ORB依赖库:Pangolin、OpenCV、Eigen3、DBoW2、g2o,ros
二:下载SDK及ORB-SLAM源码
下载地址:
SDK:http://indemind.cn/sdk.html
ORB-SLAM:https://github.com/raulmur/ORB_SLAM2
三:修改源码
1、下载好SDK之后,进入SDK-Linux/demo_ros/src目录。将下载好的放在该目录下,并对其改名,改为 ORB_SLAM
2. 进入ORB_SLAM2/Examples/Stereo目录下,修改stereo_euroc.cc文件。
1)将其橙色区域改成
代码如下:
#include#include#include#include#include#include#include#include "ros/ros.h"#include #include #include "std_msgs/String.h"#include "FileYaml.h"#include #include #include #include using namespace std;ros::Subscriber image_l_sub;ros::Subscriber image_r_sub; int image_l_count = 0;queue<:mat> que_image_l;queue<:mat> que_image_r;queue<long double> que_stamp; std::mutex limage_mutex;std::mutex rimage_mutex;std::condition_variable left_data_cond;std::condition_variable right_data_cond;// std::lock_guard<:mutex>lock(rimage_mutex);// std::thread show_thread{show_image}; //visualization thread// show_thread.detach(); cv::Mat imLeft;cv::Mat imRight;ros::Time ros_stamp;long double stamp;
修改后如下:
2)在代码,long double stamp;后添加时间转换函数
long double time_tranform(int64_t time){ //取后13位int b = time/1e13;int64_t temp = b * 1e13;int64_t c = time - temp; //小数点后9位long double d = c / 1e9;return d;}
修改后如下:
3)删除LoadImages函数
4)在time_tranform和main函数之间添加左图回调函数imagelCallback,添加右图回调函数imagerCallback,以及一些变量的定义
代码如下:
//image_l回调函数void imagelCallback(const sensor_msgs::ImageConstPtr&msg){ cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(msg, "mono8"); cv_ptr->image.copyTo(imLeft); image_l_count = cv_ptr->header.seq; ros_stamp = cv_ptr->header.stamp;std::lock_guard<:mutex> lock_l(limage_mutex); stamp = time_tranform(ros_stamp.toNSec());//cout< que_image_l.push(imLeft); que_stamp.push(stamp);}//image_r回调函数void imagerCallback(const sensor_msgs::ImageConstPtr&msg){ cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(msg, "mono8"); cv_ptr->image.copyTo(imRight);std::lock_guard<:mutex> lock_r(rimage_mutex); que_image_r.push(imRight);}Camera_Other_Parameter vecCamerasParam;cv::Mat M1l,M2l,M1r,M2r;cv::Mat data_left;cv::Mat data_right;long double frame_time;cv::Mat imLeftRect, imRightRect;
5)在cv::Mat imLeftRect, imRightRect;之后,添加show_ORB函数,用于获取图像数据和时间,以及启动ORB的功能
代码如下:
void show_ORB(){//-----------------ORB_SLAM2 Init---------------------------------------------const string param1_ORBvoc = "Vocabulary/ORBvoc.txt"; const string param3_ORByaml = "Examples/Stereo/EuRoC.yaml"; ORB_SLAM2::System SLAM(param1_ORBvoc,param3_ORByaml,ORB_SLAM2::System::STEREO,true);std::this_thread::sleep_for(std::chrono::milliseconds(500));while(true) {std::this_thread::sleep_for(std::chrono::milliseconds(30));std::unique_lock<:mutex> lock_l(limage_mutex); data_left = que_image_l.front(); frame_time = que_stamp.front(); que_image_l.pop(); que_stamp.pop(); lock_l.unlock();std::unique_lock<:mutex> lock_r(rimage_mutex); data_right = que_image_r.front(); que_image_r.pop(); lock_r.unlock();// cout.precision(13);// cout< cv::resize(data_left,data_left,cv::Size(vecCamerasParam.cols,vecCamerasParam.rows)); cv::resize(data_right,data_right,cv::Size(vecCamerasParam.cols,vecCamerasParam.rows)); cv::remap(data_left,imLeftRect,M1l,M2l,cv::INTER_LINEAR); cv::remap(data_right,imRightRect,M1r,M2r,cv::INTER_LINEAR);#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();#elsestd::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();#endif SLAM.TrackStereo(imLeftRect,imRightRect,frame_time);#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();#elsestd::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();#endifdouble ttrack= std::chrono::duration_cast<:chrono::duration> >(t2 - t1).count(); // Wait to load the next framedouble T = 0; T = que_stamp.front() - frame_time;if(ttrack < T) usleep((T-ttrack)*1e6); /* cv::imshow("left",data_left); cv::imshow("right",data_right); cv::waitKey(1);*/ }// Stop all threads SLAM.Shutdown();// Save camera trajectory SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
修改后如下:
6)将main函数内的内容全部删除,在main函数内添加ros初始化,读取配置文件,去畸变,获取参数,校正,ORB_SLAM的启动
代码如下:
ros::init(argc,argv,"ORB_SLAM2"); ros::NodeHandle n; image_l_sub = n.subscribe("/module/image_left",100,imagelCallback); image_r_sub = n.subscribe("/module/image_right", 100,imagerCallback); const char *param2_SDKyaml = "/home/indemind/u/SDK-Linux-ros/lib/1604/headset.yaml"; readConfig(param2_SDKyaml,vecCamerasParam);//-----------------fisheye rectify--------------------------------------------- cv::Mat Q; if(vecCamerasParam.K_l.empty() || vecCamerasParam.K_r.empty() || vecCamerasParam.P_l.empty() || vecCamerasParam.P_r.empty() || vecCamerasParam.R_l.empty() || vecCamerasParam.R_r.empty() || vecCamerasParam.D_l.empty() || vecCamerasParam.D_r.empty() || vecCamerasParam.rows==0 || vecCamerasParam.cols==0) {cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;return -1; } cv::fisheye::initUndistortRectifyMap(vecCamerasParam.K_l,vecCamerasParam.D_l,vecCamerasParam.R_l,vecCamerasParam.P_l.rowRange(0,3).colRange(0,3), cv::Size(vecCamerasParam.cols,vecCamerasParam.rows),CV_32FC1,M1l,M2l); cv::fisheye::initUndistortRectifyMap(vecCamerasParam.K_r,vecCamerasParam.D_r,vecCamerasParam.R_r,vecCamerasParam.P_r.rowRange(0,3).colRange(0,3), cv::Size(vecCamerasParam.cols,vecCamerasParam.rows),CV_32FC1,M1r,M2r);cout << "the P_l of initUndistortRectifyMap after" << endl;for(int i = 0;i < 3;++i)for(int j = 0;j < 3;++j) {double *ptr = vecCamerasParam.P_l.ptr(i,j);cout << *ptr< }cout << "the P_r of initUndistortRectifyMap after" << endl;for(int i = 0;i < 3;++i)for(int j = 0;j < 3;++j) {double *ptr = vecCamerasParam.P_r.ptr(i,j);cout << *ptr< } cv::stereoRectify(vecCamerasParam.K_l,vecCamerasParam.D_l,vecCamerasParam.K_r,vecCamerasParam.D_r,cv::Size(vecCamerasParam.cols,vecCamerasParam.rows), vecCamerasParam.R,vecCamerasParam.t,vecCamerasParam.R_l,vecCamerasParam.R_r,vecCamerasParam.P_l,vecCamerasParam.P_r,Q,cv::CALIB_ZERO_DISPARITY,0); //-----------------fisheye end---------------------------------------------std::thread show_thread{show_ORB}; //visualization thread show_thread.detach(); ros::spin(); return 0;
修改后如下:
7)将LoadImages函数删除掉
文件修改完成
3. 进入SDK-Linux/demo_ros/src/ORB_SLAM2/include目录下,新建FileYaml.h文件,将下列代码填入文件
#ifndef _FILEYAML_H#define _FILEYAML_H#include #include using namespace std;#include #include #include #include#include #include #include #include #include using namespace cv;using namespace Eigen;using namespace Sophus;using cv::FileStorage;using cv::FileNode;struct Camera{ Eigen::Matrix4d T_SC; Eigen::Vector2d imageDimension; Eigen::VectorXd distortionCoefficients; Eigen::Vector2d focalLength; Eigen::Vector2d principalPoint;std::string distortionType; Camera() : T_SC(Eigen::Matrix4d::Identity()), imageDimension(Eigen::Vector2d(1280, 800)), distortionCoefficients(Eigen::Vector2d(0, 0)), focalLength(Eigen::Vector2d(700, 700)), principalPoint(Eigen::Vector2d(640, 400)), distortionType("equidistant") { }void write(FileStorage& fs) const //Write serialization for this class{ fs << "{:"; fs << "T_SC"; fs << "[:";for (int i = 0; i < 4; i++) {for (int j = 0; j < 4; j++) { fs << T_SC(i, j); } } fs << "]"; fs << "image_dimension"; fs << "[:"; fs << imageDimension(0) << imageDimension(1); fs << "]"; fs << "distortion_coefficients"; fs << "[:";for (int i = 0; i < distortionCoefficients.rows(); i++) { fs << distortionCoefficients(i); } fs << "]"; fs << "distortion_type" << distortionType; fs << "focal_length"; fs << "[:"; fs << focalLength(0) << focalLength(1); fs << "]"; fs << "principal_point"; fs << "[:"; fs << principalPoint(0) << principalPoint(1); fs << "]"; fs << "}"; }void read(const FileNode& node) //Read serialization for this class{ cv::FileNode T_SC_node = node["T_SC"]; cv::FileNode imageDimensionNode = node["image_dimension"]; cv::FileNode distortionCoefficientNode = node["distortion_coefficients"]; cv::FileNode focalLengthNode = node["focal_length"]; cv::FileNode principalPointNode = node["principal_point"];// extrinsics T_SC << T_SC_node[0], T_SC_node[1], T_SC_node[2], T_SC_node[3], T_SC_node[4], T_SC_node[5], T_SC_node[6], T_SC_node[7], T_SC_node[8], T_SC_node[9], T_SC_node[10], T_SC_node[11], T_SC_node[12], T_SC_node[13], T_SC_node[14], T_SC_node[15]; imageDimension << imageDimensionNode[0], imageDimensionNode[1]; distortionCoefficients.resize(distortionCoefficientNode.size());for (size_t i = 0; i distortionCoefficients[i] = distortionCoefficientNode[i]; } focalLength << focalLengthNode[0], focalLengthNode[1]; principalPoint << principalPointNode[0], principalPointNode[1]; distortionType = (std::string)(node["distortion_type"]); }};struct Camera_Other_Parameter{ Mat K_l; Mat K_r; Mat D_l; Mat D_r; Mat R_l; Mat R_r; Mat P_l; Mat P_r; Mat R; Mat t;int cols; //图像宽int rows; //图像高};void readConfig(const char *yamlPath,Camera_Other_Parameter &vecCamerasOtherParam);#endif
注:该文件为读取文件的头文件
4. 进入SDK-Linux/demo_ros/src/ORB_SLAM2/src目录下,新建FileYaml.cc文件,将下列代码填入文件:
#include "FileYaml.h"static void write(FileStorage& fs, const std::string&, const Camera& x){ x.write(fs);}static void read(const FileNode& node, Camera& x, const Camera& default_value = Camera()){if (node.empty()) x = default_value;else x.read(node);}void readConfig(const char *yamlPath,Camera_Other_Parameter &vecCamerasOtherParam){std::vector vecCameras; cv::FileStorage fin(yamlPath, cv::FileStorage::READ);if(!fin.isOpened()) {cerr << endl << "Failed to load readConfig yamlPath " << endl;return ; } cv::FileNode cameras_node = fin["cameras"];/* cv::FileNode Rl_node = fin["Rl"]; cv::FileNode Rr_node = fin["Rr"]; cv::FileNode Pl_node = fin["Pl"]; cv::FileNode Pr_node = fin["Pr"]; cv::FileNode Kl_node = fin["Kl"]; cv::FileNode Kr_node = fin["Kr"]; cv::FileNode Dl_node = fin["Dl"]; cv::FileNode Dr_node = fin["Dr"];*/ fin["Rl"] >> vecCamerasOtherParam.R_l; fin["Rr"] >> vecCamerasOtherParam.R_r; fin["Pl"] >> vecCamerasOtherParam.P_l; fin["Pr"] >> vecCamerasOtherParam.P_r; fin["Kl"] >> vecCamerasOtherParam.K_l; fin["Kr"] >> vecCamerasOtherParam.K_r; fin["Dl"] >> vecCamerasOtherParam.D_l; fin["Dr"] >> vecCamerasOtherParam.D_r;/* vecCamerasOtherParam.R_l = Rl_node.mat(); vecCamerasOtherParam.R_r = Rr_node.mat(); vecCamerasOtherParam.P_l = Pl_node.mat(); vecCamerasOtherParam.P_r = Pr_node.mat(); vecCamerasOtherParam.K_l = Kl_node.mat(); vecCamerasOtherParam.K_r = Kr_node.mat(); vecCamerasOtherParam.D_l = Dl_node.mat(); vecCamerasOtherParam.D_r = Dr_node.mat();*/for (cv::FileNodeIterator it = cameras_node.begin(); it != cameras_node.end(); it++) { Camera camera; (*it) >> camera; vecCameras.push_back(camera); }//obtain col & row vecCamerasOtherParam.cols = vecCameras[0].imageDimension(0); vecCamerasOtherParam.rows = vecCameras[0].imageDimension(1);//obtain R & t Eigen::Matrix4d T_SC_left; Eigen::Matrix4d T_SC_right; T_SC_left = vecCameras[0].T_SC; T_SC_right = vecCameras[1].T_SC;SE3 T_SC_l(T_SC_left.topLeftCorner(3,3),T_SC_left.topRightCorner(3,1));SE3 T_SC_r(T_SC_right.topLeftCorner(3,3),T_SC_right.topRightCorner(3,1)); SE3 Tcl_cr = T_SC_l.inverse()*T_SC_r; SE3 Tcr_cl = T_SC_r.inverse()*T_SC_l; Matrix3d R = Tcr_cl.rotation_matrix(); Vector3d t = Tcr_cl.translation(); //Eigen tranfer to arraydouble * R_ptr= new double[R.size()];double *t_ptr = new double[t.size()]; Map(R_ptr, R.rows(), R.cols()) = R; Map(t_ptr, t.rows(), t.cols()) = t;cout<double R_matrix[3][3];for(int i = 0;i < 3;i++)for(int j = 0;j<3;j++) {//transpose R_matrix[j][i] = R_ptr[i+j*3];cout< } cout<double t_matrix[3];for(int i = 0;i < 3;i++) { t_matrix[i] = t_ptr[i];cout< } vecCamerasOtherParam.R = cv::Mat(3,3,CV_64FC1,R_matrix); vecCamerasOtherParam.t = cv::Mat(3,1,CV_64FC1,t_matrix);}
注:该文件为读取文件的源文件
5. 修改CMakeLists.txt
进入SDK-Linux/demo_ros/src目录下,修改CMakeLists.txt文件
将ORB中的CMakeLists.txt添加到SDK的CMakeLists.txt下,修改如下
1)
在add_compile_options(-std=c++11)后面添加
IF(NOT CMAKE_BUILD_TYPE)SET(CMAKE_BUILD_TYPE Release)ENDIF()MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")# Check C++11 or C++0x supportinclude(CheckCXXCompilerFlag)CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)if(COMPILER_SUPPORTS_CXX11)set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-DCOMPILEDWITHC11) message(STATUS "Using flag -std=c++11.")elseif(COMPILER_SUPPORTS_CXX0X)set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") add_definitions(-DCOMPILEDWITHC0X) message(STATUS "Using flag -std=c++0x.")else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")endif()LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)find_package(OpenCV 3.0 QUIET)if(NOT OpenCV_FOUND) find_package(OpenCV 2.4.3 QUIET)if(NOT OpenCV_FOUND) message(FATAL_ERROR "OpenCV > 2.4.3 not found.") endif()endif()#find_package(Eigen3 3.1.0 REQUIRED)find_package(Pangolin REQUIRED)
修改后如下:
2)
在橙色区域后面添加
#Eigeninclude_directories("/usr/include/eigen3")#Sophusfind_package(Sophus REQUIRED)include_directories( ${Sophus_INCLUDE_DIRS})include_directories(${PROJECT_SOURCE_DIR}${PROJECT_SOURCE_DIR}/ORB_SLAM/include${PROJECT_SOURCE_DIR}/ORB_SLAM#${EIGEN3_INCLUDE_DIR}${Pangolin_INCLUDE_DIRS})set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
3)
对橙色区域进行修改
add_library(${PROJECT_NAME} SHAREDORB_SLAM/src/FileYaml.ccORB_SLAM/src/System.ccORB_SLAM/src/Tracking.ccORB_SLAM/src/LocalMapping.ccORB_SLAM/src/LoopClosing.ccORB_SLAM/src/ORBextractor.ccORB_SLAM/src/ORBmatcher.ccORB_SLAM/src/FrameDrawer.ccORB_SLAM/src/Converter.ccORB_SLAM/src/MapPoint.ccORB_SLAM/src/KeyFrame.ccORB_SLAM/src/Map.ccORB_SLAM/src/MapDrawer.ccORB_SLAM/src/Optimizer.ccORB_SLAM/src/PnPsolver.ccORB_SLAM/src/Frame.ccORB_SLAM/src/KeyFrameDatabase.ccORB_SLAM/src/Sim3Solver.ccORB_SLAM/src/Initializer.ccORB_SLAM/src/Viewer.cc
4)
将其修改为
target_link_libraries(${PROJECT_NAME}${OpenCV_LIBS}#${EIGEN3_LIBS}${Pangolin_LIBRARIES}${Sophus_LIBRARIES}${PROJECT_SOURCE_DIR}/ORB_SLAM/Thirdparty/DBoW2/lib/libDBoW2.so${PROJECT_SOURCE_DIR}/ORB_SLAM/Thirdparty/g2o/lib/libg2o.so)
5)
在其橙色区域添加
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/ORB_SLAM/Examples/RGB-D)add_executable(rgbd_tumORB_SLAM/Examples/RGB-D/rgbd_tum.cc)target_link_libraries(rgbd_tum ${PROJECT_NAME}${Sophus_LIBRARIES})set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/ORB_SLAM/Examples/Stereo)add_executable(stereo_kittiORB_SLAM/Examples/Stereo/stereo_kitti.cc)target_link_libraries(stereo_kitti ${PROJECT_NAME}${Sophus_LIBRARIES})add_executable(stereo_eurocORB_SLAM/Examples/Stereo/stereo_euroc.cc)add_dependencies(stereo_euroc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})target_link_libraries(stereo_euroc ${PROJECT_NAME}${Sophus_LIBRARIES}${catkin_LIBRARIES})add_executable(module_driver src/camera_driver.cpp)add_dependencies(module_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})target_link_libraries(module_driver${PROJECT_SOURCE_DIR}/../../lib/1604/libindem.so${catkin_LIBRARIES}${PROJECT_SOURCE_DIR}/../../lib/1604/libboost_filesystem.so.1.58.0${PROJECT_SOURCE_DIR}/../../lib/1604/libboost_system.so.1.58.0${PROJECT_SOURCE_DIR}/../../lib/1604/libg3logger.so.1.3.0-0${PROJECT_SOURCE_DIR}/../../lib/1604/libnanomsg.so.5${PROJECT_SOURCE_DIR}/../../lib/1604/libopencv_core.so.3.4${PROJECT_SOURCE_DIR}/../../lib/1604/libopencv_imgproc.so.3.4${PROJECT_SOURCE_DIR}/../../lib/1604/libopencv_videoio.so.3.4 pthread stdc++fs dl)set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/ORB_SLAM/Examples/Monocular)add_executable(mono_tumORB_SLAM/Examples/Monocular/mono_tum.cc)target_link_libraries(mono_tum${Sophus_LIBRARIES}${PROJECT_NAME})add_executable(mono_kittiORB_SLAM/Examples/Monocular/mono_kitti.cc)target_link_libraries(mono_kitti${Sophus_LIBRARIES}${PROJECT_NAME})add_executable(mono_eurocORB_SLAM/Examples/Monocular/mono_euroc.cc)target_link_libraries(mono_euroc${Sophus_LIBRARIES}${PROJECT_NAME})
CMakeLists.txt修改完毕
6. 编译
1)安装SDK,教程请查看:开讲啦丨双目惯性模组运行ORB-SLAM算法示例
进入SDK-Linux/demo_ros目录下,执行catkin_make命令,结果如下
7. 执行
1)新打开一个shell,执行roscore命令,如下图
2- 进入SDK-Linux/lib/1604目录下,执行
sudo -s,sudo chmod 777 ./run.sh
之后执行
./run.sh
如下图
3)进入SDK-Linux/demo_ros/src/ORB_SLAM目录下,执行./Examples/Stereo/stereo_euroc命令
注意:此时,将获得去畸变之后的参数,把下列参数写入到SDK-Linux/demo_ros/src/ORB_SLAM/src/Tracking.cc文件中,如下图
float fx = 597.935;float fy = 597.935;float cx = 476.987;float cy = 442.158;
最后,再次编译,并执行,即可得到实时ORB
教程至此结束,Demo如下:
学 生 福 利 集
高校大使推荐同学购买可获得奖励金,购买同学福利多多,点击图片了解详情
提交相关信息,即可获得100元院校优惠券,点击图片了解详情
提交相关信息,即可享受15天免费试用,试用期购买享大额优惠,点击图片了解详情