ManhattanSLAM跑通失败记录

因为需要对比别的方法在ICL-NUIM数据集的精度,所以上周一直在尝试跑通ManhattanSLAM,但是失败了,在这里记录一下。如果有人了解或是跑过这个工程望不吝赐教!

ManhattanSLAM下载地址:GitHub - razayunus/ManhattanSLAM: A Planar RGB-D SLAM which utilizes Manhattan World structure to provide optimal camera pose trajectory while also providing a sparse reconstruction containing points, lines and planes, and a dense surfel-based reconstruction.

环境配置

1、tinyply库(2.3.2)安装

安装过程参考博客:安装编译tinyply_稻壳特筑的博客-CSDN博客

./build.sh命令运行失败,报错

make[2]: *** No rule to make target 'libtinyply.so', needed by '../lib/libManhattanSLAM.so'

解决方法:用sudo cp /usr/local/lib/libtinyply.so /ManhattanSLAM/lib命令将libtinyply.so文件复制到lib文件夹中,然后更改CMakeList文件中的一行代码如下:

target_link_libraries(${PROJECT_NAME}
        ${OpenCV_LIBS}
        ${EIGEN3_LIBS}
        ${Pangolin_LIBRARIES}
        ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
        ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
        ${PCL_LIBRARIES}
#        ${tinyply_LIBRARY_DIR}/libtinyply.so
        ${PROJECT_SOURCE_DIR}/lib/libtinyply.so
        )
2、PCL库(1.10.1)安装

参考博客:Ubuntu18.04安装PCL(详细教程) - 古月居 (guyuehome.com)

安装后测试程序报错找不到pcl,后来在CMakeList里用set指向了地址后就可以了。(但是后面把下面的代码删掉后也可以运行?)

set(PCL_DIR"/usr/local/include/pcl-1.10/pcl")

另外是在clion中build后会报很多PCL的警报:

(PCL_COMMON)   does not match the name of the calling package (PCL).

可以在CMakeList的find_package前面加上:

# 为了PCL不报警告加的
if(NOT DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS)
    set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS 1 CACHE INTERNAL "No dev warnings")
endif()

代码运行错误

错误1:
Assertion `((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime==Dynamic || size<=MaxSizeAtCompileTime)) || SizeAtCompileTime == size) && size>=0' failed.

我在clion中配置Edit Configurations -> CMake Debug进行调试,发现问题出在static_cast<Vector6d>(NULL)这里,应该是这个强制转换错误,我编了个测试程序试了一下也报错。这个代码在工程中出现了5次:

2处修改为

                    bool isEmpty = mLastFrame.Obtain3DLine(i, mImDepth, line3D);
//                    if (line3D == static_cast<Vector6d>(NULL)) {
                    if (isEmpty == true) {    

还有3处按上面修改后还需要更改返回值,因为我不会返回一个空的vector6d,所以我把返回值类型改成了void,vector6d改成了传参的形式。

错误2:

错误1修正后会运行到第二帧时出现Eigen内存对齐错误或者是程序跳出:

Assertion `((SizeAtCompileTime == Dynamic && (MaxSizeAtCompileTime==Dynamic || size<=MaxSizeAtCompileTime)) || SizeAtCompileTime == size) && size>=0' failed.
或
Process finished with exit code 139 (interrupted by signal 11: SIGSEGV)

因为gcc的优化会让单步debug时出现回跳的情况,所以我把CMakeList的O3改成了O0,然后在clion中debug。我调了挺多次,出现了非常奇怪的问题,程序不变,有些时候单步调试出错的地方和之前不一样,而且直接运行和单步调试停止的地方也不一样,我现在也不知道这问题出在哪里。

单步调试可以调试到第二帧运行到Optimizer.cc的413行:

            optimizer.optimize(its[it]);    // Frenzy 对齐错误

直接运行的话会在第二帧运行到Frame.cc的

            threadPoints.join();

然后就停了。

用gbd的backtrace命令的输出如下:

(gdb) backtrace
#0  0x00007ffff54189e8 in g2o::EdgeSE3ProjectXYZOnlyPose::linearizeOplus() ()
    at /home/llh/SLAM/ManhattanSLAM/Thirdparty/g2o/lib/libg2o.so
#1  0x00007ffff7b4d03c in g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::buildSystem() () at /home/llh/SLAM/ManhattanSLAM/lib/libManhattanSLAM.so
#2  0x00007ffff544e3a9 in g2o::OptimizationAlgorithmLevenberg::solve(int, bool) () at /home/llh/SLAM/ManhattanSLAM/Thirdparty/g2o/lib/libg2o.so
#3  0x00007ffff544606d in g2o::SparseOptimizer::optimize(int, bool) ()
    at /home/llh/SLAM/ManhattanSLAM/Thirdparty/g2o/lib/libg2o.so
#4  0x00007ffff7b3b10c in ORB_SLAM2::Optimizer::PoseOptimization(ORB_SLAM2::Frame*) () at /home/llh/SLAM/ManhattanSLAM/lib/libManhattanSLAM.so
#5  0x00007ffff7ae2388 in ORB_SLAM2::Tracking::TrackReferenceKeyFrame() ()
    at /home/llh/SLAM/ManhattanSLAM/lib/libManhattanSLAM.so
#6  0x00007ffff7ae7c37 in ORB_SLAM2::Tracking::Track() ()
    at /home/llh/SLAM/ManhattanSLAM/lib/libManhattanSLAM.so
#7  0x00007ffff7ae9efa in ORB_SLAM2::Tracking::GrabImage(cv::Mat const&, cv::Mat const&, double const&) ()
    at /home/llh/SLAM/ManhattanSLAM/lib/libManhattanSLAM.so
#8  0x00007ffff7ace8ed in ORB_SLAM2::System::Track(cv::Mat const&, cv::Mat const&, double const&) () at /home/llh/SLAM/ManhattanSLAM/lib/libManhattanSLAM.so
#9  0x000055555555c9bb in main ()

我电脑上的库版本为:

ubuntu 18.04
Eigen 3.3.4
PCL 1.10.1
OPENCV 3.4.10和3.2.0
tinyply 2.3.2

尝试过的解决办法:

1、把Debug改为Release模式运行,结果失败,release模式会在threadPoints.join();这一行前面停止。

2、网上查找资料,发现-march=native命令会使用AVX指令集,因为gdb发现问题是出在第三方g2o库里,而报错都是Eigen内存对齐失败,所以网上查内存对齐的问题,最后在Eigen和g2o库中发现有

// /usr/include/eigen3/Eigen/src/Core/util/Constants.h文件
​
enum AlignmentType {
  Unaligned=0,        /**< Data pointer has no specific alignment. */
  Aligned8=8,         /**< Data pointer is aligned on a 8 bytes boundary. */
  Aligned16=16,       /**< Data pointer is aligned on a 16 bytes boundary. */
  Aligned32=32,       /**< Data pointer is aligned on a 32 bytes boundary. */   //gai 2023/6/26
  Aligned64=64,       /**< Data pointer is aligned on a 64 bytes boundary. */
  Aligned128=128,     /**< Data pointer is aligned on a 128 bytes boundary. */
  AlignedMask=255,
  Aligned=16,         /**< \deprecated Synonym for Aligned16. */
#if EIGEN_MAX_ALIGN_BYTES==128
  AlignedMax = Aligned128
#elif EIGEN_MAX_ALIGN_BYTES==64
  AlignedMax = Aligned64
#elif EIGEN_MAX_ALIGN_BYTES==32
  AlignedMax = Aligned32
#elif EIGEN_MAX_ALIGN_BYTES==16
  AlignedMax = Aligned16
#elif EIGEN_MAX_ALIGN_BYTES==8
  AlignedMax = Aligned8
#elif EIGEN_MAX_ALIGN_BYTES==0
  AlignedMax = Unaligned
#else
#error Invalid value for EIGEN_MAX_ALIGN_BYTES
#endif
};
// /usr/include/eigen3/Eigen/src/Core/util/Macros.h文件
​
// If the user explicitly disable vectorization, then we also disable alignment
#if defined(EIGEN_DONT_VECTORIZE)
  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
#elif defined(EIGEN_VECTORIZE_AVX512)
  // 64 bytes static alignmeent is preferred only if really required
  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
#elif defined(__AVX__)
  // 32 bytes static alignmeent is preferred only if really required
  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
#else
  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
#endif

如果用了AVX指令则AlignedMax为32,我对比了ORB-SLAM2、MSC-VO(CVPR 2021)以及ManhattanSLAM中在指向的AlignedMax于下,变量为CMakeList中是否有-march=native命令:

ORBSLAM2 无-march=native(AlignedMax=16) 有-march=native(AlignedMax=32)

MSC-VO 无-march=native(AlignedMax=16) 有-march=native(AlignedMax=32)

ManhattanSLAM 无-march=native(AlignedMax=32) 有-march=native(AlignedMax=32)

可以看出,ManhattanSLAM中的AlignedMax一直32,也就是一直都使用了AVX指令集,我猜测问题应该是出在这里。

但是我没发现CMakeList文件中有会使用了AVX指令集的可疑代码,我还尝试更改Eigen库中宏为Aligned32=16,但是没有效果。

一些疑问:

  1. 我看github里的issue里面有人成功跑通这个工程,但是错误1的static_cast<Vector6d>(NULL)在C++11和C++14下应该是跑不通的,而且跑通的issue是在21年时四月左右,两个跑不通的issue是在今年23年,所以我猜测作者在后面修改了代码?

  2. g2o库是作者修改过的,不知道这里有没有问题。

一些可能有用的网页:

  1. Fix clang unit test · MRPT/mrpt@8c633ad (github.com)

  2. armhf error in unit tests: alignment to be fixed · Issue #688 · MRPT/mrpt (github.com)

  3. Eigen库数据结构内存对齐问题_AndyCheng_hgcc的博客-CSDN博客

  4. SLAM踩坑记录 | 编译框架 | 配置环境等 (taodudu.cc)

  5. c++ - error: "data is not aligned" from Eigen when performing bundle adjustment with g2o - Stack Overflow

  6. CMake指令解析 set(CMAKE_CXX_FLAGS “$ENV{CXXFLAGS} -rdynamic -O3 -fPIC -ggdb -std=c++11 -Wall -Wno-deprec_For Nine的博客-CSDN博客

  7. Issues · razayunus/ManhattanSLAM · GitHub

  • 3
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 4
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值