一起做RGB-D SLAM（5）BUG笔记

跟做高博的《一起做RGB-D SLAM》系列时，出现了挺多bug的（主要因为自己还是个小白），在此记录下解决过程。（ubuntu18.04，opencv3.4.9）

一起做RGB-D SLAM（5）：

1./home/max/slam/src/slamBase.cpp:127:9: error: ‘cv2eigen’ is not a member of ‘cv’ cv::cv2eigen(R, r);

这个错误是因为缺少头文件，需要在src/slamBase.cpp里加上：

#include <opencv2/core/eigen.hpp>

注意，这个头文件只能加在slamBase.cpp里，加到头文件slamBase.h里会报错：

In file included from /home/max/slam/include/slamBase.h:9:0, from /home/max/slam/src/slamBase.cpp:1:/usr/local/include/opencv2/core/eigen.hpp:63:22: error: ‘Eigen’ does not name a type; did you mean ‘eigen’?void eigen2cv( const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& src, OutputArray dst )

原因可能是和其他头文件顺序冲突了？

————找到答案了，要把这个头文件加到src/slamBase.h里的#include <Eigen/Core>后面才行：

2./home/max/slam/src/slamBase.cpp:151:10: error: ‘transformPointCloud’ is not a member of ‘pcl’pcl::transformPointCloud( *original, *output, T.matrix() );

这个错误同样是缺少头文件，需要在src/slamBase.h里加上：

#include <pcl/common/transforms.h>

3./home/max/slam/src/slamBase.cpp:155:17: error: ‘VoxelGrid’ in namespace ‘pcl’ does not name a template typestatic pcl::VoxelGrid voxel;/home/max/slam/src/slamBase.cpp:158:5: error: ‘voxel’ was not declared in this scope voxel.setLeafSize( gridsize, gridsize, gridsize );

同样缺少头文件，需要在src/slamBase.h里加上：

#include <pcl/filters/voxel_grid.h>

4.bash: bin/visualOdometry: 没有那个文件或目录

同学要记得在src/CMakeLists.txt里添加二进制可执行文件啊：

ADD_EXECUTABLE(visualOdometry visualOdometry.cpp)
TARGET_LINK_LIBRARIES(visualOdometry
	slambase
	${PCL_LIBRARIES}
	${OpenCV_LIBS})

5./home/max/slam/src/visualOdometry.cpp:33:42: error: ‘getDefaultCamera’ was not declared in this scopeCAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();

这个是忘了写读取相机内参的内联函数了，在src/slamBase.h里加上即可：

inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
{
    ParameterReader pd;
    CAMERA_INTRINSIC_PARAMETERS camera;
    camera.fx = atof( pd.getData( "camera.fx" ).c_str());
    camera.fy = atof( pd.getData( "camera.fy" ).c_str());
    camera.cx = atof( pd.getData( "camera.cx" ).c_str());
    camera.cy = atof( pd.getData( "camera.cy" ).c_str());
    camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
    return camera;
}

6./home/max/slam/src/visualOdometry.cpp:59:31: error: ‘cvMat2Eigen’ was not declared in this scopeEigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );/home/max/slam/src/visualOdometry.cpp:63:17: error: ‘joinPointCloud’ was not declared in this scope cloud = joinPointCloud( cloud, currFrame, T, camera );

src/slamBase.h里忘记声明cpp里对应的函数了：

PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera );
Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec );

7.CMakeFiles/jointPointCloud.dir/jointPointCloud.cpp.o：在函数‘main’中：jointPointCloud.cpp:(.text+0xd4b)：对‘pcl::visualization::CloudViewer::CloudViewer(std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&)’未定义的引用jointPointCloud.cpp:(.text+0xdd8)：对‘pcl::visualization::CloudViewer::showCloud(boost::shared_ptr<pcl::PointCloudpcl::PointXYZRGBA const> const&, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&)’未定义的引用

在src/CMakeLists.txt中，加入：

FIND_PACKAGE(PCL REQUIRED COMPONENTS common io visualization filters)

8./home/max/slam/src/visualOdometry.cpp:37:10: error: ‘pcl::visualization’ has not been declaredpcl::visualization::CloudViewer viewer(“viewer”);

缺少头文件，需要在src/slamBase.h里加上：

#include <pcl/visualization/cloud_viewer.h>

9.terminate called after throwing an instance of 'cv::Exception’what(): OpenCV(3.4.9) /home/max/SLAM-env/opencv-3.4.9/modules/calib3d/src/solvepnp.cpp:216: error: (-215:Assertion failed) npoints >= 4 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) in function ‘solvePnPRansac’ 已放弃 (核心已转储)

修改src/slamBase.cpp的RESULT_OF_PNP estimateMotion()函数，在下图中第一个白框处添加代码：

if ( minDis < 10 ) 
{
        minDis = 10;
}

在第二个白框处添加代码：

RESULT_OF_PNP result;
if (pts_obj.size() ==0 || pts_img.size()==0)
{
        result.inliers = -1;
        return result;
}

10.运行结果时，点对数据都为0，mins距离都是999。terminate called after throwing an instance of 'pcl::IOException’what(): : [pcl::PCDWriter::writeASCII] Input point cloud has no data! 已放弃 (核心已转储)

这个错误是由于没有找到数据文件，把参数文件parameters.txt里的数据目录修改为：

rgb_dir=data/rgb_png/
depth_dir=data/depth_png/

或者改为绝对路径也可以。

11.运行结果时会因为3d-2d点对不够而终止程序（到文件254时终止）。terminate called after throwing an instance of ‘cv::Exception’ what(): OpenCV(3.4.9) /home/max/SLAM-env/opencv-3.4.9/modules/calib3d/src/calibration.cpp:1097: error: (-2:Unspecified error) in function ‘void cvFindExtrinsicCameraParams2(const CvMat*, const CvMat*, const CvMat*, const CvMat*, CvMat*, CvMat*, int)’> DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences. (expected: ‘count >= 6’), where> ‘count’ is 5 > must be greater than or equal to > ‘6’ is 6 已放弃 (核心已转储)

这个问题是因为在依据两帧图像求相机位姿变化矩阵时（solvepnp类的函数），opencv会在此函数里调用另一个函数（FindExtrinsicCameraParams2），这个函数的3D-2D求解方法要求输入点对数必须大于等于6，而我们输入的点对数明显小于6.

一开始猜想是因为有两帧图像一样，导致点对数不够，加上判断就行，于是按照高博提示，在src/slamBase.cpp里（下图白框处）加上两个判断：

优化匹配点对数目至少要6对以上：

if (goodMatches.size() <= 5) 
{
     result.inliers = -1;
     return result;
}

输入solvepnpransac函数的前一帧三维点对和后一帧二维点对数目至少要大于6:

if (pts_obj.size() <=5 || pts_img.size() <= 5)
{
     result.inliers = -1;
     return result;
}

加入判断以后，bug解决了，确实可以继续运行下去，但是在254帧时又出现了同样的错误，利用之前写的detectfeatures程序运行测试，发现253和254帧并不相同，goodmatch和solvepnpransac的输入点对数也都是大于6的。做了大量的调试也没找到解决方法，甚至连问题都不知道出在哪里，就在我准备放弃的时候，在opencv官方文档里发现了猫腻：opencv官方文档

solvepnpransac函数里的第9个参数，是RANSAC 程序的内部阈值，参数值是观测点和计算点投影之间允许的最大距离。高博的代码里设置的是1，我们把这个值调大一点，改为默认值8就ok了。将src/slamBase.cpp（下图白框处）改为：

cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 8.0, 0.99, inliers );

注意，这个改法仅适用于一起做RGB-D SLAM（5），如果是（7）的话，请参照一起做RGB-D SLAM（7）BUG笔记

ps：voxel_grid分辨率设为0.01效果会好一些；结果图只显示坐标轴或绿红黑三色块，按r重置视角；parameters.txt默认是放在项目根目录下的；计算变换矩阵时的平移矩阵写反了，不过影响不大，可参考一起做RGB-D SLAM（4）BUG笔记最后一个bug。