lego-loam实践02：点云预处理ImageProjection

lego loam中的点云预处理 ImageProjection

lego loam代码解析网上有很多资料，这里并不需要把原理重复一遍又一遍，但很多解析主要是代码解读，初学者很难把代码直观的联系大具体功能或作用，为了学习以及分享用，本博客偏向于直接把代码的作用用图片展示出来。

为了实现这一目标，本人使用matlab对lego loam进行了重构，利用matlab强大的图形化功能更多的分析代码的细节以及原理。

ImageProjection在点云处理上主要分为以下几个部分：

1. projectPointCloud： 将环形的点云展开成32X1800的点云阵列（以VLP32C为例），是将点云逐一计算深度，并保存在rangeMat中。重点是rangeMat，fullinfocloud。
2. groundRemoval： 对地面点进行标记，重点是groundMat，labelMat。
3. cloudSegmentation： 对点云进行分割，主要使用了聚类的方法，重点是labelMat。
4. labelComponents： 点云聚类，在cloudSegmentation被调用
5. findStartEndAngle： 点云起点终点角度，比较简单

其他 的如
cloudHandler()
copyPointCloud();
publishCloud();
resetParameters();
主要和代码结构有关，就不多作介绍。

1.projectPointCloud

计算点云的深度并存储，由于matlab无法读取bag包中ring信息，对于垂直方向非线性分辨率的雷达线束只能通过二分法查表寻找，具体代码如下：

   function projectPointCloud(obj,point_cloud)
        
        fullInfoCloud_intensity=single(zeros(1,57600));
        fullInfoCloud_x=single(zeros(1,57600));
        fullInfoCloud_y=single(zeros(1,57600));
        fullInfoCloud_z=single(zeros(1,57600));
        
        cloudSize=length(point_cloud);
        
        for i=1:cloudSize
            
            ptx=point_cloud(i,1);
            pty=point_cloud(i,2);
            ptz=point_cloud(i,3);
            
            % 计算竖直方向上的角度（雷达的第几线）
            verticalAngle = atan2(ptz, sqrt(ptx^2 + pty^2)) * 180 / pi;
            % 当前点与x轴y轴平面的夹角
            
            %  思路：数组已经经过排序，使用二分法寻找数组中与ang_row最接近的值的下标（第几线rowIdn），由于激光雷达特性，不存在两个数值同时接近
            %  修改点：对于非线性分辨率激光雷达，理论上仅需要对rowIdn进行修改，线性方式为直接公式计算，非线性
            %  直接根据垂直点的角度来判断属于第几线(查表VLP32_res_y)
            
            ang_row = verticalAngle;
            
            st=1;
            ed=32;
            
            if (ang_row >= obj.VLP32_res_y(32))
                rowIdn=32;
            elseif ang_row < obj.VLP32_res_y(32) && ang_row > obj.VLP32_res_y(1)
                % 二分法寻找第几线（matlab无法直接获取ring信息）
                while(ed - st> 1)
                    mid = floor((ed+ st) / 2);
                    if (ang_row >= obj.VLP32_res_y(mid))
                        st = mid;
                    else
                        ed = mid;
                    end
                end
                
                if (abs(obj.VLP32_res_y(st)-ang_row) <= abs(obj.VLP32_res_y(ed)-ang_row))
                    rowIdn=st;
                else
                    rowIdn=ed;
                end
            else
                rowIdn=1;
            end
   
            if (rowIdn < 1 || rowIdn > obj.N_SCAN)   %N_SCAN雷达线数
                continue;
            end

            %  atan2(y,x)函数的返回值范围(-PI,PI],表示与复数x+yi的幅角
            %  下方角度atan2(..)交换了x和y的位置，计算的是与y轴正方向的夹角大小(关于y=x做对称变换)
            %  atan2()计算是与y轴正方向的夹角大小（雷达坐标系）
            horizonAngle = atan2(ptx, pty) * 180 / pi;    % 计算水平方向上点的角度与所在线数
            %   ang_res_x：角分辨率（水平），Horizon_SCAN：水平扫描总数（360°/水平角分辨率）
            %   columnIdn是horizonAngle的线性变换（水平方向），horizonAngle范围：(-180:180),columnIdn范围：(1/4*Horizon_SCAN:5