本机系统：Jetpack5.0.2 Ubuntu 20.04 LTS
注意事项：想要避坑，务必按照文中版本准备各种环境
一、安装软件
1.Pangolin 0.5
网址：https://github.com/stevenlovegrove/Pangolin
安装命令：
1
2 sudo apt install libglew-dev
3 sudo apt install cmake
4 sudo apt install libpython2.7-dev
5
6
7 git clone https://github.com/stevenlovegrove/Pangolin.git
8 cd Pangolin
9 mkdir build
10 cd build
11 cmake ..
12 cmake --build .
13
14 sudo make install
可能出现的报错1：
error: ‘AV_PIX_FMT_XVMC_MPEG2_MC’ was not declared in this scope
解决方案：在/Pangolin/CMakeModules/FindFFMPEG.cmake中63,64行
sizeof(AVFormatContext::max_analyze_duration);
}" HAVE_FFMPEG_MAX_ANALYZE_DURATION2
替换成
sizeof(AVFormatContext::max_analyze_duration);
}" HAVE_FFMPEG_MAX_ANALYZE_DURATION
/Pangolin/src/video/drivers/ffmpeg.cpp中第37行 namespace pangolin上面加上
#define CODEC_FLAG_GLOBAL_HEADER AV_CODEC_FLAG_GLOBAL_HEADER
第78,79行
TEST_PIX_FMT_RETURN(XVMC_MPEG2_MC);
TEST_PIX_FMT_RETURN(XVMC_MPEG2_IDCT);
替换成
#ifdef FF_API_XVMC
TEST_PIX_FMT_RETURN(XVMC_MPEG2_MC);
TEST_PIX_FMT_RETURN(XVMC_MPEG2_IDCT);
#endif
101-105行
TEST_PIX_FMT_RETURN(VDPAU_H264);
TEST_PIX_FMT_RETURN(VDPAU_MPEG1);
TEST_PIX_FMT_RETURN(VDPAU_MPEG2);
TEST_PIX_FMT_RETURN(VDPAU_WMV3);
TEST_PIX_FMT_RETURN(VDPAU_VC1);
替换成
#ifdef FF_API_VDPAU
TEST_PIX_FMT_RETURN(VDPAU_H264);
TEST_PIX_FMT_RETURN(VDPAU_MPEG1);
TEST_PIX_FMT_RETURN(VDPAU_MPEG2);
TEST_PIX_FMT_RETURN(VDPAU_WMV3);
TEST_PIX_FMT_RETURN(VDPAU_VC1);
#endif
127行
TEST_PIX_FMT_RETURN(VDPAU_MPEG4);
替换成
#ifdef FF_API_VDPAU
TEST_PIX_FMT_RETURN(VDPAU_MPEG4);
#endif
可能出现的报错2：
error: ‘AVFMT_RAWPICTURE’ was not declared in this scope
解决方案：在Pangolin/include/pangolin/video/drivers/ffmpeg.h开头加
#define AV_CODEC_FLAG_GLOBAL_HEADER (1 << 22)
#define CODEC_FLAG_GLOBAL_HEADER AV_CODEC_FLAG_GLOBAL_HEADER
#define AVFMT_RAWPICTURE 0x0020
2.OpenCV 4.2.0
由于Jetpack5.0.2系统自带OpenCV 4.5.5，所以首先我们要把OpenCV 4.5.5卸载
因为OpenCV 4.5.5不是我们安装的，所以一般网上教的卸载方法没有用，个人尝试后最好用的是将/usr中所有OpenCV的相关项删除：
whereis opencv4
使用上述命令后终端会显示OpenCV的安装路径，进入对应的路径删除OpenCV相关项
sudo rm -rf
sudo rm
删除所有相关项后使用pkg-config --modversion opencv命令查询
对于OpenCV 4.2.0版本推荐使用python安装
sudo apt update
sudo apt install libopencv-dev python3-opencv
安装完成后使用pkg-config --modversion opencv查询
4.2.0
3.ROS2(foxy)
推荐使用鱼香ROS一键安装指令(真的无敌好用)
wget http://fishros.com/install -O fishros && . fishros
回车后按照指引选择就可以了
4.ORB-SLAM3(for Ubuntu20.04)
代码地址：https://github.com/zang09/ORB-SLAM3-STEREO-FIXED
git clone https://github.com/zang09/ORB-SLAM3-STEREO-FIXED.git ORB_SLAM3
cd ORB_SLAM3
chmod +x build.sh
./build.sh
安装的时间可能会比较久一点，对于我们使用而言，可以找到/ORB_SLAM3路径下的Cmakelist.txt文件将所有example的编译全部注释掉
5.ORB-SLAM3-ROS2
代码地址：https://github.com/zang09/ORB_SLAM3_ROS2
mkdir -p orbslam_ws/src
cd ~/orbslam_ws/src
git clone https://github.com/zang09/ORB_SLAM3_ROS2.git orbslam3_ros2
将/orbslam3_ros2中Cmakelist.txt的"/opt/ros/foxy/lib/python3.8/site-packages/"替换成你自己的路径，一般就是这个，所以不用换
# You should set the PYTHONPATH to your own python site-packages path
set(ENV{PYTHONPATH} "/opt/ros/foxy/lib/python3.8/site-packages/")
将/orbslam3_ros2/CMakeModules/FindORB_SLAM3.cmake中的"~/Install/ORB_SLAM/ORB_SLAM3"替换成你步骤4中ORB-SLAM3文件夹的路径
set(ORB_SLAM3_ROOT_DIR "~/Install/ORB_SLAM/ORB_SLAM3")
然后开始编译
cd ~/orbslam
colcon build --symlink-install --packages-select orbslam3
可能出现的问题：找不到sophus/se3.hpp
解决方案：回到你步骤4中ORB-SLAM3文件夹的路径下，进入/Thirdparty/Sophus/build
cd ~/{ORB_SLAM3_ROOT_DIR}/Thirdparty/Sophus/build
sudo make install
二、安装硬件驱动
本文使用zed2相机作为视觉传感器设备
1.安装zed sdk
SDK下载地址：https://www.stereolabs.com/developers/release/
sudo chmod +x ZED_SDK_Tegra_L4T35.1_v3.7.7.run
./ZED_SDK_Tegra_L4T35.1_v3.7.7.run
验证安装是否成功
cd /usr/local/zed/tools
./ZED_Depth_Viewer
如果能看到图像则代表安装成功，在我测试过程中之前是可以的，后来又不行了
如果你也是这样不要慌，运行下面的软件，如果可以也代表可以
./ZED_explorer
2.安装zed-ros2-wrappercolcon build 包
地址：https://github.com/stereolabs/zed-ros2-wrapper
mkdir -p zed_ws/src
cd zed_ws/src
git clone --recursive https://github.com/stereolabs/zed-ros2-wrapper.git
git clone https://github.com/ros-perception/image_common.git --branch 3.0.0
cd ..
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
echo source $(pwd)/install/local_setup.bash >> ~/.bashrc
source ~/.bashrc
sudo apt remove ros-foxy-image-transport-plugins ros-foxy-compressed-depth-image-transport ros-foxy-compressed-image-transport
官方说可能会有的问题：If "rosdep" is missing you can install it with:
sudo apt-get install python-rosdep python-rosinstall-generator python-vcstool python-rosinstall build-essential
反正我是没有碰到这个问题
还有就是所有命令执行完毕后须重启终端才能应用改变
三、使用zed2跑自己的ORB-SLAM3
1.启动ZED node
ZED：
ros2 launch zed_wrapper zed.launch.py
ZED Mini：
ros2 launch zed_wrapper zedm.launch.py
ZED2：
ros2 launch zed_wrapper zed2.launch.py
ZED2i：
ros2 launch zed_wrapper zed2i.launch.py
反正我只试了zed2，你们用其他设备的应该也一样，问题不大
2.更改ORB-SLAM3-ROS2中订阅的话题名
ros2 topic list
然后将ORB-SLAM3-ROS2源码中的订阅话题名改为zed发布的话题名(这里提供的是双目-惯性模式的例子，其他的也一样)
subImu_ = this->create_subscription<ImuMsg>("zed2/zed_node/imu/data_raw", 1000, std::bind(&StereoInertialNode::GrabImu, this, _1));
subImgLeft_ = this->create_subscription<ImageMsg>("zed2/zed_node/left_raw/image_raw_color", 100, std::bind(&StereoInertialNode::GrabImageLeft, this, _1));
subImgRight_ = this->create_subscription<ImageMsg>("zed2/zed_node/right_raw/image_raw_color", 100, std::bind(&StereoInertialNode::GrabImageRight, this, _1));
修改完了之后记得
cd ~/orbslam3_ws
colcon build --symlink-install
接着开始准备相机的配置文件zed2.yaml，这里给出的是zed2的！(该配置文件可以成功运行mono、stereo模式，stereo-inertial模式还在测试中)
记住了，不同相机的配置文件不通用
%YAML:1.0
Camera.type: "PinHole"
Camera.fx: 524.650390625
Camera.fy: 524.650390625
Camera.cx: 636.3681030273438
Camera.cy: 359.5227966308594
Camera.k1: 0.0
Camera.k2: 0.0
Camera.k3: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.bFishEye: 0
Camera.width: 1280
Camera.height: 720
Camera.fps: 10.0
Camera.bf: 62.92245016
Camera.RGB: 1
ThDepth: 45.0
DepthMapFactor: 2500.0
Tbc: !!opencv-matrix
rows: 4
cols: 4
dt: f
data: [ 0.00486182, -0.00410027, 0.99997978, 0.02667628,
-0.99997499, 0.00511639, 0.00488277, 0.02439728,
-0.00513631, -0.9999785, -0.00407529, 0.0001096,
0.0, 0.0, 0.0, 1.0, ]
IMU.NoiseGyro: 0.007000000216066837
IMU.NoiseAcc: 0.016
IMU.GyroWalk: 0.0019474000437185168
IMU.AccWalk: 0.0002508999896235764
IMU.Frequency: 100
ORBextractor.nFeatures: 1000
ORBextractor.scaleFactor: 1.2
ORBextractor.nLevels: 8
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
PointCloudMapping.Resolution: 0.01
完事后开启ORB-SLAM3-ROS2
source ~/colcon_ws/install/local_setup.bash
MONO mode
ros2 run orbslam3 mono PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE
STEREO mode
ros2 run orbslam3 stereo PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY
RGBD mode(这个我没试，有兴趣的可以自己去试试)
ros2 run orbslam3 rgbd PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE
STEREO-INERTIAL mode
ros2 run orbslam3 stereo-inertial PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY [BOOL_EQUALIZE]
其中：
PATH_TO_VOCABULARY 在 orbslam3_ros2/vocabulary/ORBvoc.txt 路径下
PATH_TO_YAML_CONFIG_FILE 就是上文准备的zed2.yaml的路径
注：本文集网络各家之精华，引用了不少博客内容，具体已经分不清，所以在这就不做引用标注了
9月28日补充：
STEREO-INERTIAL mode 的源代码未对真实硬件(如zed2相机)输出的左右两目的图像数据做时间对齐，导致使用zed2相机发布的数据运行ORB-SLAM3一直报"big time difference"的错误，而后直接退出。
想要解决问题，应该将stereo-inertial-node.cpp、stereo-inertial-node.hpp、Cmakelist.txt的代码进行相应的修改
stereo-inertial-node.cpp
#include "stereo-inertial-node1.hpp"
#include <opencv2/core/core.hpp>
using std::placeholders::_1;
StereoInertialNode::StereoInertialNode(ORB_SLAM3::System *SLAM, const string &strSettingsFile, const string &strDoRectify, const string &strDoEqual) :
Node("ORB_SLAM3_ROS2"),
SLAM_(SLAM)
{
stringstream ss_rec(strDoRectify);
ss_rec >> boolalpha >> doRectify_;
stringstream ss_eq(strDoEqual);
ss_eq >> boolalpha >> doEqual_;
bClahe_ = doEqual_;
std::cout << "Rectify: " << doRectify_ << std::endl;
std::cout << "Equal: " << doEqual_ << std::endl;
if (doRectify_)
{
cv::FileStorage fsSettings(strSettingsFile, cv::FileStorage::READ);
if (!fsSettings.isOpened())
{
cerr << "ERROR: Wrong path to settings" << endl;
assert(0);
}
cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
fsSettings["LEFT.K"] >> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0)
{
cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
assert(0);
}
cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), cv::Size(cols_l, rows_l), CV_32F, M1l_, M2l_);
cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), cv::Size(cols_r, rows_r), CV_32F, M1r_, M2r_);
}
subImu_ = this->create_subscription<ImuMsg>("imu", 1000, std::bind(&StereoInertialNode::GrabImu, this, _1));
subImgLeft_ = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/left");
subImgRight_ = std::make_shared<message_filters::Subscriber<ImageMsg> >(shared_ptr<rclcpp::Node>(this), "camera/right");
syncApproximate = std::make_shared<message_filters::Synchronizer<approximate_sync_policy> >(approximate_sync_policy(10), *subImgLeft_, *subImgRight_);
syncApproximate->registerCallback(&StereoInertialNode::GrabImage, this);
syncThread_ = new std::thread(&StereoInertialNode::SyncWithImu, this);
}
StereoInertialNode::~StereoInertialNode()
{
syncThread_->join();
delete syncThread_;
SLAM_->Shutdown();
SLAM_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
}
void StereoInertialNode::GrabImu(const ImuMsg::SharedPtr msg)
{
bufMutex_.lock();
imuBuf_.push(msg);
bufMutex_.unlock();
}
void StereoInertialNode::GrabImage(const ImageMsg::SharedPtr msgLeft, const ImageMsg::SharedPtr msgRight)
{
bufMutexLeft_.lock();
if (!imgLeftBuf_.empty())
imgLeftBuf_.pop();
imgLeftBuf_.push(msgLeft);
bufMutexLeft_.unlock();
bufMutexRight_.lock();
if (!imgRightBuf_.empty())
imgRightBuf_.pop();
imgRightBuf_.push(msgRight);
bufMutexRight_.unlock();
}
cv::Mat StereoInertialNode::GetImage(const ImageMsg::SharedPtr msg)
{
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception &e)
{
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
}
if (cv_ptr->image.type() == 0)
{
return cv_ptr->image.clone();
}
else
{
std::cerr << "Error image type" << std::endl;
return cv_ptr->image.clone();
}
}
void StereoInertialNode::SyncWithImu()
{
while (1)
{
cv::Mat imLeft, imRight;
double tImLeft = 0, tImRight = 0;
if (!imgLeftBuf_.empty() && !imgRightBuf_.empty() && !imuBuf_.empty())
{
tImLeft = Utility::StampToSec(imgLeftBuf_.front()->header.stamp);
tImRight = Utility::StampToSec(imgRightBuf_.front()->header.stamp);
if (tImLeft > Utility::StampToSec(imuBuf_.back()->header.stamp))
continue;
bufMutexLeft_.lock();
imLeft = GetImage(imgLeftBuf_.front());
imgLeftBuf_.pop();
bufMutexLeft_.unlock();
bufMutexRight_.lock();
imRight = GetImage(imgRightBuf_.front());
imgRightBuf_.pop();
bufMutexRight_.unlock();
vector<ORB_SLAM3::IMU::Point> vImuMeas;
bufMutex_.lock();
if (!imuBuf_.empty())
{
vImuMeas.clear();
while (!imuBuf_.empty() && Utility::StampToSec(imuBuf_.front()->header.stamp) <= tImLeft)
{
double t = Utility::StampToSec(imuBuf_.front()->header.stamp);
cv::Point3f acc(imuBuf_.front()->linear_acceleration.x, imuBuf_.front()->linear_acceleration.y, imuBuf_.front()->linear_acceleration.z);
cv::Point3f gyr(imuBuf_.front()->angular_velocity.x, imuBuf_.front()->angular_velocity.y, imuBuf_.front()->angular_velocity.z);
vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t));
imuBuf_.pop();
}
}
bufMutex_.unlock();
if (bClahe_)
{
clahe_->apply(imLeft, imLeft);
clahe_->apply(imRight, imRight);
}
if (doRectify_)
{
cv::remap(imLeft, imLeft, M1l_, M2l_, cv::INTER_LINEAR);
cv::remap(imRight, imRight, M1r_, M2r_, cv::INTER_LINEAR);
}
SLAM_->TrackStereo(imLeft, imRight, tImLeft, vImuMeas);
std::chrono::milliseconds tSleep(1);
std::this_thread::sleep_for(tSleep);
}
}
}
stereo-inertial-node.hpp
#ifndef __STEREO_INERTIAL_NODE_HPP__
#define __STEREO_INERTIAL_NODE_HPP__
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include <cv_bridge/cv_bridge.h>
#include "System.h"
#include "Frame.h"
#include "Map.h"
#include "Tracking.h"
#include "utility.hpp"
using ImuMsg = sensor_msgs::msg::Imu;
using ImageMsg = sensor_msgs::msg::Image;
class StereoInertialNode : public rclcpp::Node
{
public:
StereoInertialNode(ORB_SLAM3::System* pSLAM, const string &strSettingsFile, const string &strDoRectify, const string &strDoEqual);
~StereoInertialNode();
private:
void GrabImu(const ImuMsg::SharedPtr msg);
cv::Mat GetImage(const ImageMsg::SharedPtr msg);
void SyncWithImu();
void GrabImage(const ImageMsg::SharedPtr msgLeft, const ImageMsg::SharedPtr msgRight);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> approximate_sync_policy;
rclcpp::Subscription<ImuMsg>::SharedPtr subImu_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > subImgLeft_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > subImgRight_;
std::shared_ptr<message_filters::Synchronizer<approximate_sync_policy> > syncApproximate;
ORB_SLAM3::System *SLAM_;
std::thread *syncThread_;
queue<ImuMsg::SharedPtr> imuBuf_;
std::mutex bufMutex_;
queue<ImageMsg::SharedPtr> imgLeftBuf_, imgRightBuf_;
std::mutex bufMutexLeft_, bufMutexRight_;
bool doRectify_;
bool doEqual_;
cv::Mat M1l_, M2l_, M1r_, M2r_;
bool bClahe_;
cv::Ptr<cv::CLAHE> clahe_ = cv::createCLAHE(3.0, cv::Size(8, 8));
};
#endif
Cmakelist.txt
cmake_minimum_required(VERSION 3.5)
project(orbslam3)
# You should set the PYTHONPATH to your own python site-packages path
set(ENV{PYTHONPATH} "/opt/ros/foxy/lib/python3.8/site-packages/")
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(message_filters REQUIRED)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
find_package(ORB_SLAM3 REQUIRED)
include_directories(
include
${ORB_SLAM3_ROOT_DIR}/include
${ORB_SLAM3_ROOT_DIR}/include/CameraModels
)
link_directories(
include
)
add_executable(mono
src/monocular/mono.cpp
src/monocular/monocular-slam-node.cpp
)
ament_target_dependencies(mono rclcpp sensor_msgs cv_bridge ORB_SLAM3 Pangolin)
add_executable(rgbd
src/rgbd/rgbd.cpp
src/rgbd/rgbd-slam-node.cpp
)
ament_target_dependencies(rgbd rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM3 Pangolin)
add_executable(stereo
src/stereo/stereo.cpp
src/stereo/stereo-slam-node.cpp
)
ament_target_dependencies(stereo rclcpp sensor_msgs cv_bridge message_filters
ORB_SLAM3 Pangolin)
add_executable(stereo-inertial
src/stereo-inertial/stereo-inertial.cpp
src/stereo-inertial/stereo-inertial-node.cpp
)
ament_target_dependencies(stereo-inertial rclcpp sensor_msgs cv_bridge message_filters ORB_SLAM3 Pangolin)
install(TARGETS mono rgbd stereo stereo-inertial
DESTINATION lib/${PROJECT_NAME})
# Install launch files.
#install(DIRECTORY launch config vocabulary
# DESTINATION share/${PROJECT_NAME}/)
ament_package()
修改完再
cd ~/orbslam3_ws
colcon build --symlink-install
就可以运行 stereo-inertial 模式了
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
