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);