PCL 和 solid state Lidar SLAM

在这里主要的目的是想要使用固态激光器solid state lidar设备来实现SLAM算法。

1. PCL库浏览，并且分析可能会固态激光SLAM有帮助的部分
2. 固态激光器介绍
3. 激光SLAM实现 Video Lidar SLAM

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 cl