Windows 端 ROSless ORB-SLAM3 的移植

Windows 端 ROSless ORB-SLAM3 的移植

最近项目上有对放射源进行三维扫描与辐射场建模的需求,而必要的能谱分析软件是基于 Windows 端开发的,故需要在 Win 端实现 SLAM 建图。目前实际应用中 ORB-SLAM 为效果最好的 基于特征点的视觉 SLAM 系统,而其 v3 版本相较于 v2 版本增加了使用 IMU 与视觉里程计融合的功能并提高了运算速度,因此便选择使用 ORB-SLAM3 实现功能,同时准备把其基于 Linux gcc 平台实现的库移植到 Win MSVC 平台上。

GitHub 上可以搜到一些移植完成的项目,基本上都是 v2 版本,极少数的 ORB-SLAM3 使用的都是 v0.x rc 版本。而且就算这样也都没有一个移植过程的记录,展示下 进行摄像头在线联调 的结果。是没调出来嘛还是不愿意分享呢,这我就不知道了🙁。

考虑到项目有稳定与安全性方面的要求,我最终选择 ORB-SLAM3 使用最新版本、所有依赖库均使用官方 Release 的方式来构建项目。构建完成的项目会放到 GitHub 上,但还是建议过一遍本文的流程,了解移植时在什么地方做了什么修改有什么坑,触类旁通的同时还能便于以后在 ORB-SLAM3 库更新版本时及时跟上。

移植前准备

工具链

构建工具链大版本一致就不会有什么问题,但我还是把我的版本放在这里:

  • CMake v3.31.4
  • MSVC 2022 v14.43.34808 on Windows 11

依赖库与驱动

ORB-SLAM3 有着众多的依赖包。除了 Repo 里 Thirdparty 文件夹中自带的魔改过的 DBoW2、g20 和 Sophus 外,其它的均需要手动下载编译。这里先列个清单,附上我在编译时实际使用的参考版本及下载地址:

示例数据集

由于我使用的是 Orbbec Astra 2,这是一个奥比中光推出的 带 IMU 的单目结构光深度相机,因此选择的示例数据为 RGB-D 类型。
这是 RGB-D 数据集下载地址:cvg.cit.tum.de/data/datasets/rgbd-dataset/download。由于 ORB-SLAM3 Repo 中的 Examples 文件夹自带有 associations 文件(关联 RGB 与 Depth 图片的 txt 文件),因此不用再下载 associate.py 脚本来生成。

其它类型(单目、双目等)可在这篇知乎专栏中找到:https://zhuanlan.zhihu.com/p/625605417

ORB-SLAM3 库 构建流程

unistd.h

unistd.h 文件在 Unix 类系统中的作用类似于 Windows 中的 Windows.h 文件。从 Linux 移植到 Windows 平台的程序大多需要手动创建一个,避免编译时出现 无法打开源文件 的错误。

在 msvc include 文件夹(...\Microsoft Visual Studio\2022\Community\VC\Tools\MSVC\14.43.34808\include)中添加一个 unistd.h 文件:

/** This file is part of the Mingw32 package.
unistd.h maps (roughly) to io.h
*/
#ifndef _UNISTD_H
#define _UNISTD_H
#include <io.h>
#include <process.h>
#endif /* _UNISTD_H */

注意⚠️:文件末尾需留有一行空行,否则编译时会报 Unexpected end of file 错误。

P.S. MSVC 工具链升级更新后 Visual Studio 会在原版本工具链的文件夹下保留该文件,需要手动移动至新版本工具链的 include 目录中。

OpenCV 安装

直接下载后解压安装到 Thirdparty 子文件夹即可。

  • OpenCV_DIR 目录为 .\build

Win32OpenSSL 安装

ORB-SLAM3 使用到了 md5 相关函数。如果缺少该库,在其编译时会出现 无法打开包括文件: “openssl/md5.h”: No such file or directory 的错误。

直接下载后安装到 C:\Program Files 即可。

  • include 目录为 .\include
  • lib 目录为 .\lib\VC\x64\MT

OpenNI2 安装

OpenNI2 是 Pangolin 和 ORB-SLAM3 的依赖,因其包含驱动,故直接下载安装到 C:\Program Files 即可。

Boost 编译

v1.87.0 版本的 Boost 可直接双击 bootstrap.bat 脚本即可生成 Boost 专用的构建工具 b2.exe(Boost-Build)。
然后使用下面的命令来生成 debug/release 版本的动态与静态库:

.\b2.exe stage --stagedir=".\stage" link=static runtime-link=shared runtime-link=static threading=multi debug release
  • Boost_DIR 目录为 .\stage\lib

若在链接时不能指定正确的链接库,则会在构建 ORB-SLAM3 和 Demo 时报大量的未定义符号错误;因此这里要介绍一下 Boost 编译得到的链接库命名规则:

libboost_serialization-vc143-mt-sgd-x64-1_87.lib
---  b   -------------   d   --  f  ---  h
 a -----       c       ----- e  ---  g  ----
  • a: 静态库以 lib 开头,动态库则无此前缀
  • b: boost 库前缀
  • c: 库子类名,serialization 库
  • d: 编译器版本,visual c++ 14.3
  • e: mt=multi-threaded,单线程则没有此段;可使用 BOOST_LIB_THREAD_OP 参数指定
  • f: s=static 静态链接,gd=debug 构建类型为调试; 可使用 BOOST_LIB_RT_OPT 参数指定
  • g: 平台架构名 x86;可使用 BOOST_LIB_ARCH_AND_MODEL_OPT 参数指定
  • h: Boost 版本 v1.87;可使用 BOOST_LIB_VERSION 参数指定

DBoW2 编译

从这个依赖开始,下面的都需要使用 CMake GUI 来进行项目的生成和编译了。
步骤大致都相同,这里先详细列出,后面基本相同的地方便不再赘述:

  1. 在待编译项目的根目录下创建一个 build 文件夹
  2. 打开 CMake GUI,Where is the source code 填写 CMakeLists.txt 所在位置(都是根目录),Where to build the binaries 填写刚创建的 build 文件夹位置
  3. 点击 Configure 后,在弹出的对话框中选择使用的 MSVC 编译器版本,多次点击 Configure 和填写 CMakeLists 中空缺的变量值(大多都是类似 OpenCV / Boost 等依赖库所在的位置),DBoW2 需要填写 OpenCV CMake 文件所在路径:
  1. 填写完依赖路径之后,填写 CMAKE_INSTALL_PREFIX.\build\install,以避免部分未指定输出路径变量的库直接将构建产物安装在系统路径 program files 下而混淆可能存在的其它版本:
  1. 再次点击 Configure 确认不再有错误后,点击 Generate 以检查并生成项目。若在生成时出现 Invalid escape sequence \U 的错误,可将路径中的反斜杠替换为斜杠,例如:C:\ => C:/
  2. 成功生成项目后点击 Open Project 以在 Visual Studio 中打开
  3. 根据需要的构建类型,更改顶部菜单栏选择为 Debug/Release(建议一次性地将 Debug/Release 版本的 静态/动态链接库 构建好)
  4. 右键项目名(DBoW2)->属性->VC++目录 添加需要的依赖项:
  • 包含目录:添加依赖项的 include 路径;如 DBoW2 需要添加 boost 库的 include 路径为 ..\..\..\Thirdparty\boost;
  • 库目录:添加依赖项的 lib 路径;如 DBoW2 需要添加 boost 库的 lib 路径为 ..\..\..\Thirdparty\boost\stage\lib;
  1. 由于 除了 ORB-SLAM3 本体外(后面有讲原因) 编译的几个依赖库库都使用 静态链接 的方式生成,因此需要在项目属性中配置:
  • 常规->配置类型 选择 静态库(.lib)
  • 高级->目标文件扩展名 更改为 .lib
  • 根据需要选择 MSVC 运行库版本为 静态链接(MT/MTd) 或 动态链接 (MD/MDd),整个项目 需保持一致;C/C++->代码生成->运行库 若构建类型为 Debug 则选择 多线程调试(/MTd)多线程调试 DLL(/MDd),若为 Release 则选择 多线程(/MT)多线程 DLL(/MD)
  1. 右键 ALL_BUILD->生成

需要填写的 CMake 变量值:

  • OpenCV_DIR: C:\Users\usrlibzy\Documents\Visual Studio 2022\Projects\ORB_SLAM3\Thirdparty\opencv\build

由于是使用 MSVC 编译,gcc 标准库中的 stdint-gcc.h 头文件自然是找不到的,但由于里面的类型由 MSVC 的其它头文件定义过,因此可以直接注释掉 Thirdparty\DBoW2\DBoW2\FORB.cpp 文件中的 include 引用:

//--//#include <stdint-gcc.h>

最终构建产物位于 Thirdparty\DBoW2\lib

Eigen3 生成

作为 g2o 的依赖的 Eigen3,其依赖 Boost 库只需在 CMake 配置时提供其 include 路径即可:

  • Boost_INCLUDE_DIR: ../../boost
    然后打开工程,右键 INSTALL 生成即可。此生成只包含头文件与 CMake 文件供 g2o 编译使用,不产生实际二进制文件。

Eigen3_DIR 目录为 .\build\artifacts\share\eigen3\cmake

g2o 编译

g2o 在使用 CMake GUI 生成项目时需配置其依赖 Eigen3 的 include 路径参数 G2O_EIGEN3_INCLUDE../eigen/build/artifacts/include/eigen3

g2o 的 CMakeLists.txt 中警告层级 -W 参数没有值,在 gcc 下为默认打印所有警告,而 MSVC 下必须显示赋值,因此手动去掉或修改为 -W1 即可:

#-- Explicitly set Warning param
%(AdditionalOptions) -W1 -O3 -march=native

项目中有以下几个文件的 unordered_mapshared_ptr 使用的还是 较高版本编译器已移除的 std::tr1 命名空间,可以选择手动将 std::tr1::unordered_mapstd::tr1::shared_ptr 分别替换为 std::unordered_mapstd::shared_ptr

  • core/hyper_graph.h
  • core/estimate_propagator.h
  • core/marginal_covariance_cholesky.h
  • core/robust_kernel.h
  • core/sparse_block_matrix_ccs.h

项目属性->C/C++->预处理器->预处理器定义 中需要添加一项 WINDOWS; (等价于在所有源文件中添加 #define WINDOWS 宏定义),否则会出现 vasprintf 未定义符号 的问题。

最终构建产物位于 Thirdparty\g2o\build\ReleaseThirdparty\g2o\build\Debug

Pangolin 编译

Pangolin 建议严格使用 v0.6 版本(release date 与 ORB-SLAM3 v1.0 最近)。

Pangolin 在使用 CMake GUI 生成项目时需配置其依赖 Eigen3 的 include 路径参数 EIGEN_INCLUDE_DIRC:/.../Thirdparty/eigen/build/artifacts/include/eigen3
注意:Pangolin 的 EIGEN_INCLUDE_DIRCMAKE_INSTALL_PREFIX 参数均需使用绝对路径。

Pangolin v0.6 存在一个 已知问题 #609,在编译过程中会出现大量类似于 没有可用于扩展的参数包使用类模板需要模板参数列表 的错误。
解决方法为,打开 include\mpark\variant.hpp 文件:

#if defined(__cpp_constexpr) && __cpp_constexpr >= 201304
//-- Commenting out line for Known-Issue: https://github.com/stevenlovegrove/Pangolin/issues/609#issuecomment-645653656
//--//#define MPARK_CPP14_CONSTEXPR
#endif

将中间的 define MPARK_CPP14_CONSTEXPR 行注释掉即可。
然后即可右键 ALL_BUILD->生成 以构建 Pangolin 依赖的静态库。生成结束后,可右键 INSTALL->生成 pangolin.lib。

注意⚠️:右键 INSTALL->生成 Pangolin.lib 时,生成的 Debug/Release 版本的 pangolin.lib 以及 jpeg.lib 名称相同会相互覆盖。建议先配置构建版本为 Debug,将这两个库的文件名末尾添加代表 debug 的字符 d 后,再切换为 Release 生成。最终当前目录下的所有库应该都是 Debug/Release 成对的。

最终构建产物位于 Thirdparty\Pangolin\build\artifacts

ORB-SLAM3 编译

先对 CMakeLists.txt 进行修改。

第 #27 行,由于 MSVC 跳过 C++11 直接支持的 C++14,因此这里注释掉 FATAL_ERROR:

# Check C++11 or C++0x support
include(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)
···
#-- Commenting out for compiling with MSVC C++14 support
#--#else()
#--#   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()

找到刚才 Thirdparty 中编译出的静态链接库位置,然后编辑 CMakeLists.txt 第 #221 行处的 target_link_libraries

target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
#-- Update thirdparty dependencies static libs
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/Release/DBoW2.lib
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/build/Release/g2o.lib
)

另外,由于 C++ 实现的不同以及平台更换后标准库间的差异,需要对 ORB-SLAM3 库项目 源码进行修改:

MSVC 特性:

打开 MapPoint.cc 文件,找到第 #371 行:

//-- Replacing float 2d-array with vector for MSVC not supporting dynamic-length array.
//--//float Distances[N][N];
vector<vector<float> > Distances(N, vector<float>(N));

float Distances[N][N]; 替换为 vector<vector<float> > Distances(N, vector<float>(N));

以及同一个文件第 #388 行:

//--//vector<int> vDists(Distances[i],Distances[i]+N);
vector<int> vDists(Distances[i].begin(), Distances[i].end());

标准库符号缺失:
编译时会出现大量 “usleep”: 找不到标识符 的问题,可使用 StackOverflow 上一个回答中的一个 usleep 实现。
新建文件 __port.cc,添加函数定义:

void usleep(__int64 usec) 
{ 
    HANDLE timer; 
    LARGE_INTEGER ft; 

    ft.QuadPart = -(10*usec); // Convert to 100 nanosecond interval, negative value indicates relative time

    timer = CreateWaitableTimer(NULL, TRUE, NULL); 
    SetWaitableTimer(timer, &ft, 0, NULL, NULL, 0); 
    WaitForSingleObject(timer, INFINITE); 
    CloseHandle(timer); 
}

新建文件 __port.h,添加函数签名:

//-- Added usleep implementation. Source: http://stackoverflow.com/questions/5801813/c-usleep-is-obsolete-workarounds-for-windows-mingw
void usleep(__int64 usec);

然后再在下面各文件中 include __port.h

  • Viewer.cc
  • Tracking.cc
  • System.cc
  • LocalMapping.cc
  • LoopClosing.cc
  • MapPoint.cc
  • Atlas.cc

与前面相同的原因,需要在 ORBmatcher.cc 文件中直接注释掉 #include <stdint-gcc.h>

项目属性->C/C++->预处理器->预处理器定义 中需要添加一项 COMPILEDWITHC11; (等价于在所有源文件中添加 #define COMPILEDWITHC11 宏定义),否则会出现 "monotonic_clock": 不是 "std::chrono" 的成员 及类似的编译期错误。
最后就可以右键 ORB_SLAM3 项目->生成。可能会看到许多的 Warning,但最终是能够构建成功的。
如果在链接时报错 无法解析的外部符号 MD5_Init 等 MD5 相关错误,则链接器未找到 OpenSSL 库,可手动添加 OpenSSL-Win64\lib\VC\x64\MT\libcrypto.lib

注意⚠️:ORB-SLAM3 需要编译为 与 OpenCV 同类型 的链接方式。由于 OpenCV 项目较大,静态链接出的 .lib 文件巨大,因此选择直接安装动态链接的 OpenCV 库。如果 ORB-SLAM3 在这里选择使用静态的方式来链接,能够正常运行 TUM 数据集,但 Port 到摄像头时便会出现 Debug Assertion Failed! Expression: __acrt_first_block == header 的问题:

StackOverflow 上有对此问题的详细解释,大概可以总结为:给动态链接库和静态链接库的分配的是不同的堆内存,因此可能出现 alloc 与 dealloc 并非同一堆内存 的问题。

导出符号:
由于 MSVC 对于 动态链接库的生成 需要通过 __declspec(dllexport) 声明 或使用 def 文件枚举 的方式 显式指定导出符号,才能正确生成包含这些符号的 DLL。因此还需要最后一步:将 需要在自己的项目中引用的符号(即 ORB-SLAM3 的接口) 使用宏定义的方式声明。
在上一步创建的 __port.h 文件中添加宏定义:

//-- Macro Declarations for exporting symbols
#ifdef ORB_SLAM3_BUILD
#define ORB_SLAM3_API __declspec(dllexport)
#else
#define ORB_SLAM3_API __declspec(dllimport)
#endif

然后在 System.h 文件中找到 System 类,在所有需要调用的函数前面添加 ORB_SLAM3_API,例如

ORB_SLAM3_API Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");

最后在 项目属性->C/C++->预处理器->预处理器定义 中添加一项 ORB_SLAM3_BUILD; 即可。

最终构建产物位于 .\build\Release.\build\Debug

Demo 构建流程

最后准备编译 RGB-D Example 来验证移植结果:

  1. 同样地,需要右键 rgbd_tum 项目->属性->C/C++->预处理器->预处理器定义 中添加一项 COMPILEDWITHC11;(等价于在所有源文件中添加 #define COMPILEDWITHC11 宏定义),否则会出现 "monotonic_clock": 不是 "std::chrono" 的成员 及类似的编译期错误
  2. 链接器->高级->导入库 删除所有项
  3. 尝试右键项目->生成,若出现找不到链接库的错误时,可再打开项目属性,在链接器->输入->附加依赖项中添加即可

P.S. 若在链接时出现 error LNK2038: 检测到“RuntimeLibrary”的不匹配项 问题时,先查看是否更改 项目属性->C/C++->代码生成->运行库 是否一致(在本例中为 多线程 /MT),然后查看 报错的动态运行时链接库所对应的静态运行时链接库是否在 链接器->输入->附加依赖项 中出现。若未出现,应手动添加。

构建完成后,可打开 Examples/RGB-D/Release 文件夹,将生成的文件连同 ORB-SLAM3 自带的 TUM1 配置文件、associations 文件夹和下载的数据集,以及 OpenCV 动态链接库 复制到单独的文件夹中,然后右键打开 Terminal,执行命令:

.\rgbd_tum.exe ..\..\Vocabularies\ORBvoc.txt ..\..\Vocabularies\ORBvoc.txt .\tum\TUM1.yaml ..\..\Datasets\rgbd_dataset_freiburg1_room\ ..\..\Datasets\associations\fr1_room.txt

等待几秒 Vocabulary 的加载后,demo 大概是能成功运行起来咯:

完全按照文章流程的步骤,是可以正常加载 Debug 符号进行调试的:

Happy debugging😛

后记

后面还会有一篇来详细记录下密集点云建图的实现
(因为搜到了几篇很有意思的论文所以)打算开一个专题,专门记录三维建图算法的学习折腾。今天先这样啦😊

参考

  1. https://github.com/lydieusang/orbslam3-windows
  2. https://zhuanlan.zhihu.com/p/625605417
  3. https://www.cnblogs.com/oloroso/p/8574936.html
  4. https://blog.csdn.net/xp178171640/article/details/102371279
  5. https://learn.microsoft.com/zh-cn/cpp/build/determining-which-exporting-method-to-use?view=msvc-170#pros-and-cons-of-using-def-files
  6. https://learn.microsoft.com/en-us/cpp/build/exporting-from-a-dll-using-declspec-dllexport?view=msvc-170