rpg_ig_active源码阅读(一)

最新推荐文章于 2021-01-20 13:31:40 发布

顾朝贝

最新推荐文章于 2021-01-20 13:31:40 发布

阅读量936

点赞数

分类专栏：源码阅读

本文链接：https://blog.csdn.net/weixin_42518636/article/details/110826333

版权

源码阅读专栏收录该内容

7 篇文章 0 订阅

订阅专栏

主要有四个launch文件

robot_interface.launch 打开gazebo环境，
flying_gazebo_stereo_cam.launch 打开立体相机
octomap_world_representation.launch 有一个ros节点是使用octomap为容器来表示环境，并且提供information gain的计算方法
basic_view_planner.launch 规划视野

今天看的主要是基础部分

octomap_information_gain.hpp

计算ig的方法:
在namespace octomap下

makeReadyForNewRay 清除上次射线的数据
includeRayMeasurement 计算每条射线经过的voxel，除了最后一个格子
includeEndPointMeasurement 计算最后一个格子
informAboutVoidRay 如果射线经过的是空的地图(未知的，没有初始化的)，调用该函数
getInformation 最后一条射线计算之后返回加和的ig

类utils里包含一个config结构
config里包括:

p_unknown_prior : Prior occupancy likelihood for unknown voxels 先前占用概率，默认是0.5
p_unknown_upper_bound : 认为是不确定性的上限 0.8
p_unknown_lower_bound: 不确定性的下限 0.2
voxels_in_void_ray : 一条射线最多考虑的格子，默认100
还有其他函数
entropy( typename TREE_TYPE::NodeType* voxel ) 返回一个格子的熵
double entropy( double likelihood ) 给定不确定性后计算熵
pOccupancy( typename TREE_TYPE::NodeType* voxel ) 经过的格子的占据概率

具体的实现在octomap_information_gain.inl里
关于inl的使用可以参考这个博客
在这里是用于存放模板类模板函数的定义
比较大的工程，声明放一个文件里，定义放inl文件，只需要在头文件里包含就可以了

octomap_information_gain.inl

其实就实现了一个有用的entropy，计算熵的函数
根据占用概率计算熵

 double CSCOPE::Utils::entropy( double likelihood )
  {
    double p_free = 1-likelihood;
    //如果占用概率为0或者1，熵都为0
    if(likelihood==0 || p_free==0)
    {
	return 0;
    }
    ig的公式为 -p*log(p)-(1-p)*log(1-p)
    double vox_ig = -likelihood*log(likelihood)-p_free*log(p_free);
    return vox_ig;
  }

occlusion_aware.inl

实现了这几个函数

makeReadyForNewRay 清除上次射线的数据把p_vis_设置为1.0
reset : ig_ = 0; p_vis_ = 1; voxel_count_ = 0;
includeRayMeasurement 计算每条射线经过的voxel，除了最后一个格子
includeEndPointMeasurement 计算最后一个格子
都调用的这个函数includeMeasurement(node)
informAboutVoidRay 如果射线经过的是空的地图(未知的，没有初始化的)，调用该函数
getInformation 最后一条射线计算之后返回加和的ig
includeMeasurement(node) : 根据占用概率算出熵，熵和p_vis_相乘作为ig与之前计算的ig相加，p_vis_是用概率一直相乘

     ++voxel_count_;
    double p_occ = utils_.pOccupancy(node); 
    double vox_ent = utils_.entropy(p_occ);  
    ig_ += p_vis_*vox_ent; 
    p_vis_ *= p_occ;

变量有