RGBD-SLAM学习4

版权声明：学习记录~ https://blog.csdn.net/robinhjwy/article/details/79275317

距离上一篇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<<"keypoints : "<<frame.kp.size()<<endl;

    return;
}

注释部分是高博原来的。

2、构造相机矩阵
原来高博是这么写的：

    double camera_matrix_data[3][3] = {
            {camera.fx, 0, camera.cx},
            {0, camera.fy, camera.cy},
            {0, 0, 1}
    };
    cv::Mat cameraMatrix(3, 3, CV_64F, camera_matrix_data);

结构上很明了，就是用double类型二维数组构造Mat类型矩阵。瞅着不懂的就是这个CV_64F。
先说说这个CV_64F。参看了下面博客：
http://blog.csdn.net/cheng1988shu/article/details/6330376
里面有这么一段：

你如果用cv_32fc1，那么后面对该矩阵的输入输出的数据指针类型都应该是float，这在32位编译器上是32位浮点数，也就是单精度。
你如果用cv_64fc1，那么后面对该矩阵的输入输出的数据指针类型都应该是double，这在32位编译器上是64位浮点数，也就是双精度。

简单理解可能就是：
double类型数组，转化成mat类型要对应CV_64F；
float类型数组，转化成mat类型要对应CV_32F；

下面是改动的：

cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3)<<camera.fx, 0, camera.cx, 0, camera.fy, camera.cy, 0, 0, 1);

用一句，将数据全塞进去。。。看起来比较直接暴力。实验结果是没问题的，但是clion会提示报错int类型与mat类型可能不符。可能是检查器蠢的原因。不过分开两句来写又不报警了。。。。：

cv::Mat cameraMatrix;
    cameraMatrix = (cv::Mat_<double>(3, 3)<<camera.fx, 0, camera.cx, 0, camera.fy, camera.cy, 0, 0, 1);

clion——该精明时候麦基，不该精明时候基德

3、solvePnPRansac()所需rvec, tvec, inliers参数类型都为Mat！
在给solvePnPRansac()传输出承接参数：旋转、平移、内点数组时：

cv::Mat rvec, tvec, inliers;

虽然inliers最后是个列向量(查看内点数量时取的是 .rows() 值)，但必须是mat类型，传入vector类型会报错。。。

4、点云变换函数transformPointCloud()的调用问题
最终获取到了rvec和tvec之后，要做的是把前一帧的整个点云旋转加移动，变换到当前帧的坐标系下，然后拼接起来，构成一个大的点云，这样整个点云建图就出来了。
变换整个点云就是用的这个transformPointCloud()函数，有N种重载，看了下源码，瞅见两种可能常用的：
一种就是高博用的，直接传进去个4*4的transform矩阵；
另外就是可以传入旋转四元数和平移向量；

琢磨看看传入四元数和平移向量：

    Eigen::Matrix3f r_eigen_vec;//这里必须是Matrix，不能是vector类型，因为后面要用其构造四元数，Quaternionf构造函数需要matrix类型构造。
    Eigen::Vector3f t_eigen_vec;//这里必须是Vector类型，不能是matrix类型，因为pcl::transformPointCloud()函数需要参数类型就是vector。
    cv::cv2eigen(result.rvec, r_eigen_vec);
    cv::cv2eigen(result.tvec, t_eigen_vec);

    Eigen::Quaternionf r_eigen_quaternion(r_eigen_vec);

    pcl::transformPointCloud( *cloud1, *output, t_eigen_vec, r_eigen_quaternion);

跑出来的结果看，并没有问题，但是仔细观察对比高博代码出来的点云，貌似这个的点云比高博的拼接痕迹要明显一丢丢，不知道是不是跟这个改动有关系。对比了计算出来的rvec和tvec是完全一样的。

改高博的此处代码是因为感觉由rvec和tvec构造T有点太绕了。。。

中间还有些auto使用和基于范围for循环的改动，仅是看起来省劲。

贴下改动的代码：
slamBase.cpp

//
// Created by robin on 18-2-3.
//

#include "slamBase.h"

PointCloud::Ptr image2PointCloud(cv::Mat &rgb, cv::Mat &depth, CAMERA_INTRINSIC_PARAMETERS camera)
{
    PointCloud::Ptr cloud (new PointCloud);
    for (int m = 0; m < depth.rows; ++m)
    {
        for (int n = 0; n < depth.cols; ++n)
        {
            //获取深度图中(m, n)处的值
            ushort d = depth.ptr<ushort>(m)[n];
            if (d == 0)
                continue;

            //定义空间点，并计算坐标
            PointT p;
            p.z = double(d)/camera.scale;
            p.x = (n-camera.cx)*p.z/camera.fx;
            p.y = (m - camera.cy) * p.z / camera.fy;

            //获取点的颜色
            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];

            //把p加入到点云中
            cloud->points.push_back(p);
        }
    }

    //设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cloud->is_dense = false;

    return cloud;
}

cv::Point3f point2dTo3d(cv::Point3f& uvd, CAMERA_INTRINSIC_PARAMETERS& camera)
{
    cv::Point3f p; //3D点
    p.z = uvd.z / camera.scale;
    p.x = (uvd.x - camera.cx)*p.z / camera.fx;
    p.y = (uvd.y - camera.cy)*p.z / camera.fy;

    return p;
}

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<<"keypoints : "<<frame.kp.size()<<endl;

    return;
}

RESULT_OF_PNP estimateMotion(FRAME &frame1, FRAME &frame2, CAMERA_INTRINSIC_PARAMETERS camera)
{
    //匹配
    static ParameterReader pd;
    vector<cv::DMatch> matches;

    cv::BFMatcher matcher;
    matcher.match( frame1.desp, frame2.desp, matches );
    cout<<"find total "<<matches.size()<<" matches."<<endl;

    //找最小距离值
    double minDis = 999;

    for (auto item:matches)
    {
        if (item.distance < minDis)
            minDis = item.distance;
    }

    cout<<"minDis = "<<minDis<<endl;

    //筛选
    vector<cv::DMatch> goodMatches;
    double good_match_threshold = atof(pd.getData("good_match_threshold").c_str());
    for (auto item:matches)
    {
        if (item.distance < good_match_threshold*minDis)
            goodMatches.push_back(item);
    }

    cout<<"good matches: "<<goodMatches.size()<<endl;

    // 说一下下面的工作，有了goodMatches向量后，要把matche的点对坐标找出来。
    // 由于整个计算关键点、描述子及匹配过程都是rgb图的操作，跟depth没一点关系。
    // 所以这里需要把点对中有深度数据的点的深度加上，但是由于深度数据有可能不存在，所以当depth不存在时，点对直接舍弃！！
    // 结构上就是寻找点对的for循环中，先找depth数据，没有的话直接continue。
    // 有的话再push像素点和空间坐标点。

    //第一帧3d点
    vector<cv::Point3f> pts_obj;
    //第二帧2d点
    vector<cv::Point2f> pts_img;

    for (size_t i = 0; i < goodMatches.size(); ++i)
    {
        //3d点要先获取深度，然后构造，最后push。
        cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;
        ushort d = frame1.depth.ptr<ushort>(int(p.y))[int(p.x)];
        if (d==0) continue;

        //这里一定注意！！2d点的坐标要push进去，一定要在上方d==0判断之后，因为这个像素点如果找不到d，整个点对都要舍弃！
        pts_img.push_back(cv::Point2f(frame2.kp[goodMatches[i].trainIdx].pt));

        //由投影方程计算空间坐标
        cv::Point3f uvd (p.x, p.y, d);
        cv::Point3f point3f = point2dTo3d(uvd, camera);
        pts_obj.push_back(point3f);

    }

    cout<<"pts_obj.size()"<<pts_obj.size()<<endl
        <<"pts_img.size()"<<pts_img.size()<<endl;

    //相机内参
    double camera_matrix_data[3][3] = {
            {camera.fx, 0, camera.cx},
            {0, camera.fy, camera.cy},
            {0, 0, 1}
    };

    cout<<"solving pnp"<<endl;

    //构建相机内参矩阵，用于给solvePnPRansac()传递参数。
    //cv::Mat cameraMatrix(3, 3, CV_64F, camera_matrix_data);

    cv::Mat c;
    c = (cv::Mat_<int>(3, 3) << 0, 1, 2, 3, 4, 5, 6, 7, 8);

    cv::Mat cameraMatrix;
    cameraMatrix = (cv::Mat_<double>(3, 3)<<camera.fx, 0, camera.cx, 0, camera.fy, camera.cy, 0, 0, 1);

    //cv::Mat c = (cv::Mat_<int>(3, 3)<<0, 1, 2, 3, 4, 5, 6, 7, 8);

    cv::Mat rvec, tvec, inliers;

    //求解PNP
    cv::solvePnPRansac(pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 0.99, inliers);
    //cv::solvePnP(pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec);

    RESULT_OF_PNP result;
    result.rvec = rvec;
    result.tvec = tvec;
    result.inliers = inliers.rows;

    cout<<"result.inliers = "<<result.inliers<<endl
        <<"result.rvec = "<<result.rvec<<endl
        <<"result.tvec = "<<result.tvec<<endl;


    return result;
}

ParameterReader::ParameterReader(string filename)
{
    //创建文件输入流
    ifstream fin (filename.c_str());
    if(!fin)
    {
        cerr<<"parameter file does not exist."<<endl;
        return;
    }

    //遍历数据
    while (!fin.eof())
    {
        //读出每一行
        string str;
        getline(fin, str);
        if(str[0] == '#') continue;

        //由“=”定位，“=”前是键，“=”后是值。最终构建出参数类键值对成员data。
        auto pos = str.find("=");
        if (pos == -1) continue;
        string key = str.substr(0, pos);
        string value = str.substr(pos+1, str.length());
        data[key] = value;

        if ( !fin.good() ) break;
    }
}

string ParameterReader::getData(string key)
{
    auto iter = data.find(key);
    if(iter == data.end())
    {
        cerr<<"Parameter name "<<key<<" not found"<<endl;
        return string("NOT_FOUND");
    } else
    {
        return data[key];
    }
}

joinPointCloud.cpp

//
// Created by robin on 18-2-3.
//

#include <iostream>
using namespace std;

#include <slamBase.h>

#include <opencv2/core/eigen.hpp>

#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>


int main(int argc, char** argv)
{
    //参数读取器
    ParameterReader pd;
    //帧
    FRAME frame1, frame2;

    //读取相机内参
    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());

    //读入图像
    frame1.rgb = cv::imread("../inputdata/rgb1.png");
    frame1.depth = cv::imread("../inputdata/depth1.png", -1);
    frame2.rgb = cv::imread( "../inputdata/rgb2.png" );
    frame2.depth = cv::imread( "../inputdata/depth2.png", -1 );

    //提取特征并计算描述子
    cout<<"extracting features"<<endl;
    computeKeyPointsAndDesp(frame1);
    computeKeyPointsAndDesp(frame2);

    cout<<"camera = "<<camera.fx<<" "<<camera.fy<<" "<<camera.cx<<" "<<camera.cy<<" "<<camera.scale<<endl;

    //求解PNP
    RESULT_OF_PNP result = estimateMotion(frame1, frame2, camera);

    //处理下result的类型，因为后面pcl::transformPointCloud()函数需要的关于变换或者旋转平移的参数需要的都是eigen类型的。
    Eigen::Matrix3f r_eigen_vec;//这里必须是Matrix，不能是vector类型，因为后面要用其构造四元数，Quaternionf构造函数需要matrix类型构造。
    Eigen::Vector3f t_eigen_vec;//这里必须是Vector类型，不能是matrix类型，因为pcl::transformPointCloud()函数需要参数类型就是vector。
    cv::cv2eigen(result.rvec, r_eigen_vec);
    cv::cv2eigen(result.tvec, t_eigen_vec);

    //pcl::transformPointCloud()函数需要的变换，要么是4*4 eigen类型矩阵，也可以是eigen类型的平移向量与eigen类型的四元数旋转。
    //构造4*4 eigen类型矩阵结构上比较整齐划一，但是构造起来代码比价臃肿。。。还是直接构造四元数然后传入调用即可
    Eigen::Quaternionf r_eigen_quaternion(r_eigen_vec);

    // 转换点云
    cout<<"converting image to clouds"<<endl;
    PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
    PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );

    // 合并点云
    cout<<"combining clouds"<<endl;
    PointCloud::Ptr output (new PointCloud());
    pcl::transformPointCloud( *cloud1, *output, t_eigen_vec, r_eigen_quaternion);
    *output += *cloud2;
    pcl::io::savePCDFile("../output_pcd/result.pcd", *output);
    cout<<"Final result saved."<<endl;

    pcl::visualization::CloudViewer viewer( "viewer" );
    viewer.showCloud( output );
    while(!viewer.wasStopped()) {};


    return 0;
}