1. PCL学习关注点

1.1 数据结构-KDtree和八叉树

k-d tree decomposition for the point set : (2,3), (5,4), (9,6), (4,7), (8,1), (7,2)。

在这里插入图片描述

可以用来实现邻域的快速搜索。比如在ICP中，搜索最近邻点，可以设定ICP方法使用KD-Tree加速搜索。
底层的八叉树数据格式可以支持点云的快速压缩和合并。
可以使用八叉树结构分析点云数据之间的空间变化 code，通过检测体素的变化实现，可以用在motion detection上。

1.1.1 检测体素变化

下面就是使用这种方法，在ICP匹配之后寻找变化的点（如果ICP能够提供outlier就好了。。就不用这么麻烦的搞了，但是我死活没有找到接口，哪位大神知道的话指点一下）。

int GLidarSLAM::CompareAndFindOutlier(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_old, 
                                      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_new,
                                      Eigen::Matrix4d transformation_matrix,
                                      Eigen::Matrix4d transformation_matrix_to_world)
{
    float octreeResolution = 1.0f;
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZRGBA> octree(octreeResolution);
 
    // 加入旧的点云数据
    octree.setInputCloud(cloud_old);  
    octree.addPointsFromInputCloud();   

    // switch buffer 
    octree.switchBuffers();

    // 把新的点云数据通过ICP求得的变换矩阵转移到旧点的坐标系下
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::transformPointCloud(*cloud_new, *transformed_cloud, transformation_matrix);

    // 加入新的转换后的点云
    octree.setInputCloud(transformed_cloud);
    octree.addPointsFromInputCloud();

    // 然后获取变化的点云数据
    // retrieve the new points -> indices are index of the new point cloud
    std::vector<int> newPointIdxVector;
    octree.getPointIndicesFromNewVoxels(newPointIdxVector);

    int nFound = newPointIdxVector.size(); 
}

下面是显示的结果。可以发现，虽然确实生效了，并且有一定的筛选作用，但是明显误差还是太大了。所以下面需要进一步的滤波操作才可以。
Video show
在这里插入图片描述

1.1.1 对结果滤波

首先使用简单的范围计数滤波

    pcl::RadiusOutlierRemoval<pcl::PointXYZRGBA> outrem;
    outrem.setInputCloud(cloud_tmp);
    outrem.setRadiusSearch(1.0);
    outrem.setMinNeighborsInRadius(30);
    outrem.filter(*cloud_changed_ptr);

1.1.2 滤波之后按照欧式距离生长

在滤波之后，点云只是变化的一部分，如果要正确的获取运动物体的整体信息，还需要对过滤之后的点云进行生长。

可以使用PCL提供的根据Normal生长的方法，但是其实物体的normal分别不一定是均匀的，所以我觉得这种方法并不好。
PCL也提供了根据颜色的生长方法，和上面的理由一样，不太适合运动物体的生长。
我觉得最适合的应该是在去除地面之后的使用欧式距离的生长。这就要求我们自己书写生长的函数。

我就简单的实现了一个按照欧式距离的生长函数。其中很多参考了PCL点云生长的源代码，最终也得到了不错的结果。

void GLidarSLAM::ConstumeRegionGrowing(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_new,
                           pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_seed,
                           pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_grown)
{
    float distanceThreshold = 1.0f;
    
    // expand the changed points using KD Tree search
    // creates kdtree object
    // 使用KD Tree生长，这里先新建树，并且设定点云和参数
    pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
    // sets our randomly created cloud as the input
    kdtree.setInputCloud(cloud_new);
    // K nearest neighbor search
    int neighbour_number = 20;
    int point_number = static_cast<int> (cloud_seed->points.size());

    std::vector<int> neighbours;
    std::vector<float> distances;

    std::vector<std::vector<int>> point_neighbours;
    std::vector<std::vector<float>> point_distances;
    point_neighbours.resize(point_number, neighbours);
    point_distances.resize(point_number, distances);

    // for each points search its kdtree neighbours
    // 对每一个seed点云中的点云进行生长，然后将结果全部保存下来。
    for (int i_point = 0; i_point < point_number; i_point++)
    {
        neighbours.clear();
        distances.clear();
        if (!pcl::isFinite (cloud_seed->points[i_point]))
            continue;
        kdtree.nearestKSearch(cloud_seed->points[i_point], neighbour_number, neighbours, distances);
        // 使用swap操作，可以大大节省对内存的操作
        point_neighbours[i_point].swap(neighbours);
        point_distances[i_point].swap(distances);
    }
    std::cout << " Find " << neighbour_number << " Kd tree neighbours. \n";
    
    // 之后对邻域点分别处理，如果满足条件就把它加入输出的结果。
    // 这是只是简单的使用欧式距离过滤，其实可以使用其他很多的方式筛选，这就不列举了
    std::vector<int> indicesAdded;
    cloud_grown->clear();
    // check and add the points
    for (int i = 0; i < point_number; i++){
        for(int k = 0 ; k < neighbour_number; k++){
            int index = point_neighbours[i][k];
            if(point_distances[i][k] > distanceThreshold)
                continue;
            if(CheckExistance(indicesAdded, index))
                continue;
            indicesAdded.push_back(index);
            pcl::PointXYZRGBA pt = cloud_new->points[index];
            cloud_grown->points.push_back(pt);
        }
    }
    cloud_grown->width = cloud_grown->points.size();
    cloud_grown->height = 1;
    cloud_grown->is_dense = false;
    std::cout << " Have grown " << indicesAdded.size() << " points. \n";
}

最终的结果可以见下图和视频。算法很不错的得到了迎面行驶而来的车辆，但是明显的问题也存在：生长的结果明显还不够完善，可以考虑进行多轮生长。
Video Show
在这里插入图片描述

1.2 可视化

暂时不关注太多的内容，因为更习惯用pangolin。

1.3 点云滤波

直通滤波PassTrough，简单去除指定区域范围的点。
VoxelGrid体素滤波。每个体素保留一个数据（可以选择存储的是体素三维中心或者体素所有点云的重心）
StatisticalOutlierRemoval按统计规律移除outlier。比如使用KNN过滤（之前使用这种方法过滤SFM稠密重建点云上，得到了比较理想的结果）。
使用参数模型投影点云。（比如将点云投影到相机平面，用来和视觉SLAM结合）
ExtractIndices滤波分割点云。之前尝试使用SACSegmentation分割提取平面，效果很好，但是不够稳定，需要再研究一下。
ConditionalRemoval（可以自定义滤波条件）和RadiusOutlierRemoval（删除孤立的点）。
CropHull使用二维结构分割点云。
RandomFilter:

void RandomDownSample(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float rate)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
    (*cloud_temp) += (*cloud);
    cloud->clear();
    for(size_t i = 0; i < cloud_temp->points.size();i++)
    {
       float r = float(rand_between(0, 100)) / 100.0;
       if(r < rate)
       {
         pcl::PointXYZ pt = cloud_temp->points[i];
         cloud->points.push_back(pt);
       }
    }
    cloud->width = cloud->points.size();
    cloud->height = 1;
    cloud->is_dense = false;
}

在这里可能会用的到的有：ExtractIndices的分割点云，ConditionalRemoval自定义条件的滤波。

1.4 深度图像

可以用来从点云生成深度图像。这里我自己设定了相机参数，并且将点云投影。

UVandZ VirtualCamera::ProjectPointToImage(float x, float y, float z)
{
    UVandZ result;
    float inv_z = 1/(z);
    result.u = (fx * x) * inv_z + cx;
    result.v = (fy * y) * inv_z + cy;
    result.z = z;
    return result;
}

    image = cv::Mat(rows, cols, CV_8UC3, cv::Scalar(0,0,0));
    depth = cv::Mat(rows, cols, CV_32FC1, cv::Scalar(-1));

    int SpeadPixel = 3;

    int pointsize = cloud->points.size();
    std::cout << " points count : " << pointsize << std::endl;
    for(int i = 0; i < pointsize; ++i)
    {
        int r = cloud->points[i].r;
        int g = cloud->points[i].g;
        int b = cloud->points[i].b;
        float x = cloud->points[i].x;
        float y = cloud->points[i].y;
        float z = cloud->points[i].z;
        //printf("x: %f, y : %f, z : %f. \n",x, y, z);
        UVandZ projected = ProjectPointToImage(x, y, z);
        //printf("u: %d, v : %d, z : %f. \n",projected.u, projected.v, projected.z);
        if(!PointInRange(projected)){
            continue;
        }

        cv::circle(image, cv::Point2f(projected.v, projected.u), SpeadPixel, cv::Scalar(b,g,r), -1);
        cv::circle(depth, cv::Point2f(projected.v, projected.u), SpeadPixel, cv::Scalar(z/100), -1);
/*
        depth.at<float>(projected.u, projected.v) = z;
        image.at<cv::Vec3b>(projected.u, projected.v)[0] = b;
        image.at<cv::Vec3b>(projected.u, projected.v)[1] = g;
        image.at<cv::Vec3b>(projected.u, projected.v)[2] = r;
*/
    }

在这里插入图片描述

use Range Image. good reference csdn
可以从深度图像中提取边界（岭纬的数据就是深度图像，可以提取边界，不知道能不能有帮助）
深度图可以之间连接三角形建模（估计对我没什么用）

1.5 关键点-深度图像

之前就考虑想用，非常适合岭纬的传感器，不知道用来SLAM追踪效果怎么样（TODO）

NARF（normal aligned radial feature）从深度图像，提取的是边缘和表面。
Harris 通过harris矩阵的对应的特征值判断，获得角点。点云中使用的法向量的信息。
SIFT（暂时还不知道于图像SIFT的区别 TODO）

1.6 采样一致性RANSAC

RandomSampleConsensus没什么好讲的。

1.7 3D点云特征于描述子

histograms

法向估计（不知道为什么放在这里，这个很重要）。有法向之后就可以做很多事情了。
PFH（point feature histograms）。使用多维直方图对点的领域点云描述。Fast PFH，快了，肯定是有损失的。
VFH（viewpoint feature histogram），将视点方向融入FPFH的相对法线角计算中。

others

RoPS(Rotational Projection Statistics)，旋转投影统计。
惯性矩和偏心率（moment of inertia， eccentricity）

总的来说，感觉还是深度图像的分割更适合我们的情境

1.7.1 Normal Estimation

总的来说就是统计当前点周围的点云。分析它的Covariance matrix，观察它的特征向量（有点类似主成分分析PCA）。主要特征值对应的特征向量就是法向的方向。
代码实现的部分代码：

    // 1. calculate the centorid
    Eigen::Vector3d centroid(0,0,0);
    for(auto point : nearPoints){
        centroid = centroid + point;
    }
    centroid = centroid/nearPoints.size();

    Eigen::Matrix3d sigma = Eigen::Matrix3d::Zero();
    for(auto point : nearPoints){
        Eigen::Vector3d tmp = point - centroid;
        sigma = sigma + tmp * tmp.transpose();
    }
    sigma = sigma/nearPoints.size();

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();

    normal(0) = U(0,1);
    normal(1) = U(1,1);
    normal(2) = U(2,1);

PCL中的实现

    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud(cloud);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    normalEstimation.setSearchMethod(kdtree);

    // choose one of the following two methods
    normalEstimation.setKSearch(20);
    //normalEstimation.setRadiusSearch(0.3);

    normalEstimation.compute(*normals);

在这里插入图片描述