今天讲一下论文里的Polygon Extraction部分：

Algorithm 1: Create binary image I from points in S

// Algorithm 1: Create binary image I from points in S 
void ContourDetector::UpdateImgMatWithCloud(const PointCloudPtr& pc, cv::Mat& img_mat) {
    int row_idx, col_idx, inf_row, inf_col;
    const std::vector<int> inflate_vec{-1, 0, 1};   //to expand the spot in img
    for (const auto& pcl_p : pc->points) {                                      //把pc->points里的点取出来赋值给pcl_p
        this->PointToImgSub(pcl_p, odom_pos_, row_idx, col_idx, false, false);  //通过一层PointTOImgSub 更新row_idx和col_idx
        if (!this->IsIdxesInImg(row_idx, col_idx)) continue;    //如果该点不在图的范围内，直接跳过

        for (const auto& dr : inflate_vec) {
            for (const auto& dc : inflate_vec) {
                inf_row = row_idx+dr, inf_col = col_idx+dc;
                //这个inf_row 和 inf_col 是干嘛的？？把位置放大？1x1的变成3x3的
                if (this->IsIdxesInImg(inf_row, inf_col)) {
                    img_mat.at<float>(inf_row, inf_col) += 1.0; //把图里(inf_row, inf_col)的位置内容+1
                }
            }
        }
    }
    if (!FARUtil::IsStaticEnv) {
        cv::threshold(img_mat, img_mat, cd_params_.kThredValue, 1.0, cv::ThresholdTypes::THRESH_BINARY);
    }
    if (cd_params_.is_save_img) this->SaveCurrentImg(img_mat);
}

这部分的代码简明扼要，就是从三维点云信息创建二值图像。获取到点云的Point后，通过PointToImgSub得到对应二值图片的位置点，在通过inflate_vec{-1, 0, 1}将区域进行膨胀。也就是原来1x1的点，变成了一个3x3的小方块。

同时函数还提供了一个开放接口，可以让你看到点云被压缩到二值图像上的画面。

if (cd_params_.is_save_img) this->SaveCurrentImg(img_mat) 这部分可以去yaml里改配置文件，将is_save_img设置为true。

这个就是点云映射后的二值图片了。

之后就要对其进行模糊

Algorithm 1:Apply average fifilter to generate blurred image Iblur

void ContourDetector::ResizeAndBlurImg(const cv::Mat& img, cv::Mat& Rimg) {
    img.convertTo(Rimg, CV_8UC1, 255);
    cv::resize(Rimg, Rimg, cv::Size(), cd_params_.kRatio, cd_params_.kRatio, 
               cv::InterpolationFlags::INTER_LINEAR);
    cv::boxFilter(Rimg, Rimg, -1, cv::Size(cd_params_.kBlurSize, cd_params_.kBlurSize), cv::Point2i(-1, -1), false);
}

yaml文件里给的resize_ratio是3.0 ；可以看到首先将img转成CV_8UC1的格式输出成Rimg，然后将Rimg进行resize。我们来看看同一张图片的对比

左边是没有进行resize和blur的原始二值化图片，右边是进行了resize和blur的二值化图片。原始尺寸是401x401像素，resize后的尺寸为1203x1203像素。

通过图像的平滑处理，得到新的Rimg。

Algorithm 1:Extract polygons {Pk contour} based on [29]

这一步就是通过上面的Rimg来进行轮廓的提取也就是contour。

ExtractRefinedContours(Rimg, img_contours)

第一步先通过Rimg抽取出contours 这时候的contours还没经过任何过滤，称为img_contours。

void ContourDetector::ExtractRefinedContours(const cv::Mat& imgIn,
                                            std::vector<CVPointStack>& refined_contours) 
{ 

    std::vector<std::vector<cv::Point2i>> raw_contours;
    refined_contours.clear(), refined_hierarchy_.clear();

    //raw_contours定义为“std::vector<std::vector<cv::Point2i>> raw_contours”，
    //是一个双重向量（向量内每个元素保存了一组由连续的Point构成的点的集合的向量），每一组点集就是一个轮廓，有多少轮廓，contours就有多少元素；

    
    cv::findContours(imgIn, raw_contours, refined_hierarchy_, 
                     cv::RetrievalModes::RETR_TREE, //RETR_TREE 检测所有轮廓，所有轮廓建立一个等级树结构。外层轮廓包含内层轮廓，内层轮廓还可以继续包含内嵌轮廓。
                     cv::ContourApproximationModes::CHAIN_APPROX_TC89_L1);//定义轮廓的近似方法，取值如下：CV_CHAIN_APPROX_TC89_L1：使用teh-Chinl chain 近似算法;
                     
    refined_contours.resize(raw_contours.size());
    for (std::size_t i=0; i<raw_contours.size(); i++) {
        // using Ramer–Douglas–Peucker algorithm url: https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
        cv::approxPolyDP(raw_contours[i], refined_contours[i], DIST_LIMIT, true);//最小多边形拟合，输入raw_contours  输出refined_contours
    }
    // 过滤掉一些poly，删除父轮廓，得到新的refined_contours
    //这个应该就是Downsample vertices in Pk contour based on [30]
    this->TopoFilterContours(refined_contours); 

    //Check the inner angle of each vertex and eliminate the vertices with inner angle < ζ;
    this->AdjecentDistanceFilter(refined_contours);
    //这两步都是在对contour进行过滤 第一步过滤父轮廓，第二步过滤重叠的向量。
}

其中，主要是通过findContours函数进行图片轮廓提取，再经过一系列的过滤，最后得到refined_contours。

Algorithm 1:Downsample vertices in Pk contour based on [30];

void ContourDetector::TopoFilterContours(std::vector<CVPointStack>& contoursInOut) {
    std::unordered_set<int> remove_idxs;
    for (int i=0; i<contoursInOut.size(); i++) {
        if (remove_idxs.find(i) != remove_idxs.end()) continue;
        const auto poly = contoursInOut[i];
        //把一组小于3个point的这个index索引值放到无序集合remove_idxs里
        if (poly.size() < 3) {
            remove_idxs.insert(i);
        //大于3个point的组，还要过一层判断 判断机器人是否在poly内部 ，
        //判断所有poly的点和机器人的odom的位置，如果点始终位于构成线段的同一侧， 那么它就是平面的内部点。

        //!点在平面内部 返回1  点在平面外部返回0
        } else if (!FARUtil::PointInsideAPoly(poly, free_odom_resized_)) {
            //如果点在平面外部 就执行InternalContoursIdxs  这个主要是把父轮廓给放到remove_idxs里。
            InternalContoursIdxs(refined_hierarchy_, i, remove_idxs);
        }
    }
    //对remove_idxs进行操作，把不在remove_idxs里面的索引重新push_back到contoursInOut，也就是从refine_contour里把remove_idx里的索引都删除。
    if (!remove_idxs.empty()) {
        std::vector<CVPointStack> temp_contours = contoursInOut;
        contoursInOut.clear();
        for (int i=0; i<temp_contours.size(); i++) {
            if (remove_idxs.find(i) != remove_idxs.end()) continue;
            contoursInOut.push_back(temp_contours[i]);
        }
    }
}

Algorithm 1:eliminate the vertices with inner angle < ζ

void ContourDetector::AdjecentDistanceFilter(std::vector<CVPointStack>& contoursInOut) {
    /* filter out vertices that are overlapped with neighbor */
    std::unordered_set<int> remove_idxs;
    //循环refined_contour
    for (std::size_t i=0; i<contoursInOut.size(); i++) { 
        //取出元素 赋值给c  c应该也是一串
        const auto c = contoursInOut[i];
        const std::size_t c_size = c.size();
        std::size_t refined_idx = 0;
        //第一轮过滤
        for (std::size_t j=0; j<c_size; j++) {
            cv::Point2f p = c[j]; 
            //如果刚开始，refined_idx<1  或者已经进行到中间，p点和前一个点的距离大于DIST_LIMIT 
            if (refined_idx < 1 || FARUtil::PixelDistance(contoursInOut[i][refined_idx-1], p) > DIST_LIMIT) {
                /** Reduce wall nodes */
                //contoursInOut[i] 这是一组点，p是当前contoursInOut[i]里的一个点
                RemoveWallConnection(contoursInOut[i], p, refined_idx);

                contoursInOut[i][refined_idx] = p;// 修改refined_contour了  把一些点过滤掉 然后可能原来是5边形，过滤完就是4边形
                refined_idx ++;
            }
        }
        /**--------------------------------------------------------------------------------------------------------*/
        /** Reduce wall nodes */
        //第二轮过滤 过滤完后refined_idx可能会改变  判断[i]里所有的点和[i]里第一个点
        RemoveWallConnection(contoursInOut[i], contoursInOut[i][0], refined_idx);
        //比如原来一组[i]里第一轮过滤完有8个point，第二轮过滤完剩下7个point 
        contoursInOut[i].resize(refined_idx);       //把point的数量resize掉。

        //refined_idx大于1 且 contoursInOut[i]的第一个元素和最后一个元素的距离小于DIST_LIMIT
        if (refined_idx > 1 && FARUtil::PixelDistance(contoursInOut[i].front(), contoursInOut[i].back()) < DIST_LIMIT) {
            contoursInOut[i].pop_back();//尾删 删除最后一个点
        }
        //如果删完了 或者 没删之前，contoursInOut[i]的点的数量小于3，就把这些点都加到remove_idxs里。
        if (contoursInOut[i].size() < 3) remove_idxs.insert(i);
    }
    if (!remove_idxs.empty()) { // clear contour with vertices size less that 3
        std::vector<CVPointStack> temp_contours = contoursInOut;
        contoursInOut.clear();
        for (int i=0; i<temp_contours.size(); i++) {
            if (remove_idxs.find(i) != remove_idxs.end()) continue;
            contoursInOut.push_back(temp_contours[i]);
        }
    }
}

经过上面几个步骤，得到最终的refined_contour