ompl中BITstar代码阅读

基本介绍

ompl

ompl是开放的路径规划库，由许多最先进的基于采样的路径规划算法组成。
官方网站：http://ompl.kavrakilab.org/
由于资料的稀缺，关于BIT*代码的阅读基本依赖于官方提供的API。

BITstar

2015年的基于采样的路径规划算法，结合了批采样、启发函数等策略。
论文《Batch Informed Trees (BIT*): Sampling-based Optimal Planning via
the Heuristically Guided Search of Implicit Random Geometric Graphs》
被引用 243
来源：ICRA

一，.h文件的内容

// 位置：/ompl-.5.1/src/ompl/geometric/planners/informedtrees/BITstar.h
namespace ompl
{
    namespace geometric
    {
        class BITstar : public ompl::base::Planner
        {
        public:
        	a. 其他类的声明，包含顶点、顶点ID生成器、一个辅助类、隐式图、边队列。
        	b. 使用using简化一些类型，例如顶点的指针、顶点vector、边、邻近结构。
        	c. 基本函数。
        		构造函数: BITstar();
        		析构函数: ~BITstar(); 
        		初始化:	 setup();
        		清空:    clear();
        		solve();
        		输出结果: getPlannerData();
        	d. Debug函数，包括输出边队列信息等函数。
        	e. 设置函数-用于设置规划算法的相关参数
        protected:
        	a. 一些为ABIT*准备的函数
        private:
        	a. 高级原语:
        		迭代 iterate();
        		申请新的批 newBatch();
        		剪枝 prune();
        		输出找到的解 publishSolution();
        	b. 低级原语
        		边缘碰撞检测 checkEdge(); 
        		将一条边加入黑名单(碰撞检测不通过) blacklistEdge() 
        		将一条边加入白名单(碰撞检测通过)   whitelistEdge() 
        		将边添加到路径树中 addEdge(); 
        		替换边 replaceParent();
				更新目标顶点 updateGoalVertex();
			c. 一些logging函数 
			d. 一些输出检索信息
			e. 成员变量 所有的变量再setup()中被指定、并在reset()中被重设
				costHelpPtr_ 计算代价和启发值得辅助函数
				graphPtr_    隐式图
				queuePtr_ 边队列(类型:SearchQueue)

			f. 变量
				initialInflationFactor_ 初始膨胀系数 
				truncationFactor_ 截断因子
				truncationScalingParameter_ 
				inflationScalingParameter_ 

				// 每找到新的路径解,更新以下三个值 
				curGoalVertex_   目标顶点,找到解后,记录目标顶点 
				bestCost_  最佳代价,找到解后,记录最佳代价 
				bestLength_  最佳路径的顶点数

				prunedCost_  剪枝代价：每次剪枝后，剪枝代价会记录剪枝后的最佳代价。只有当一个新的解*足够小于这个值时，我们才会进行剪枝。
				prunedMeasure_  剪枝量：
			g. 标志位 
				isFinalSearchOnBatch_  	// 是否穷尽了当前的采样/开启新的批的标志位
				hasExactSolution_      	// 如果找到解 置位true 
				isSearchDone_       	// 当前边队列中是否还存在有价值的边
				stopLoop_           	// =stopOnSolutionChange_   如果为true,那么算法每次找到新解将停止循环
			h. 记录量
				numBatches_ 			// 记录一共采样了几批采样点
				numPrunings_			// 剪枝次数
				numIterations_			// 循环次数
				numRewirings_			// 
				numEdgeCollisionChecks_	// 
			i. 参数 随着构造函数而确定,不会变更
				samplesPerBatch_		// 每批的采样量
				isPruningEnabled_		// 
				pruneFraction_   		// 
				stopOnSolutionChange_ 	// 如果找到新解,循环停止标志位 
		}
	}
}

1. 需要关注的函数有，配置函数setup()与规划主体solve()。
2. 同时也要关注BITstar特有的三个成员变量：
   costHelpPtr_ 计算代价和启发值的辅助函数
   graphPtr_ 隐式图
   queuePtr_ 边队列(类型:SearchQueue)
   算法的大部分实现由这三个类承担。
3. 注意标志位：
   isFinalSearchOnBatch_ // 是否穷尽了当前的采样/开启新的批的标志位
   hasExactSolution_ // 如果找到解 置位true
   isSearchDone_ // 当前边队列中是否还存在有价值的边
   他们将控制算法的流程走向

二，配置函数BITstar::setup()

注意 ompl::base::planner 作为 ompl::geometric::BITstar的基类，也包含一些需要注意的变量/类：

//ompl::base::planner
SpaceInformationPtr si_;  //空间信息  
ProblemDefinitionPtr pdef_;  //问题定义 
PlannerInputStates pis_;   //规划器输入状态 
bool setup_;  //配置是否被调用的标志位 

BITstar::setup()的代码以及注释：

void BITstar::setup()
{
	//a. 调用 空间信息配置Plnner::si_->setup(), Plnner::setup_ 置位true
	Planner::setup();

	//b. 检查问题定义 Planner::pdef_
	if (static_cast<bool>(Planner::pdef_))
    {
        // 如果问题定义planner::pdef_ 没有定义了优化目标,设路径长度为优化目标。
        if (!Planner::pdef_->hasOptimizationObjective())
        { 
            OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", Planner::getName().c_str());
            Planner::pdef_->setOptimizationObjective(std::make_shared<base::PathLengthOptimizationObjective>(Planner::si_));
        }
	    //当前最优解bestCost_ = 无穷
	    bestCost_ = Planner::pdef_->getOptimizationObjective()->infiniteCost();

        // 检查问题定义 Planner::pdef_ 中是否有目标点  并确保类型一致 
	    if (static_cast<bool>(Planner::pdef_->getGoal())){
	        if (!Planner::pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION)){
	            OMPL_ERROR("%s::setup() BIT* currently only supports goals that can be cast to a sampleable "  "goal region.", Planner::getName().c_str());
	            Planner::setup_ = false;
	            return;
	        }
	    }

	    // c. 初始化BITstar相关变量 
		// 用优化目标和隐式图初始化 CostHelper (用来处理启发函数的助手类) 
        costHelpPtr_->setup(Planner::pdef_->getOptimizationObjective(), graphPtr_.get());
        // 用costHelper和隐式图 初始化队列
        queuePtr_->setup(costHelpPtr_.get(), graphPtr_.get());
        // 用空间配置  Planner::si_,问题定义 Planner::pdef_, costHelp , 边队列 queuePtr_, PlannerInputStates pis_ 初始化 隐式图 graphptr_
        graphPtr_->setup(Planner::si_, Planner::pdef_, costHelpPtr_.get(), queuePtr_.get(), this, Planner::pis_);
        // 告诉隐式图 算法启用了剪枝,隐式图需要做一些调整
        graphPtr_->setPruning(isPruningEnabled_);
        // 当前最优成本和剪枝成本皆设为无穷
        bestCost_ = costHelpPtr_->infiniteCost();
        prunedCost_ = costHelpPtr_->infiniteCost();

        // 剪枝量=空间的度量值
        prunedMeasure_ = Planner::si_->getSpaceMeasure();

        // d. 命名修正 
        // 如果使用的并非k邻近搜索，但是使用的是"kBITstar"的名称 
        if (!graphPtr_->getUseKNearest() && Planner::getName() == "kBITstar")
        {
            // 警告
            OMPL_WARN("BIT*: An r-disc version of BIT* can not be named 'kBITstar', as this name is reserved " "for the k-nearest version. Changing the name to 'BITstar'.");
            // 把名字改为 BITstar 
            Planner::setName("BITstar");
        }
        // 如果使用的是k邻近搜索,但是使用的是"BITstar" ,改为"kBITstar"
        else if (graphPtr_->getUseKNearest() && Planner::getName() == "BITstar")
        { 
            OMPL_WARN("BIT*: A k-nearest version of BIT* can not be named 'BITstar', as this name is reserved " "for the r-disc version. Changing the name to 'kBITstar'.");
            Planner::setName("kBITstar");
        }
        // 如果使用的非k邻近,且名字为 "kABITstar"
        else if (!graphPtr_->getUseKNearest() && Planner::getName() == "kABITstar")
        {
            // It's current the default k-nearest ABIT* name, and we're toggling, so set to the default r-disc
            OMPL_WARN("ABIT*: An r-disc version of ABIT* can not be named 'kABITstar', as this name is "
                      "reserved for the k-nearest version. Changing the name to 'ABITstar'.");
            Planner::setName("ABITstar");
        }
        else if (graphPtr_->getUseKNearest() && Planner::getName() == "ABITstar")
        {
            // It's current the default r-disc ABIT* name, and we're toggling, so set to the default k-nearest
            OMPL_WARN("ABIT*: A k-nearest version of ABIT* can not be named 'ABITstar', as this name is "
                      "reserved for the r-disc version. Changing the name to 'kABITstar'.");
            Planner::setName("kABITstar");
        }
        // It's not default named, don't change it
    }
    else
    {
      // 如果没有问题定义,无法进行配置 
        Planner::setup_ = false;
    }
}

需要关注的有：

1. 如果我们不指定优化目标，那么算法默认以路径长度作为优化目标。

Planner::pdef_->setOptimizationObjective(std::make_shared<base::PathLengthOptimizationObjective>(Planner::si_));

2. BITstar相关变量的初始化

costHelpPtr_->setup(Planner::pdef_->getOptimizationObjective(), graphPtr_.get());
queuePtr_->setup(costHelpPtr_.get(), graphPtr_.get());
graphPtr_->setup(Planner::si_, Planner::pdef_, costHelpPtr_.get(), queuePtr_.get(), this, Planner::pis_);

3. 当前最佳成本、剪枝成本、剪枝量

bestCost_ = costHelpPtr_->infiniteCost();
prunedCost_ = costHelpPtr_->infiniteCost();
prunedMeasure_ = Planner::si_->getSpaceMeasure();

三，BITstar::solve函数

ompl::base::PlannerStatus BITstar::solve(const ompl::base::PlannerTerminationCondition &ptc)
{

    // a. 检查配置标志位 Planner::setup_ 是否为true,如果不为真,重新配置,如果仍未成功则报错
    Planner::checkValidity();
    if (!Planner::setup_){ throw ompl::Exception("%s::solve() failed to set up the planner. Has a problem definition been set?", Planner::getName().c_str()); }
    OMPL_INFORM("%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());


    // 该值初始化与默认值都为false,用来停止循环的标志位。
    stopLoop_ = false;


    //b. 检查起点和终点设置是否正常,如果在更新一次后，仍然找不到起点或者终点，警告。
    if (!graphPtr_->hasAGoal()) { graphPtr_->updateStartAndGoalStates(Planner::pis_, ptc); }
    if (!graphPtr_->hasAStart()){OMPL_WARN("%s: A solution cannot be found as no valid start states are available.",Planner::getName().c_str());}
    if (!graphPtr_->hasAGoal()) {OMPL_WARN("%s: A solution cannot be found as no valid goal states are available.",Planner::getName().c_str()); }


    //a. 开始搜索空间,向边队列中插入与起点相连接的边——实际只添加了一条边(连接起点和终点的边)
    // 函数 insertOutgoingEdgesOfStartVertices() 循环了隐式图中的所有起点it,并查找it的所有邻居,填入边队列 queuePtr_ 中。
    if (numIterations_ == 0u)  { queuePtr_->insertOutgoingEdgesOfStartVertices(); }

    // b. 迭代循环 
    /*
    如果没有手动设置停止 ptc:来自主函数的停止位;stopLoop_:BITstar自带的停止位
    如果 理论上还存在更好的解(graphPtr_->minCost() 优于 当前最优解bestCost_) 或者 有其他的终点或起点尚未考虑。
    迭代继续。
    */
    while (!ptc && !stopLoop_ && !costHelpPtr_->isSatisfied(bestCost_) &&
           (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(), bestCost_) ||
            Planner::pis_.haveMoreStartStates() || Planner::pis_.haveMoreGoalStates()))
    { this->iterate(); }

    // 如果有解,进行结果的发布
    if (hasExactSolution_) { this->endSuccessMessage(); }
    else { this->endFailureMessage(); } 
    if (hasExactSolution_ || graphPtr_->getTrackApproximateSolutions()) { this->publishSolution(); }

    // hasExactSolution_: 找到路径解的标志位
    // ImplicitGraph::findApprox_:找到近似解的标志位(ImplicitGraph::getTrackApproximateSolutions()的返回值)
    return {hasExactSolution_ || graphPtr_->getTrackApproximateSolutions(), !hasExactSolution_ && graphPtr_->getTrackApproximateSolutions()};
}

需要特别关注的有函数 SearchQueue::insertOutgoingEdgesOfStartVertices()

// 将起点的邻域出边加入边队列中去
void BITstar::SearchQueue::insertOutgoingEdgesOfStartVertices()
{
	ASSERT_SETUP
	// 遍历所有的起点顶点,并将其出边加入到边队列中
	for (auto it = graphPtr_->startVerticesBeginConst(); it != graphPtr_->startVerticesEndConst(); ++it) 
		{ this->insertOutgoingEdges(*it); }
}

// 将顶点的邻域出边加入到边队列中去
void BITstar::SearchQueue::insertOutgoingEdges(const VertexPtr &vertex)
{
	// 如果顶点有改善解的可能性
	if (this->canPossiblyImproveCurrentSolution(vertex))
	{
		// 获取邻域采样点
		VertexPtrVector neighbourSamples;
		graphPtr_->nearestSamples(vertex, &neighbourSamples);

		//将边<顶点,邻域点>加入到边队列中去
		this->enqueueEdges(vertex, neighbourSamples);
	}
}

四，迭代 BITstar::iterate()

用较为通俗的语言表达一下算法流程

void BITstar::iterate()
{
	// 完成搜索,重新填充队列
	if( isSearchDone_ || queuePtr_->isEmpty() ){
		if( isFinalSearchOnBatch_ || !hasExactSolution_ ){
			确实穷尽了当前的近似,算法开启新的批
			isFinalSearchOnBatch_ = false;
		}
		else{
			通过执行uninflated的搜索,继续探索当前近似
			isFinalSearchOnBatch_ = true;
		}
		isSearchDone_ = false; // 无论是开新的批还是重新搜索，边队列重新填充了新的数据
	}
	else {
		拿出边队列中的最优边
		if(当前边已经在路径树中){}
		else if(当前边有改善解的可能){}
		else {// 边队列中没有有价值的边了
			isSearchDone_ = true; 
		}
	}
}

注意算法采用 标志位：
isSearchDone_ ： 当边队列中没有有价值的边时置为true，即完成了搜索过程，需要更进一步的近似。当通过搜索采样点或不一致点集重新填充了边队列时，置为false。
isFinalSearchOnBatch_ ：搜索过程会填充不一致点集，当搜索完采样点后，需要搜索不一致点集，即需要该标志位判断搜索完成后是进一步采样点还是搜索不一致点集。
hasExactSolution_ ：算法是否找到了解路径。

五，批采样过程

// BITstar::iterate() 中的部分代码
{
	// 如果算法允许剪枝(默认允许剪枝),进行剪枝操作
	if (isPruningEnabled_) { this->prune(); }

	// 添加一个新批.
	this->newBatch();

	// 将膨胀系数 SearchQueue::inflationFactor_ 设为初始值(默认值为1)。
	queuePtr_->setInflationFactor(initialInflationFactor_);

	// 清空边队列
	queuePtr_->clear();

	// 重启边队列，将与起点连接的边插入边队列中 
	queuePtr_->insertOutgoingEdgesOfStartVertices();

	// 标志位
	isFinalSearchOnBatch_ = false;

	// 设置截断因子
	truncationFactor_ = 1.0 + truncationScalingParameter_ / (static_cast<float>(graphPtr_->numVertices() + graphPtr_->numSamples())); 
}

点采样部分有两个重点：

1. 批采样点，将点采样到哪里去了。
2. 加入边队列的是哪些边。

批采样过程是否采样了点？

void BITstar::newBatch()
{
    // numBatches_跟踪采样次数.
    ++numBatches_;

    // 如有变动，更新起点和终点
    if (Planner::pis_.haveMoreStartStates() || Planner::pis_.haveMoreGoalStates()) 
      {  graphPtr_->updateStartAndGoalStates(Planner::pis_, ompl::base::plannerAlwaysTerminatingCondition()); }
    

    // samplesPerBatch_是批采样的数量——默认为100
    // 由隐式图graphPtr_实际执行批采样
    graphPtr_->addNewSamples(samplesPerBatch_);
}

void BITstar::ImplicitGraph::addNewSamples(const unsigned int &numSamples)
{
	ASSERT_SETUP

	// Set the cost sampled to the minimum
	sampledCost_ = minCost_;

	// Store the number of samples being used in this batch
	numNewSamplesInCurrentBatch_ = numSamples;

	// 更新邻域结构的 r或者k,即半径邻域、K邻域的参数。
	this->updateNearestTerms();

	// samples_填入回收样本点。  
	// samples_ 是隐式图中用来保存样本点的最邻近数据结构，主要用来求邻近点。
	samples_->add(recycledSamples_);

	// 这些回收样本点是我们当前仅有的新采样点。
	newSamples_ = recycledSamples_;

	// 重置回收样本点集合 recycledSamples_ 
	recycledSamples_.clear();

	// approximationId_ 记录当前近似/采样的次数 
	++(*approximationId_);

	// 上述代码来看，算法在这一步实际上并没有采样新的一批点。
	// 注释提示：调用 nearestSamples() 会实际填上新的采样点。
	// We don't add actual *new* samples until the next time "nearestSamples" is called. This is to support JIT
	// sampling.
}

注意，这里实际上添加入样本点集合ImplicitGraph::samples_ 的点只有回收采样点集合 ImplicitGraph::recycledSamples_。
同样的新采样点集合ImplicitGraph::newSamples_ 也只有回收点集合。
实际的批采样点将会在其实际用到的时候在进行采样获得。

函数insertOutgoingEdgesOfStartVertices()向边队列中添加的是哪些边？

该函数在 solve中实际已经调用过一次。
是由起始点 到 邻域所有采样点的连接边，不仅包括新采样的点也包括以往的采样点。

 // 将起点的邻域出边加入边队列中去
void BITstar::SearchQueue::insertOutgoingEdgesOfStartVertices()
{
	ASSERT_SETUP
	// 遍历所有的起点顶点,并将其出边加入到边队列中
	// 其中 ImplicitGraph::startVerticesBeginConst() 与 ImplicitGraph::startVerticesEndConst 分别是 起点集合 startVertices_ 的首末迭代器。
	for (auto it = graphPtr_->startVerticesBeginConst(); it != graphPtr_->startVerticesEndConst(); ++it) 
		{ this->insertOutgoingEdges(*it); }
}

// 将顶点的邻域出边加入到边队列中去
void BITstar::SearchQueue::insertOutgoingEdges(const VertexPtr &vertex)
{
	// 如果顶点有改善解的可能性(代价+启发值 < 当前解代价)
	if (this->canPossiblyImproveCurrentSolution(vertex))
	{
		// 获取邻域采样点 
		// 这一步 也包含批采样点的生成过程。
		VertexPtrVector neighbourSamples;
		graphPtr_->nearestSamples(vertex, &neighbourSamples);

		//将边<顶点,邻域点>加入到边队列中去
		this->enqueueEdges(vertex, neighbourSamples);
	}
}
 void BITstar::ImplicitGraph::nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
 {
     ASSERT_SETUP

     // 如果没有进行批采样，那么进行批采样，并添加入采样点集合。
     this->updateSamples(vertex);

     // 跟踪进行了几次邻域搜索  
     ++numNearestNeighbours_;	
     // 从samples_中生成邻域点集合 neighbourSamples
     if (useKNearest_)
     {
         samples_->nearestK(vertex, k_, *neighbourSamples);
     }
     else
     {
         samples_->nearestR(vertex, r_, *neighbourSamples);
     }
 }


 // Private functions:
 void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
 {
     // requiredCost = 无穷大 
     ompl::base::Cost requiredCost = this->calculateNeighbourhoodCost(vertex);

     // sampledCost_在 ImplicitGraph::addNewSamples() 赋值为  minCost_
 	 // minCost_ 在 ImplicitGraph::updateStartAndGoalStates() 中赋值为 有限值
 	 // 即每次调用 ImplicitGraph::addNewSamples() 可以出发该if  
 	 // 且执行过后 sampledCost_ 赋值为 requiredCost = 无穷大, 不会重复执行批采样。
     if (costHelpPtr_->isCostBetterThan(sampledCost_, requiredCost)) 
     {
         unsigned int numRequiredSamples = 0u;

         // useJustInTimeSampling_ 默认为0,忽略该部分
         if (useJustInTimeSampling_)  { ... }
         else
         {
         	// 一次生成 单批所需的所有采样点：
         	// numSamples_ 是现有的采样点数量 
         	// numNewSamplesInCurrentBatch_ 在 ImplicitGraph::addNewSamples() 赋值为单批采样的数量。
         	// numRequiredSamples 是采样后一共需要的点数量
             numRequiredSamples = numSamples_ + numNewSamplesInCurrentBatch_;
         }

         // 实际的批采样过程 
         // numSamples_ 跟踪采样点数量。
         VertexPtrVector newStates{};
         newStates.reserve(numRequiredSamples);
         for (std::size_t tries = 0u;
              tries < averageNumOfAllowedFailedAttemptsWhenSampling_ * numRequiredSamples &&  numSamples_ < numRequiredSamples;
              ++tries)
         {
             auto newState = std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_);
             if (sampler_->sampleUniform(newState->state(), sampledCost_, requiredCost))
             {
                 ++numStateCollisionChecks_;
                 if (spaceInformation_->isValid(newState->state()))
                 {
                     newStates.push_back(newState);

                     // Update the number of uniformly distributed states
                     ++numUniformStates_;

                     // Update the number of sample
                     ++numSamples_;
                 }
                 // No else
             }
         }

         // Add the new state as a sample.
         // 点集合 newSamples_  添加入新采样点
         // 最邻近结构 samples_  添加入新采样点
         this->addToSamples(newStates);

         // 标志位重新置为无穷大
         sampledCost_ = requiredCost;
     }
 }

六，对不一致点集进行搜索

注意，不一致点集 inconsistentVertices_ 会在搜索路径时更改graph结构时，出现代价前后不一致时，得到填充。
而这一步又重新遍历不一致点集，连接不一致点与其邻域点加入边队列中。
（但为什么算法先排序后添边？）

{
    // inflationScalingParameter_ 默认为0,实际膨胀系数为1,即不膨胀
    queuePtr_->setInflationFactor( 1.0 + inflationScalingParameter_ / (static_cast<float>(graphPtr_->numVertices() + graphPtr_->numSamples())));

    // 1. 更新队列中所有的边，更新边对应的key值，并根据key值重新构建队列。
    queuePtr_->rebuildEdgeQueue();
    // 2. 将 边队列 中的 inconsistentVertices_ 引出的边添加入边队列中
    queuePtr_->insertOutgoingEdgesOfInconsistentVertices();
    // 3. 清空 边队列 中的 inconsistentVertices_
    queuePtr_->clearInconsistentSet();
    isFinalSearchOnBatch_ = true;
}

// 使用到的三个函数 摘自 SearchQueue.cpp 
 void BITstar::SearchQueue::rebuildEdgeQueue()
 {
	// 1.取得边队列中所有的元素<key,edge>
	std::vector<SortKeyAndVertexPtrPair> contentCopy; // 一个 <key,edge>的向量
	edgeQueue_.getContent(contentCopy); // 从边队列中取出所有的<key,edge> 


	// 2. 取得所有边的父节点。
	// element是<key,edge> ,element.second.first 是edge的父顶点,即 parents 存储了所有边的父节点
	std::set<VertexPtr> parents;
	for (const auto &element : contentCopy) { parents.insert(element.second.first); }


	// 更新所有元素的key值
	// parent 的类型为 EdgeQueue::Element* ,即  <key,edge> 
	// 函数edgeQueueOutLookupConstBegin()到函数edgeQueueOutLookupConstEnd() 循环的是顶点的出边。
	// 根据parent的edge的质量(估计代价) 更新 parent的key值 
	for (const auto &parent : parents)
	{
		for (auto it = parent->edgeQueueOutLookupConstBegin(); it != parent->edgeQueueOutLookupConstEnd(); ++it)
		{ (*it)->data.first = this->createSortKey((*it)->data.second);  }
	}

     // 当所有的元素都更新了key值后，重建队列
     edgeQueue_.rebuild();
 }


void BITstar::SearchQueue::insertOutgoingEdgesOfInconsistentVertices()
{
    // Insert all outgoing edges of the inconsistent vertices.
    // 循环 SearchQueue 中 inconsistentVertices_ 中的所有顶点，
    // 考虑该顶点的所有邻近节点，并将顶点引出的所有边添入 SearchQueue 中。
    for (const auto &vertex : inconsistentVertices_)
    {
        this->insertOutgoingEdges(vertex);
    }
}

void BITstar::SearchQueue::clearInconsistentSet()
{
	 inconsistentVertices_.clear();
}

七 从队列中弹出最优的边

VertexPtrPair edge = queuePtr_->popFrontEdge();

八 如果当前边已经在路径树中

因为添加边时，考虑了新旧所有采样点，一定会添加进过去已经遍历过的边。

if (edge.second->hasParent() && edge.second->getParent()->getId() == edge.first->getId())
{
	// 如果父节点在本次搜索中未被扩展，那么记录为扩展 
    if (!edge.first->isExpandedOnCurrentSearch()) { edge.first->registerExpansion(); }
    // 边队列 插入 所有子节点引出的边 (没有找到修改近似id的过程)
    queuePtr_->insertOutgoingEdges(edge.second);
}

void BITstar::SearchQueue::insertOutgoingEdges(const VertexPtr &vertex)
{

	//通过启发函数确定该顶点是否有改善解的可能性
	if (this->canPossiblyImproveCurrentSolution(vertex)) 
	{
		// Get the neighbouring samples.
		VertexPtrVector neighbourSamples;
		// vertex的邻域中所有样本点。
		graphPtr_->nearestSamples(vertex, &neighbourSamples);

		// 向边队列中添加边
		this->enqueueEdges(vertex, neighbourSamples);
	}
}

九 如果当前边有改善解的可能性

// currentHeuristicEdge(edge) = 父节点的成本cost_to_come + 边的估计成本 + 子节点的启发成本cost_to_go 
// 如果  currentHeuristicEdge(edge)*truncationFactor_ 优于  bestCost_  (truncationFactor_默认为1)
else if (costHelpPtr_->isCostBetterThan( costHelpPtr_->inflateCost(costHelpPtr_->currentHeuristicEdge(edge), truncationFactor_), bestCost_))
{

	// currentHeuristicToTarget(edge) = 父节点成本cost_to_come + 边缘的估计成本
	// 如果 currentHeuristicToTarget(edge) 优于 边的子节点的成本， 即 这个边的出现可能改善当前图的结构。
    if (costHelpPtr_->isCostBetterThan(costHelpPtr_->currentHeuristicToTarget(edge), edge.second->getCost()))
    {
        // 计算边的真实代价 
        ompl::base::Cost trueEdgeCost = costHelpPtr_->trueEdgeCost(edge);

        // 在考虑边的真实代价下,该边仍有可能改善解
        if (costHelpPtr_->isCostBetterThan( costHelpPtr_->combineCosts(costHelpPtr_->costToComeHeuristic(edge.first), trueEdgeCost, costHelpPtr_->costToGoHeuristic(edge.second)), bestCost_))
        {
            // 碰撞检测
            if (this->checkEdge(edge))
            {
                // 该边无碰撞，将其加入白名单
                this->whitelistEdge(edge); 

                // Does the current edge improve our graph?
                // 在考虑边的真实代价下，能否改善graph结构
                if (costHelpPtr_->isCostBetterThan( costHelpPtr_->combineCosts(edge.first->getCost(), trueEdgeCost), edge.second->getCost())) {
                    // YAAAAH. Add the edge! Allowing for the sample to be removed from free if it is not currently connected and otherwise propagate cost updates to descendants. 
                    // addEdge will update the queue and handle the extra work that occurs if this edge improves the solution.
                    this->addEdge(edge, trueEdgeCost);

                    // If the path to the goal has changed, we will need to update the cached info about the solution cost or solution length:
                    this->updateGoalVertex();

                    // If this is the first edge that's being expanded in the current search, remember
                    // the cost-to-come and the search / approximation ids.
                    if (!edge.first->isExpandedOnCurrentSearch())
                    {
                        edge.first->registerExpansion();
                    }
                }
                // No else, this edge may be useful at some later date.
            }
            // 检测到碰撞、放入黑名单
            else{this->blacklistEdge(edge);}
        }
        // No else, we failed
    }
    // No else, we failed
}


void BITstar::addEdge(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost)
{
    // 如果边的子节点已经有父节点,这是一次重连,替换子节点的父节点
    if (edge.second->hasParent()){ this->replaceParent(edge, edgeCost); }   
    // 不是重连，直接添加父子关系
    else {
        edge.second->addParent(edge.first, edgeCost);
        edge.first->addChild(edge.second);
        graphPtr_->registerAsVertex(edge.second); // 将子节点 添加入 隐式图中。
    }

    // If the vertex hasn't already been expanded, insert its outgoing edges
    if (!edge.second->isExpandedOnCurrentSearch()) // 如果子节点未被扩展过，那么将子节点与邻居点连线成边，添入边队列中
    {
        queuePtr_->insertOutgoingEdges(edge.second);
    }
    else  // 如果子节点已经被扩展过，由于子节点代价的变化使得其后的路径成本不再正确，将子节点放入不一致集合。
    { queuePtr_->addToInconsistentSet(edge.second); }
}


void BITstar::updateGoalVertex()
{ 
    // 标志位初始化为false
    bool goalUpdated = false; 

    // newBestGoal 初始化为旧的目标顶点
    VertexConstPtr newBestGoal = curGoalVertex_;

    // newCost 初始化为旧的最佳代价
    ompl::base::Cost newCost = bestCost_;  

    // 循环所有目标顶点， （一般只有一个目标点）
    for (auto it = graphPtr_->goalVerticesBeginConst(); it != graphPtr_->goalVerticesEndConst(); ++it)
    { 
        if ((*it)->isInTree())// 如果目标点在树中
        {
            // 现存的是否有解 
            if (static_cast<bool>(newBestGoal))
            {
                // There is already a solution, is it to this goal?
                // 目标顶点没有变化(循环过程中算法会检测问题输入,去确认目标点是否变化,暂不考虑)
                if ((*it)->getId() == newBestGoal->getId()) 
                {
                    // Ah-ha, We meet again! Are we doing any better? We check the length as sometimes the path
                    // length changes with minimal change in cost.
                    if (!costHelpPtr_->isCostEquivalentTo((*it)->getCost(), newCost) ||
                        ((*it)->getDepth() + 1u) != bestLength_)
                    {
                        // The path to the current best goal has changed, so we need to update it.
                        goalUpdated = true;
                        newBestGoal = *it;
                        newCost = newBestGoal->getCost();
                    }
                    // No else, no change
                }
                else  // 目标顶点不一样。 
                {
                    // It is not to this goal, we have a second solution! What an easy problem... but is it better?
                    if (costHelpPtr_->isCostBetterThan((*it)->getCost(), newCost))
                    {
                        // It is! Save this as a better goal:
                        goalUpdated = true;
                        newBestGoal = *it;
                        newCost = newBestGoal->getCost();
                    }
                    // No else, not a better solution
                }
            }
            else  // 现存没有解，那么说明当前发现的解为新解 
            {
                // There isn't a preexisting solution, that means that any goal is an update:
                goalUpdated = true;  // 标志位置位1
                newBestGoal = *it;   // 解 记录当前的目标顶点 
                newCost = newBestGoal->getCost(); // newCost 记录解的代价 
            }
        }
        // No else, can't be a better solution if it's not in the spanning tree, can it?
    }

    // Did we update the goal?

    if (goalUpdated) // 如果我们更新了解 
    {
        // Mark that we have a solution
        hasExactSolution_ = true; // 拥有解 

        // Store the current goal
        curGoalVertex_ = newBestGoal; // 记录 目标顶点 

        // Update the best cost:
        bestCost_ = newCost;  // 更新代价 

        // and best length
        bestLength_ = curGoalVertex_->getDepth() + 1u; // 更新长度 

        // Tell everyone else about it.
        queuePtr_->registerSolutionCost(bestCost_);
        graphPtr_->registerSolutionCost(bestCost_);

        // Stop the solution loop if enabled:
        stopLoop_ = stopOnSolutionChange_;

        // Brag:
        this->goalMessage();

        // If enabled, pass the intermediate solution back through the call back:
        if (static_cast<bool>(Planner::pdef_->getIntermediateSolutionCallback()))
        {
            Planner::pdef_->getIntermediateSolutionCallback()(this, this->bestPathFromGoalToStart(), bestCost_);
        }
    }
    // No else, the goal didn't change
}

十 剪枝

void BITstar::prune()
{
    // If we don't have an exact solution, we can't prune sensibly.
    if (hasExactSolution_)
    {
        // 计算可被修剪得采样点数量  
        auto samples = graphPtr_->getCopyOfSamples();
        unsigned int numSamplesThatCouldBePruned(0u);
        for (const auto &sample : samples)
        {
            if (graphPtr_->canSampleBePruned(sample))
            {
                ++numSamplesThatCouldBePruned;
            }
        }
        // 我们当  可被修建得采样点数量/(采样点数量+顶点数量) >= pruneFraction_(默认为0.05) 
        // 即当可被修建得数量达到一定比例时,我们才进行修建 
        if (static_cast<float>(numSamplesThatCouldBePruned) / static_cast<float>(graphPtr_->numSamples() + graphPtr_->numVertices()) >= pruneFraction_)
        {
            double informedMeasure = graphPtr_->getInformedMeasure(bestCost_);
            // 追踪剪枝的次数
            ++numPrunings_;
            // 对隐式图中的采样点进行剪枝 
            std::pair<unsigned int, unsigned int> numPruned = graphPtr_->prune(informedMeasure);
            // 记录剪枝时的最佳样本。
            prunedCost_ = bestCost_;

            // 记录剪枝量
            prunedMeasure_ = informedMeasure;

            OMPL_INFORM("%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.", Planner::getName().c_str(), numPruned.first, numPruned.second);
        }
    }
    // No else.
}


std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::prune(double prunedMeasure)
{
	ASSERT_SETUP

	// Store the measure of the problem I'm approximating
	approximationMeasure_ = prunedMeasure;

	// 剪枝起点和终点(不考虑)
	std::pair<unsigned int, unsigned int> numPruned = this->pruneStartAndGoalVertices();

	// 剪枝采样点
	numPruned = numPruned + this->pruneSamples();

	return numPruned;
}


 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneSamples()
 {
     std::pair<unsigned int, unsigned int> numPruned{0u, 0u};
     VertexPtrVector samples;
     samples_->list(samples);
     // 默认为 false
     if (dropSamplesOnPrune_) 
     {
     	...
     }
     else
     {
         // Iterate through the vector and remove any samples that should not be in the graph.
         for (const auto &sample : samples)
         {
             if (sample->isInTree()) // 采样点在路径树中，即其已经时graph中的顶点。
             {

             	// 该顶点能否剪枝——顶点的cost_to_come+启发成本 > 当前最佳成本
                 if (this->canVertexBeDisconnected(sample))
                 {
                     numPruned = numPruned + this->pruneVertex(sample);
                 }
             }
             // Check if this state should be pruned.
             else if (this->canSampleBePruned(sample))
             {
                 // It should, remove the sample from the NN structure.
                 this->pruneSample(sample);

                 // Keep track of how many are pruned.
                 ++numPruned.second;
             }
             // No else, keep sample.
         }
     }

     return numPruned;
 }

//cost_to_come + 启发式cost_to_go > 当前最佳解代价
bool BITstar::ImplicitGraph::canVertexBeDisconnected(const VertexPtr &vertex) const
{
ASSERT_SETUP
return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(vertex), solutionCost_);
}

// 计算采样点/顶点的成本下界——启发式的cost-to-come + 启发式cost-to-go 
bool BITstar::ImplicitGraph::canSampleBePruned(const VertexPtr &sample) const
{
ASSERT_SETUP
return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(sample),solutionCost_);
}


 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneVertex(const VertexPtr &vertex)
 {
     ASSERT_SETUP

	 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when this function is given an element of the maintained set as the argument)
     VertexPtr vertexCopy(vertex); // 为要剪枝的顶点创建一个副本
 
 	 // 将其从不一致集合中删除
     if (!vertexCopy->isConsistent()) { queuePtr_->removeFromInconsistentSet(vertexCopy); }

     // Disconnect from parent if necessary, not cascading cost updates.
     // 断开与父亲节点的连接,且不更新级联成本
     if (vertexCopy->hasParent()) { this->removeEdgeBetweenVertexAndParent(vertexCopy, false); }

     // 断开所有的孩子节点；从不一致集合中剔除所有孩子；从边队列中删除所有孩子的出边
     VertexPtrVector children;
     vertexCopy->getChildren(&children);
     for (const auto &child : children)
     {
         // Remove this edge.
         vertexCopy->removeChild(child);
         child->removeParent(false);

         // If the child is inconsistent, it needs to be removed from the set of inconsistent vertices.
         if (!child->isConsistent())
         {
             queuePtr_->removeFromInconsistentSet(child);
         }

         // If the child has outgoing edges in the queue, they need to be removed.
         queuePtr_->removeOutEdgesConnectedToVertexFromQueue(child);
     }

     // 从边队列中 删除该顶点的出边
     queuePtr_->removeAllEdgesConnectedToVertexFromQueue(vertexCopy);

     // samples_ 中删除该顶点.
     samples_->remove(vertexCopy);
 
     // 查看该顶点对应的样本点是否需要 被剪枝 
     if (this->canSampleBePruned(vertexCopy))
     {
         // It's not even useful as sample, mark it as pruned.
         vertexCopy->markPruned();
         return {0, 1};  // The vertex is actually removed.
     }
     else
     { 
         recycleSample(vertexCopy); // 将该样本点重新利用起来 
         return {1, 0};  // The vertex is only disconnected and recycled as sample.
     }
 }


void BITstar::ImplicitGraph::pruneSample(const VertexPtr &sample)
 {
     ASSERT_SETUP
 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
 // this function is given an element of the maintained set as the argument)
     VertexPtr sampleCopy(sample);
     // Remove all incoming edges from the search queue.
     queuePtr_->removeInEdgesConnectedToVertexFromQueue(sampleCopy);

     // Remove from the set of samples
     samples_->remove(sampleCopy);

     // Increment our counter
     ++numFreeStatesPruned_;

     // Mark the sample as pruned
     sampleCopy->markPruned();
 }