rpg_ig_active源码阅读(七)

接下来主要看rpg_ig_active_reconstruction
这一部分应该会比较复杂

整理了一下octomap里的各个类模板

只有一个可执行的节点，主要看octomap_world_representation.cpp
今天的目标主要看三个有关的
一个是IgTree 这个是类模板WorldRepresentation具体用的节点类型
一个是RosInterface 这个是和ros结合的接口
一个是StdPclInputPointXYZ 这个是与输入有关的设置

IgTree

namespace octomap下的一个类IgTreeNode 是继承的octomap::OcTreeNode
定义了一些关于节点Node的属性，比较重要的三个参数有

• double occ_dist_ 如果节点被遮挡 设置为到附近的已注册的被遮挡的占用节点的最短距离
• double max_dist_ 更新遮挡的最大距离
• bool has_no_measurement_
  并将类IgTree设为友元
  IgTree 则是继承了octomap::OccupancyOcTreeBase<IgTreeNode>
  里面重要的是一个config 一个expandNode

struct config

    struct Config
    {
    public:
      Config();

    public:
      double resolution_m; //! OcTree leaf node size, default: 0.1 [m]. 节点大小 默认是0.1m
      double occupancy_threshold; //! 占用概率阈值Occupancy probability over which nodes are considered occupied, default: 0.5 [range 0-1].
      double hit_probability; //! 匹配的概率更新值Probability update value for hits, default 0.7 [range 0-1].
      double miss_probability; //! 未命中概率更新值 Probability update value for misses, default 0.4 [range 0-1].
      double clamping_threshold_min; //! 固定概率的最小概率阈值 Min probability threshold over which the probability is clamped, default: 0.12, range [0-1].
      double clamping_threshold_max; //! Max probability threshold over which the probability is clamped, default: 0.97, range [0-1].
    };

updateOctreeConfig函数则是将这些参数传入octomap里，在构造函数中被调用

  void IgTree::updateOctreeConfig()
  {
    // 这几个函数是ros里octomap的函数 是把config里的各个概率参数传入octomap里
    setOccupancyThres(config_.occupancy_threshold);
    setProbHit(config_.hit_probability);
    setProbMiss(config_.miss_probability);
    setClampingThresMin(config_.clamping_threshold_min);
    setClampingThresMax(config_.clamping_threshold_max);
  }

expandNode函数

  // 扩展节点
  void IgTree::expandNode(IgTreeNode* node)
  {
    assert(!nodeHasChildren(node));

    for (unsigned int k=0; k<8; k++) {
      IgTreeNode* child = createNodeChild(node, k);
      // 把node里的数值复制到了child里 具体包括value occ_dist_ max_dist_ 具体可以看octomap_ig_tree_node.cpp
      child->copyData(*node);
    }
  }

RosInterface

总的来说就是根据输入(输入看的是this->link_.octree)，把数据填充到rviz的marker里，然后发布名称为occupied_cells_vis_array的话题

这里只提供了到octomap::WorldRepresentation的ROS接口 与输入无关
这个类继承的是WorldRepresentation<TREE_TYPE>::LinkedObject
成员参数有ros::NodeHandle nh_ 和std::string world_frame_name_
其他重要的是一个struct config和publishVoxelMap函数 还有一个ros发布话题用的voxel_map_publisher_

struct config

    struct Config
    {
      ros::NodeHandle nh;
      std::string world_frame_name;
    };

就是来设置的

构造函数

  TEMPT
  CSCOPE::RosInterface(Config config)
  : nh_(config.nh)
  , world_frame_name_(config.world_frame_name)
  {
    voxel_map_publisher_ = nh_.advertise<visualization_msgs::MarkerArray>("occupied_cells_vis_array", 1);
  }

除了设置参数外 就是发布一个名称为occupied_cells_vis_array的消息

publishVoxelMap函数

主要是填充信息

// 在rviz中显示的marker
    visualization_msgs::MarkerArray occupiedNodesVis;

设置颜色和大小等等
还有设置frame和时间戳

      occupiedNodesVis.markers[i].header.frame_id = world_frame_name_;
      occupiedNodesVis.markers[i].header.stamp = ros::Time::now();

最后发布出去

voxel_map_publisher_.publish(occupiedNodesVis);

StdPclInputPointXYZ

很短的几行，一个类模板，其中定义了

• pcl类型 pcl::PointCloud<pcl::PointXYZ>非常标准的点云格式
• TreeType 树的类型 实际用的时候是IgTree
• 用了另外一个类模板StdPclInput，并传入了参数，实际是
  StdPclInput<IgTree, pcl::PointCloudpcl::PointXYZ>
• 上面类型的指针

  template<class TREE_TYPE>
  struct StdPclInputPointXYZ
  {
    typedef pcl::PointCloud<pcl::PointXYZ> PclType;
    typedef TREE_TYPE TreeType;
    typedef StdPclInput< TreeType, PclType > Type;
    typedef boost::shared_ptr< StdPclInput< TreeType, PclType > > Ptr;
  };

StdPclInput 继承的是PclInput

PclInput

template<class TREE_TYPE, class POINTCLOUD_TYPE>
  class PclInput: public WorldRepresentation<TREE_TYPE>::LinkedObject

• 一个虚函数push 插入一个新的点云
  virtual void push( const Eigen::Transform<double,3,Eigen::Affine>& sensor_to_world, POINTCLOUD_TYPE& pcl )=0;
• setOcclusionCalculator 设置遮挡计算方法的函数

StdPclInput

继承了PclInput<TREE_TYPE,POINTCLOUD_TYPE>

主要是实现了push函数
void push( const Eigen::Transform<double,3,Eigen::Affine>& sensor_to_world, POINTCLOUD_TYPE& pcl );
sensor_to_world 是传感器到世界坐标系的转换
pcl 是在传感器坐标系下的新的数据
目的就是转换到世界坐标系下然后插入到之前的数据后面