SLAM学习——建图（二）

1.对单目构建稠密地图的讨论

像素梯度的问题

在前面中，通过块匹配来获得匹配结果。块匹配的正确与否，依赖于图像块是否具有区分度，有明显梯度的小块将具有良好的区分度。对于梯度不明显的像素，将比较难估计其有效深度。立体视觉一个常见问题：对物体纹理的依赖性。

像素梯度与极线的关系：像素梯度平行于极线方向以及垂直于极线方向。
像素梯度垂直于极线：当沿极线寻找的时候，发现匹配程度是一样的，得到不到有效匹配。

像素梯度平行于极线：能够精确的找到匹配程度最高的地方。

但是实际上极线与相机深度是有一定的角度的，从上面我们可以看到角度越大，匹配的不确定性越大，角度越小，匹配的不确定性越小。

逆深度问题

当我们描述一个点时，可以用（x,y,z）三个量来描述，而这三个亮可能存在明显的相关性，如果不太好。我们可以描述成（u，v）和d来表示一个点，毕竟相互独立。我们假设的是深度值满足高斯分布
但是，存在以下问题：
1.场景深度大概5-10米，但近处肯定不会小于相机焦距。这个分布并不是像高斯分布那样，形成对称形状，尾部可能稍微长一点，而负数区域为0。
2.在室外应用中，存在距离非常远的点，高斯分布描述他们会有数值计算上的困难。
因此我们设深度的倒数，即逆深度为服从高斯分布，可修改上述程序。

图像间的变换

在块匹配之前，做一次图像到图像间的变换是一种常见的预处理方式。我们假设图像小块在相机运动时保持不变，而这个假设在相机平移时能保持不变，但是发生明显的旋转时难以保持。

根据相机模型，参考帧上的一个像素与真实的三维点世界坐标关系为： ，对于当前帧，亦有在它上面的投影。，消去，可以得到俩副图像之间的像素关系。

—-讨论：尽管双目和移动单目能够建立稠密的地图，但是我们认为它们过于依赖环境纹理和光照，不够可靠。—–

2.RGB-D稠密建图

在使用RGB-D进行建图的过程中，我们使用俩种滤波器过滤掉部分点云：外点去除滤波器和降采样滤波器。

#include <ros/ros.h>#include <iostream>#include <fstream>using namespace std;#include <opencv2/core/core.hpp>#include <opencv2/highgui/highgui.hpp>#include <Eigen/Geometry>#include <boost/format.hpp>  // for formating strings#include <pcl/point_types.h>#include <pcl/io/pcd_io.h>#include <pcl/filters/voxel_grid.h>#include <pcl/visualization/pcl_visualizer.h>#include <pcl/filters/statistical_outlier_removal.h>int main( int argc, char** argv ){    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图    vector<Eigen::Isometry3d> poses;         // 相机位姿    ifstream fin("/home/hansry/Slam_Book/src/Pointcloud_mapping/data/pose.txt");    if (!fin)    {        cerr<<"cannot find pose file"<<endl;        return 1;    }    for ( int i=0; i<5; i++ )    {        boost::format fmt( "/home/hansry/Slam_Book/src/Pointcloud_mapping/data/%s/%d.%s" ); //图像文件格式        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像        double data[7] = {0};        for ( int i=0; i<7; i++ )        {            fin>>data[i];        }        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );        Eigen::Isometry3d T(q);        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));        poses.push_back( T );    }    // 计算点云并拼接    // 相机内参    double cx = 325.5;    double cy = 253.5;    double fx = 518.0;    double fy = 519.0;    double depthScale = 1000.0;    cout<<"正在将图像转换为点云..."<<endl;    // 定义点云使用的格式：这里用的是XYZRGB    typedef pcl::PointXYZRGB PointT;    typedef pcl::PointCloud<PointT> PointCloud;    // 新建一个点云    PointCloud::Ptr pointCloud( new PointCloud );    for ( int i=0; i<5; i++ )    {        PointCloud::Ptr current( new PointCloud );        cout<<"转换图像中: "<<i+1<<endl;        cv::Mat color = colorImgs[i];        cv::Mat depth = depthImgs[i];        Eigen::Isometry3d T = poses[i];        for ( int v=0; v<color.rows; v++ )            for ( int u=0; u<color.cols; u++ )            {                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值                if ( d==0 ) continue; // 为0表示没有测量到                if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉                Eigen::Vector3d point;                point[2] = double(d)/depthScale;                point[0] = (u-cx)*point[2]/fx;                point[1] = (v-cy)*point[2]/fy;                Eigen::Vector3d pointWorld = T*point; //将相机的点转换成世界坐标系的点                PointT p ;                p.x = pointWorld[0];                p.y = pointWorld[1];                p.z = pointWorld[2];                p.b = color.data[ v*color.step+u*color.channels() ];                p.g = color.data[ v*color.step+u*color.channels()+1 ];                p.r = color.data[ v*color.step+u*color.channels()+2 ];                current->points.push_back( p );            }        // depth filter and statistical removal，外点去除滤波器        PointCloud::Ptr tmp ( new PointCloud );        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;        statistical_filter.setMeanK(50);        statistical_filter.setStddevMulThresh(1.0);        statistical_filter.setInputCloud(current);        statistical_filter.filter( *tmp );        (*pointCloud) += *tmp;    }    pointCloud->is_dense = false;    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;    // voxel filter，降采样滤波器    pcl::VoxelGrid<PointT> voxel_filter;    voxel_filter.setLeafSize( 0.01, 0.01, 0.01 );       // resolution    PointCloud::Ptr tmp ( new PointCloud );    voxel_filter.setInputCloud( pointCloud );    voxel_filter.filter( *tmp );    tmp->swap( *pointCloud );    cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl;    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//保存pcd文件    return 0;}

通过pcl_viewer map.pcd 可查看结果如下：

3.八叉树地图

尽管pcd有很多好处，但是也有几个明显的缺陷：
1.点云地图通常规模很大，所以pcd文件也会很大。同时点云图也提供了一些非必要的细节，例如地毯的褶皱等，导致空间浪费。
2.点云地图无法处理运动物体，只能“添加点”而不能“去除点”。

提供灵活的、压缩的、又能随时更新的地图形式：八叉图(把一个小方块分成同样大小的八块或从一个节点展开成八个子节点)。结构如下图所示：

在这里八叉树的结构与字典有点不同，八叉树的节点都是8个，如果叶子节点的方块大小为1，当有10层的时候，就有