因为需要对比别的方法在ICL-NUIM数据集的精度,所以上周一直在尝试跑通ManhattanSLAM,但是失败了,在这里记录一下。如果有人了解或是跑过这个工程望不吝赐教!
环境配置
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
,但是没有效果。
一些疑问:
-
我看github里的issue里面有人成功跑通这个工程,但是错误1的
static_cast<Vector6d>(NULL)
在C++11和C++14下应该是跑不通的,而且跑通的issue是在21年时四月左右,两个跑不通的issue是在今年23年,所以我猜测作者在后面修改了代码? -
g2o库是作者修改过的,不知道这里有没有问题。