RGBD-SLAM学习4

距离上一篇3隔了好久。。。主要是卡在了一些比较愚蠢的坑上了，还有一个弱智的手抖错误。。。

首先说一下，高博github上的代码稍微改一下就能运行，没有任何毛病。貌似只有两处：
一个就是slamBase.cpp中computeKeyPointsAndDesp(）函数中的。keypoints检测器和descripter计算器的创建问题，可能由于OpenCV版本更新导致的feature2d类里面已经没有create()函数了，所以报错找不到相应函数。改成auto detector = cv::ORB::create();这种ORB的就好了。
另外也是因为OpenCV更新导致的solvePnPRansac()增加了一个置信区间参数，值为0-1。设置为默认值0.99就好了。

下面说一下好好的平路自己挖坑跳的地方。总结一句血的教训就是：
参照别人代码敲的时候，代码没跑通之前不要作死尝试更改或优化任何一句代码！never！never！never！！！！！！！！
因为你不知道是你改动的错误，参考代码本身的错误，cmakelists的错误，环境配置的错误，语法错误，逻辑错误等等一万个可能性导致跑不出来想要的结果。有时还不报错，完美运行正常结束，然后结果为蛋。。。满脑子：老子裤子都脱了，你就给我看这个？！！

首先第一个：
Clion右键generate可以生成头文件中函数原型的定义。就是写函数原型时，不用再把函数返回值、函数名，函数参数列表啥的再写一遍，直接给你生成，然后自动跳到大括号中，直接写函数体就行了。像下面酱婶滴：

cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
{
    return cv::Point3f();
}

怎么样，看着不错吧！但是那个自动return的point是什么鬼！clion你咋这么牛逼呢？！直接给我构造了一个cv::Point3f()，直接返回。导致我写完了忘了删掉改成自己写的数据了：

cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
{
    cv::Point3f p; // 3D 点
    p.z = double( point.z ) / camera.scale;
    p.x = ( point.x - camera.cx) * p.z / camera.fx;
    p.y = ( point.y - camera.cy) * p.z / camera.fy;
    return cv::Point3f();//忘了改成p!!!!!!
}

然后返回值无论何时全是零值。而且绝对不会报错，毕竟语法没一点毛病！！！
直接的结果就是我用solvePNPransac()函数时怎么算结果都是零值！内点数量都为0！猜测是不是solvePNPransac()函数用法不对？然后又试了是solvePNP()倒是有数据了，但是明显感觉算出的r，t不对。我就迷茫了，于是一句一句对着高博代码看，原来是这的毛病。。。

还有一些自己改动的坑。
1、图像.ptr<>指针一定要写上类型！因为不写并不会报错！可以编译通过并运行！！
在获取彩色图像通道值时，忘了写，导致查看点云时一片黑，什么玩意都没有，但是瞅着又像有点云在里面，因为FPS在跳。看了看输出，说没有点云图像的颜色值。回头看代码：

p.b = rgb.ptr<uchar>(m)[n*3];
p.g = rgb.ptr<uchar>(m)[n*3+1];
p.r = rgb.ptr<uchar>(m)[n*3+2];

原来是把uchar忘了。。。加上就好了。

2、还一个就是没有depth数据3d-2d点对要全部舍弃！
脑子抽筋，在获得goodmatches点对后，需要把depth数据添加上，没有depth数据的点对直接舍弃掉。所以结构上if(depth == 0) continue;这句应该在img.push_back()和obj.push_back()这两句之前，因为只要depth数据没有，整个点对都要被舍弃掉。脑子进水，上来先把点全push进2d数组，然后再查depth数据，没有的continue。想成了深度数据跟第二张图没有关系了。。。
直接结果就是操作完后，2d数组数量和3d数组的数量不一样！然后solvePNPransac()函数报错了，说点的数量必须大于零并相等！想当然的后果！

下面主要是做的一些自己的改动，修修补补最终跑起来了。
1、简化了下计算关键点和描述子的过程
OpenCV中找到个同时计算keypoint和descriptier的函数detectAndCompute()

void computeKeyPointsAndDesp(FRAME &frame)
{
/*
    cv::Ptr<cv::FeatureDetector> _detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> _descriptor = cv::ORB::create();

    _detector->detect(frame.rgb, frame.kp);
    _descriptor->compute(frame.rgb, frame.kp, frame.desp);
*/

    auto detector = cv::ORB::create();
    detector->detectAndCompute(frame.rgb, cv::noArray(), frame.kp, frame.desp);

    cout<<