patchwork++源码学习(3)—主代码逻辑梳理

前言

patchwork++源码中参数变量较多，并且除了R-VPF（区域垂直平面拟合），R-GPF（区域地面拟合），A-GLE（自适应地面似然估计）和TGR（ 临时地面还原）这些主要功能模块，还有很多处理的细节，需要先对算法的主要逻辑框架进行梳理，然后再对每个功能模块的细节进行分析。

1 主代码逻辑梳理

算法逻辑梳理，主体代码中通过三个for循环对于每一个扇形区域进行访问，主要处理过程分为以下六步：

1. 根据扇形区域点的数量（默认为10）直接认为某些区域的点为非地面点；
2. 采用区域排序法进行排序，根据点云Z轴大小进行排序
3. 地面区域拟合，采用 R-VPF（区域垂直平面拟合）和R-GPF（ 区域地面拟合）进行
4. 可视化每个扇形区域
5. 通过一系列条件判断是否是地面
6. 检查是否需要开启TGR 临时地面还原功能

    std::vector<RevertCandidate<PointT>> candidates;
    std::vector<double> ringwise_flatness;

    for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx)
    {

        auto zone = ConcentricZoneModel_[zone_idx];

        // 遍历区块中的每一个环
        for (int ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx)
        {

            // 遍历区块中的每一个扇形区域
            for (int sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx)
            {

                // 1.扇形区域点数小于阈值，认为非地面点
                if (zone[ring_idx][sector_idx].points.size() < num_min_pts_)
                {
                    cloud_nonground += zone[ring_idx][sector_idx];
                    continue;
                }

                // 2. 采用区域排序，根据z值的大小进行排序// --------- region-wise sorting (faster than global sorting method) ---------------- //
                double t_sort_0 = ros::Time::now().toSec();
                sort(zone[ring_idx][sector_idx].points.begin(), zone[ring_idx][sector_idx].points.end(), point_z_cmp<PointT>);
                double t_sort_1 = ros::Time::now().toSec();
                t_sort += (t_sort_1 - t_sort_0);

                // 3. 采用 R-VPF（区域垂直平面拟合）和R-GPF（ 区域地面拟合）进行地面拟合
                double t_tmp0 = ros::Time::now().toSec();
                extract_piecewiseground(zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_); // 区域优化后的地面
                double t_tmp1 = ros::Time::now().toSec();

                t_total_ground += t_tmp1 - t_tmp0;
                pca_time_ += t_tmp1 - t_tmp0;

                // 4.检查每个补丁的参数，包括垂直度、标高和平整度
                //  used in checking uprightness, elevation, and flatness, respectively
                const double ground_uprightness = normal_(2);
                const double ground_elevation = pc_mean_(2, 0);
                const double ground_flatness = singular_values_.minCoeff();
                const double line_variable = singular_values_(1) != 0 ? singular_values_(0) / singular_values_(1) : std::numeric_limits<double>::max();

                double heading = 0.0;

                // for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i);

                Eigen::Vector4f normal_pc_mean_ = pc_mean_.normalized();
                for (int i = 0; i < 3; i++)
                    heading += normal_pc_mean_(i, 0) * normal_(i);

                // 4. 可视化每个扇形区域
                if (visualize_)
                {
                    auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3);
                    polygons.header = poly_list_.header;
                    poly_list_.polygons.push_back(polygons);
                    // 设置地面似然估计状态
                    set_ground_likelihood_estimation_status(zone_idx, ring_idx, concentric_idx, ground_uprightness, ground_elevation, ground_flatness);

                    pcl::PointXYZINormal tmp_p;
                    tmp_p.x = pc_mean_(0, 0);
                    tmp_p.y = pc_mean_(1, 0);
                    tmp_p.z = pc_mean_(2, 0);
                    tmp_p.normal_x = normal_(0);
                    tmp_p.normal_y = normal_(1);
                    tmp_p.normal_z = normal_(2);
                    normals_.points.emplace_back(tmp_p);
                }

                double t_tmp2 = ros::Time::now().toSec();

                // 5. 通过一系列条件判断是否是地面
                /*
                    About 'is_heading_outside' condidition, heading should be smaller than 0 theoretically.
                    ( Imagine the geometric relationship between the surface normal vector on the ground plane and
                        the vector connecting the sensor origin and the mean point of the ground plane )

                    However, when the patch is far awaw from the sensor origin,
                    heading could be larger than 0 even if it's ground due to lack of amount of ground plane points.

                    Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition )
                */
                bool is_upright = ground_uprightness > uprightness_thr_;
                bool is_not_elevated = ground_elevation < elevation_thr_[concentric_idx];
                bool is_flat = ground_flatness < flatness_thr_[concentric_idx];
                bool is_near_zone = concentric_idx < num_rings_of_interest_;
                // bool is_heading_outside = heading < 0.0;
                bool is_heading_outside = heading < 0.35; // caoxl

                // ROS_WARN_STREAM("-concentric_idx--num_rings_of_interest_--ring_idx---> "<<concentric_idx<<", "<<num_rings_of_interest_
                //                 <<", "<<ring_idx);
                /*
                    Store the elevation & flatness variables
                    for A-GLE (Adaptive Ground Likelihood Estimation)
                    and TGR (Temporal Ground Revert). More information in the paper Patchwork++.
                */
                if (is_upright && is_not_elevated && is_near_zone)
                {
                    update_elevation_[concentric_idx].push_back(ground_elevation);
                    update_flatness_[concentric_idx].push_back(ground_flatness);

                    ringwise_flatness.push_back(ground_flatness);
                }

                // Ground estimation based on conditions，进行地面点判断
                if (!is_upright)
                {
                    cloud_nonground += regionwise_ground_;
                }
                else if (!is_near_zone)
                {
                    cloud_ground += regionwise_ground_;
                }
                else if (!is_heading_outside)
                {

                    // ROS_WARN_STREAM("------heading--uprightness_thr_---visualize_----> "<<heading<<", "<<uprightness_thr_<<", "<<visualize_);
                    // ROS_WARN_STREAM("------normal-------------> "<<normal_(0)<<", "<<normal_(1)<<", "<<normal_(2));
                    // ROS_WARN_STREAM("------pc_mean_-----------> "<<pc_mean_(0,0)<<", "<<pc_mean_(1,0)<<", "<<pc_mean_(2,0));
                    cloud_nonground += regionwise_ground_;
                }
                else if (is_not_elevated || is_flat)
                {
                    cloud_ground += regionwise_ground_;
                }
                else
                {
                    RevertCandidate<PointT> candidate(concentric_idx, sector_idx, ground_flatness, line_variable, pc_mean_, regionwise_ground_);
                    candidates.push_back(candidate);
                }
                // Every regionwise_nonground is considered nonground.
                cloud_nonground += regionwise_nonground_;

                double t_tmp3 = ros::Time::now().toSec();
                t_total_estimate += t_tmp3 - t_tmp2;
            }

            double t_bef_revert = ros::Time::now().toSec();

            // 6.检查是否需要开启TGR 临时地面还原功能
            if (!candidates.empty())
            {
                if (enable_TGR_)
                {
                    temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx);
                }
                else
                {
                    for (size_t i = 0; i < candidates.size(); i++)
                    {
                        cloud_nonground += candidates[i].regionwise_ground;
                    }
                }

                candidates.clear();
                ringwise_flatness.clear();
            }

            double t_aft_revert = ros::Time::now().toSec();

            t_revert += t_aft_revert - t_bef_revert;

            concentric_idx++;
        }
    }

区域排序法

//返回z值较小的值
template <typename PointT>
bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; }

//sort函数的自定义排序，规则是从小到大
sort(zone[ring_idx][sector_idx].points.begin(), zone[ring_idx][sector_idx].points.end(), point_z_cmp<PointT>);

2 理解与测试

个人理解

1. 根据点数确定非地面点，可以理解为噪点过滤，单个区域内的点数过少，后面拟合出来的平面非常不可靠
2. 区域排序法，暂时还不清楚用到的点
3. 地面区域拟合， 可视化每个扇形区域， 通过一系列条件判断是否是地面， 检查是否需要开启TGR 临时地面还原功能这些内容比较多，暂时还没发直观的给出立即，后面对每个功能模块单独测试
4. 现有的代码主体逻辑框架比较混乱，增加了理解的难度，可以进行必要的代码整理，

3 后续测试

下次把整体的代码逻辑简化，然后对地面区域拟合的功能模块进行测试理解