5、RGB-D— 视觉里程计 Visual Odometry

（在做高博的“一起做RGB-D_SLAM”的一些问题，作为自己笔记总结,以督促自己完成并理解)
与上一讲的课程，修改了4个地方：
1. src/slamBase.cpp
首先工具函数：将cv的旋转矢量与位移矢量转换为变换矩阵，类型为Eigen::Isometry3d；
另一个函数：将新的帧合并到旧的点云里：

 // joinPointCloud 
 // 输入：原始点云，新来的帧以及它的位姿
 // 输出：将新来帧加到原始帧后的图像

1. parameters.txt

# part 5 
# 数据相关
# 起始与终止索引
start_index=1
end_index=700
# 数据所在目录##注意路径
rgb_dir=../data/rgb_png/
rgb_extension=.png
depth_dir=../data/depth_png/
depth_extension=.png
# 点云分辨率
voxel_grid=0.02
# 是否实时可视化
visualize_pointcloud=yes
# 最小匹配数量
min_good_match=10
# 最小内点
min_inliers=5
# 最大运动误差
max_norm=0.3

３. src/visualOdometry.cpp　　实现一个VO
4. src/CMakeLists.txt　改了一行（什么意思呢？）
Ｑ１、不然会出现这个，pcl 链接错误

[ 14%] Built target slambase
Linking CXX executable /home/××/learn/my_code/rgbd_slam/bin/detectFeatures
/home/××/learn/my_code/rgbd_slam/lib/libslambase.a(slambase.cpp.o):(.rodata._ZTVN3pcl9VoxelGridINS_12PointXYZRGBAEEE[_ZTVN3pcl9VoxelGridINS_12PointXYZRGBAEEE]+0x48): undefined reference to `pcl::VoxelGrid<pcl::PointXYZRGBA>::applyFilter(pcl::PointCloud<pcl::PointXYZRGBA>&)'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/××/learn/my_code/rgbd_slam/bin/detectFeatures] Error 1
make[1]: *** [src/CMakeFiles/detectFeatures.dir/all] Error 2
make: *** [all] Error 2
10:56:48: The process "/usr/bin/cmake" exited with code 2.
Error while building/deploying project slam (kit: Desktop)
The kit Desktop has configuration issues which might be the root cause for this problem.
When executing step "Make"
10:56:48: Elapsed time: 00:01.

FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization )

改成了

FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization filters )

Ｑ２、
运行

bin/visualOdometry

显示：

Reading files 6
find total 500 matches.
good matches: 126
solving pnp
norm = 0.0107077
T=    0.999997   0.00130699    0.0020222   0.00362735
 -0.00130765     0.999999  0.000322329   0.00742123
 -0.00202178 -0.000324972     0.999998 -0.000545456
           0            0            0            1
Reading files 7
find total 500 matches.
good matches: 0
solving pnp
OpenCV Error: Assertion failed (opoints.isContinuous()) in solvePnPRansac, file /build/buildd/opencv-2.4.8+dfsg1/modules/calib3d/src/solvepnp.cpp, line 283
terminate called after throwing an instance of 'cv::Exception'
  what():  /build/buildd/opencv-2.4.8+dfsg1/modules/calib3d/src/solvepnp.cpp:283: error: (-215) opoints.isContinuous() in function solvePnPRansac

Aborted (core dumped)

几秒就消失了
有人回答说是配置文件路径问题？

# 数据所在目录
rgb_dir= data/rgb_png/
rgb_extension=.png
depth_dir= data/depth_png/
depth_extension=.png

改了也不行
后面发现错误里good matches: 0
所以考虑问题应该是在ｓｌａｍbase.cpp里的estimateMotion 计算两个帧之间的运动那一块有问题

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

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

    //RESULT_OF_PNP result;　　／／要注释掉

添加：// cvMat2Eigen

最后效果：