ORB-SLAM2 ---- ORBextractor::DistributeOctTree函数

目录

1.ORBextractor::DistributeOctTree函数用处

2. DistributeOctTree函数解析一：主要思想输入及输出参数

2.1 输入输出参数介绍

2.2 函数主要思想介绍 

3. DistributeOctTree函数无死角解析

3.1 根据宽高比确定初始节点数目

3.2 生成初始提取器节点 

3.3 将特征点分配到子提取器中

3.4 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点 

3.5 利用四叉树方法对图像进行划分区域，均匀分配特征点

3.6 当再划分之后所有的Node数大于要求数目时，就慢慢划分直到使其刚刚达到或者超过要求的特征点个数 

3.7 保留每个区域响应值最大的一个特征点

---

1.ORBextractor::DistributeOctTree函数用处

先前链接：ORB-SLAM2 ---- ORBextractor::ComputeKeyPointsOctTree函数_Courage2022的博客-CSDN博客        我们在先前（ComputeKeyPointsOctTree函数）中将金字塔的每层图像分成一个个小的网格，并在小网格内提取了特征点，但是这个特征点不能直接存放进vector<vector<KeyPoint> >& allKeypoints 容器中，原因有以下两点。

        1.我们要求每一层图像金字塔存放的特征点个数有要求。

先前链接

ORB-SLAM2 ---- ORBextractor::ORBextractor类_Courage2022的博客-CSDN博客

        2.我们要求特征点分配均匀。

        于是，我们要进行特征点的均匀化分布并剔除一部分过于密集的特征点，这就是这个函数的作用。

2. DistributeOctTree函数解析一：主要思想输入及输出参数

2.1 输入输出参数介绍

/**
 * @brief 使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发
 * 
 * @param[in] vToDistributeKeys     等待进行分配到四叉树中的特征点
 * @param[in] minX                  当前图层的图像的边界，坐标都是在“半径扩充图像”坐标系下的坐标
 * @param[in] maxX 
 * @param[in] minY 
 * @param[in] maxY 
 * @param[in] N                     希望提取出的特征点个数
 * @param[in] level                 指定的金字塔图层，并未使用
 * @return vector<cv::KeyPoint>     已经均匀分散好的特征点vector容器
 */
vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
                                       const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)

        在某一层图像金字塔中，我们传来了待分配特征点的容器const vector<cv::KeyPoint>& vToDistributeKeys，图像的信息，以及期望最终经过均匀化剔除留下的特征点数目N，返回均匀化特征点的容器。

2.2 函数主要思想介绍 

        我们要把一层金字塔图像分成若干个节点，然后判断每个节点的特征点数目：

                若该节点内特征点数目大于1，则将节点分裂成四个小结点。

                若该节点内特征点数目等于1，则不分裂该节点。

                若该节点内特征点数目等于0，则删除该节点。

        最终得到等于参数N或者大于参数N的节点数量，对特征点进行评分，留下评分好的前N个特征点进行输出，作为最终提取到的特征点传给 vResultKeys 容器，ruturn给keypoints容器。

3. DistributeOctTree函数无死角解析

3.1 根据宽高比确定初始节点数目

// Step 1 根据宽高比确定初始节点数目
    const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));

	//一个初始的节点的x方向有多少个像素
    const float hX = static_cast<float>(maxX-minX)/nIni;

	//节点的链表
    list<ExtractorNode> lNodes;

	//节点指针的vector
    vector<ExtractorNode*> vpIniNodes;

	//重新设置其大小,下图只有两个节点
    vpIniNodes.resize(nIni);

        先通过宽高比确定初始节点数目nIni，这里我们假设为2，如下图，计算了一个初始的节点的x方向有多少像素hX，这时为了将两个节点平均分开的缘故，然后创建节点的链表lNodes以及指针vpIniNodes，将指针容器初始化为节点数目（2）。           

        这一步我们仅仅有两个节点，它的大小，里面特征点信息完全不知道...也就是说是个空壳子..                          

3.2 生成初始提取器节点 

for(int i=0; i<nIni; i++)
    {      
		//生成一个提取器节点
        ExtractorNode ni;

		//设置提取器节点的图像边界
		//注意这里和提取FAST角点区域相同，都是“半径扩充图像”，特征点坐标从0 开始 
        ni.UL = cv::Point2i(hX*static_cast<float>(i),0);    //UpLeft
        ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);  //UpRight
		ni.BL = cv::Point2i(ni.UL.x,maxY-minY);		        //BottomLeft
        ni.BR = cv::Point2i(ni.UR.x,maxY-minY);             //BottomRight

		/*
		保存有当前节点的特征点，可以理解为临时变量，因为一层for之后就重新分配了
		std::vector<cv::KeyPoint> vKeys;
		*/
        ni.vKeys.reserve(vToDistributeKeys.size());

		//将刚才生成的提取节点添加到链表中
        lNodes.push_back(ni);
		//存储这个初始的提取器节点句柄
        vpIniNodes[i] = &lNodes.back();
    }

        对每一个节点进行遍历（现在一共是2个），确定节点的边界然后pushback到我们上方的节点列表lNode和指针容器vpIniNodes中。

        这一步我们有两个节点还有它的大小信息，但里面特征点信息完全不知道...也就是说是个空壳子进阶了..

3.3 将特征点分配到子提取器中

//Associate points to childs
    // Step 3 将特征点分配到子提取器节点中
    for(size_t i=0;i<vToDistributeKeys.size();i++)
    {
		//获取这个特征点对象
        const cv::KeyPoint &kp = vToDistributeKeys[i];
		//按特征点的横轴位置，分配给属于那个图像区域的提取器节点（最初的提取器节点）
        vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
    }

        vToDistributeKeys容器存储的是当前金字塔图像层的所有特征点。我们用特征点的列坐标去除一个初始的节点的x方向有多少像素hX，这样，如果是第一个节点内的x除以hX不足1，就分配到第1个节点内；如果是第二个节点内的x除以hX=[1,2]，就分配到第2个节点内。

         这一步我们有两个节点还有它的大小信息，里面特征点信息也完全知道了...就差分配了...

3.4 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点 

    list<ExtractorNode>::iterator lit = lNodes.begin();
    while(lit!=lNodes.end())
    {
		//如果初始的提取器节点所分配到的特征点个数为1
        if(lit->vKeys.size()==1)
        {
			//那么就标志位置位，表示此节点不可再分
            lit->bNoMore=true;
			//更新迭代器
            lit++;
        }
        ///如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它
        else if(lit->vKeys.empty())
            //注意，由于是直接删除了它，所以这里的迭代器没有必要更新；否则反而会造成跳过元素的情况
            lit = lNodes.erase(lit);			
        else
			//如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器 
            lit++;
    }

        我们遍历提取器容器中的每一个节点。如果其中就有一个节点，那么更改其标志位bNoMore为true，意味这此节点不可再分。如果节点是空的，则删除节点信息，否则迭代器向后移动。

3.5 利用四叉树方法对图像进行划分区域，均匀分配特征点

   //结束标志位清空
    bool bFinish = false;

	//记录迭代次数，只是记录，并未起到作用
    int iteration = 0;

	//声明一个vector用于存储节点的vSize和句柄对
	//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
    vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;

	//调整大小，这里的意思是一个初始化节点将“分裂”成为四个
    vSizeAndPointerToNode.reserve(lNodes.size()*4);

    // Step 5 利用四叉树方法对图像进行划分区域，均匀分配特征点
    while(!bFinish)
    {
		//更新迭代次数计数器，只是记录，并未起到作用
        iteration++;

		//保存当前节点个数，prev在这里理解为“保留”比较好
        int prevSize = lNodes.size();

		//重新定位迭代器指向列表头部
        lit = lNodes.begin();

		//需要展开的节点计数，这个一直保持累计，不清零
        int nToExpand = 0;

		//因为是在循环中，前面的循环体中可能污染了这个变量，所以清空
		//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
        vSizeAndPointerToNode.clear();

        // 将目前的子区域进行划分
        while(lit!=lNodes.end())
        {
			//如果提取器节点只有一个特征点，
            if(lit->bNoMore)
            {
                lit++;
                continue;
            }
            else
            {
				//如果当前的提取器节点具有超过一个的特征点，那么就要进行继续分裂
                ExtractorNode n1,n2,n3,n4;

				//再细分成四个子区域
                lit->DivideNode(n1,n2,n3,n4); 

				//如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中
                if(n1.vKeys.size()>0)
                {
					//注意这里也是添加到列表前面的
                    lNodes.push_front(n1);   

					//再判断其中子提取器节点中的特征点数目是否大于1
                    if(n1.vKeys.size()>1)
                    {
						//如果有超过一个的特征点，那么待展开的节点计数加1
                        nToExpand++;

						//保存这个特征点数目和节点指针的信息
                        vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));

						//?这个访问用的句柄貌似并没有用到？
                        // lNodes.front().lit 和前面的迭代的lit 不同，只是名字相同而已
                        // lNodes.front().lit是node结构体里的一个指针用来记录节点的位置
                        // 迭代的lit 是while循环里作者命名的遍历的指针名称
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                //后面的操作都是相同的，这里不再赘述
                if(n2.vKeys.size()>0)
                {
                    lNodes.push_front(n2);
                    if(n2.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                if(n3.vKeys.size()>0)
                {
                    lNodes.push_front(n3);
                    if(n3.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                if(n4.vKeys.size()>0)
                {
                    lNodes.push_front(n4);
                    if(n4.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();
                    }
                }

                //当这个母节点expand之后就从列表中删除它了，能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的
                // 分裂方式是后加的节点先分裂，先加的后分裂
                lit=lNodes.erase(lit);

				//继续下一次循环，其实这里加不加这句话的作用都是一样的
                continue;
            }//判断当前遍历到的节点中是否有超过一个的特征点
        }//遍历列表中的所有提取器节点

        // Finish if there are more nodes than required features or all nodes contain just one point
        //停止这个过程的条件有两个，满足其中一个即可：
        //1、当前的节点数已经超过了要求的特征点数
        //2、当前所有的节点中都只包含一个特征点
        if((int)lNodes.size()>=N 				//判断是否超过了要求的特征点数
			|| (int)lNodes.size()==prevSize)	//prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的
												//节点区域中只有一个特征点，已经不能够再细分了
        {
			//停止标志置位
            bFinish = true;
        }

        在开始前先建立变量：结束标志位bFinish = false 、迭代次数 iteration = 0、vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode 记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄、并将其初始化为4，要是大于1个节点的话不应该初始化为 4*nIni吗（疑似bug）！

        随后进入循环：

        当没有结束时（bFinish = true）不断进行循环....例如第一次循环

        我们先记录迭代次数iteration=1，保存当前节点个数prevSize=2，重新定位迭代器lit指向列表头部如下图所示。

        记录需要展开的节点个数nToExpand = 0。

        接着对子区域（上图lit所指）进行划分：

        如果仅仅该节点有1个特征点，则无需分裂，让迭代器lit++遍历下一个节点，否则对节点进行分裂。

ORB-SLAM2 ---- ExtractorNode::DivideNode函数_Courage2022的博客-CSDN博客

        紧接着对这分裂出来的四个节点进行判断。

        如果这其中的特征点的数目大于零，将此节点添加到lNode容器最前面，在判断如果这个节点包含的特征点数目大于1，更新nToExpand标志位（可以再次分裂的节点数目），再将这个节点的信息（特征点的数目，该节点的句柄）放入记录存放可以继续分裂的节点的pair对组的容器vSizeAndPointerToNode中，之后的3个节点也是照此操作。

        当一次节点分裂完毕后，删除父节点lit，继续下一个节点的分裂。

        当节点持续分裂，分裂出来的节点数目到达预定的数量N或者一次分裂前后节点数量与上一次分裂时的节点数量一致时，记标记为为bFinish = true，结束分裂。

        分裂示意图如下：

3.6 当再划分之后所有的Node数大于要求数目时，就慢慢划分直到使其刚刚达到或者超过要求的特征点个数 

       if((int)lNodes.size()>=N 				//判断是否超过了要求的特征点数
			|| (int)lNodes.size()==prevSize)	//prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的
												//节点区域中只有一个特征点，已经不能够再细分了
        {
			//停止标志置位
            bFinish = true;
        }

        // Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数
        //可以展开的子节点个数nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3
        else if(((int)lNodes.size()+nToExpand*3)>N)
        {
			//如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出
			//这里的nToExpand和vSizeAndPointerToNode不是一次循环对一次循环的关系，而是前者是累计计数，后者只保存某一个循环的
			//一直循环，直到结束标志位被置位
            while(!bFinish)
            {
				//获取当前的list中的节点个数
                prevSize = lNodes.size();

				//保留那些还可以分裂的节点的信息, 这里是深拷贝
                vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
				//清空
                vSizeAndPointerToNode.clear();

                // 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序
				// 优先分裂特征点多的节点，使得特征点密集的区域保留更少的特征点
                //! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort
                sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());

				//遍历这个存储了pair对的vector，注意是从后往前遍历
                for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
                {
                    ExtractorNode n1,n2,n3,n4;
					//对每个需要进行分裂的节点进行分裂
                    vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);

                    // Add childs if they contain points
					//其实这里的节点可以说是二级子节点了，执行和前面一样的操作
                    if(n1.vKeys.size()>0)
                    {
                        lNodes.push_front(n1);
                        if(n1.vKeys.size()>1)
                        {
							//因为这里还有对于vSizeAndPointerToNode的操作，所以前面才会备份vSizeAndPointerToNode中的数据
							//为可能的、后续的又一次for循环做准备
                            vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n2.vKeys.size()>0)
                    {
                        lNodes.push_front(n2);
                        if(n2.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n3.vKeys.size()>0)
                    {
                        lNodes.push_front(n3);
                        if(n3.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n4.vKeys.size()>0)
                    {
                        lNodes.push_front(n4);
                        if(n4.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }

                    //删除母节点，在这里其实应该是一级子节点
                    lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);

					//判断是是否超过了需要的特征点数？是的话就退出，不是的话就继续这个分裂过程，直到刚刚达到或者超过要求的特征点个数
					//作者的思想其实就是这样的，再分裂了一次之后判断下一次分裂是否会超过N，如果不是那么就放心大胆地全部进行分裂（因为少了一个判断因此
					//其运算速度会稍微快一些），如果会那么就引导到这里进行最后一次分裂
                    if((int)lNodes.size()>=N)
                        break;
                }//遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂，直到刚刚达到或者超过要求的特征点个数

                //这里理想中应该是一个for循环就能够达成结束条件了，但是作者想的可能是，有些子节点所在的区域会没有特征点，因此很有可能一次for循环之后
				//的数目还是不能够满足要求，所以还是需要判断结束条件并且再来一次
                //判断是否达到了停止条件
                if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
                    bFinish = true;				
            }//一直进行nToExpand累加的节点分裂过程，直到分裂后的nodes数目刚刚达到或者超过要求的特征点数目
        }//当本次分裂后达不到结束条件但是再进行一次完整的分裂之后就可以达到结束条件时
    }// 根据兴趣点分布,利用4叉树方法对图像进行划分区域

        在每次分裂的时候我们会判断当前节点数量lNodes.size+3*待分裂节点数量nToExpand。如果比我们最终要得到金字塔该层的特征点数高时我们将所有待分裂的节点存储的pair对组按特征点的数量进行从小到大排序，然后将节点数多的节点进行分裂。当节点持续分裂，分裂出来的节点数目到达预定的数量N或者一次分裂前后节点数量与上一次分裂时的节点数量一致时，记标记为为bFinish = true，结束分裂。

3.7 保留每个区域响应值最大的一个特征点

//使用这个vector来存储我们感兴趣的特征点的过滤结果
    vector<cv::KeyPoint> vResultKeys;

	//调整容器大小为要提取的特征点数目
    vResultKeys.reserve(nfeatures);

	//遍历这个节点链表
    for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
    {
		//得到这个节点区域中的特征点容器句柄
        vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;

		//得到指向第一个特征点的指针，后面作为最大响应值对应的关键点
        cv::KeyPoint* pKP = &vNodeKeys[0];

		//用第1个关键点响应值初始化最大响应值
        float maxResponse = pKP->response;

		//开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了
        for(size_t k=1;k<vNodeKeys.size();k++)
        {
			//更新最大响应值
            if(vNodeKeys[k].response>maxResponse)
            {
				//更新pKP指向具有最大响应值的keypoints
                pKP = &vNodeKeys[k];
                maxResponse = vNodeKeys[k].response;
            }
        }

        //将这个节点区域中的响应值最大的特征点加入最终结果容器
        vResultKeys.push_back(*pKP);
    }

    //返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点
    return vResultKeys;

      我们用vResultKeys容器存放输出的特征点（经过均匀化、并且达到ORBextractor::ORBextractor类 中规定的特征点数量的特征点）。

        我们遍历我们的特征点节点列表，用选择排序的方法选择出最优特征点，将vResultKeys容器返回给上层调用。