关于ego-planner里面的GridMap

浙大这套开源的代码写得很nice 很值得借鉴 ， 对于 GridMap 类的实现。该类通过智能指针的封装简化了 GridMap 实例的创建和管理过程。一旦通过 GridMap::initMap(ros::NodeHandle &nh) 方法初始化，就可以方便地调用 GridMap 及其所有相关功能

它主要订阅了d435i的深度话题以及odom来实现的建图

 typedef std::shared_ptr<GridMap> Ptr;

单独写一个node试一下

#include <ros/ros.h>
#include "plan_env/grid_map.h"


GridMap::Ptr   grid_map_test_ ; 

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gridmap_test_node");
    ros::NodeHandle nh_("~");

    grid_map_test_.reset(new GridMap) ;
    grid_map_test_->initMap(nh_) ; 

    ros::spin();
  
    return 0;
}

然后再在原来的基础上搞一个launch

<launch>

<node pkg="ego_planner" name="gridmap_test_node" type="gridmap_test_node" output="screen">

    <remap from="~grid_map/odom" to="/mavros/local_position/odom"/>
    <remap from="~grid_map/cloud" to="nouse2"/>
    <remap from="~grid_map/pose"   to = "nouse1"/> 
    <remap from="~grid_map/depth" to = "/iris/realsense/depth_camera/depth/image_raw"/>

    <param name="grid_map/resolution"      value="0.15" /> 
    <param name="grid_map/map_size_x"   value="50" /> 
    <param name="grid_map/map_size_y"   value="25" /> 
    <param name="grid_map/map_size_z"   value="3.0" /> 

    <param name="grid_map/local_update_range_x"  value="5.5" /> 
    <param name="grid_map/local_update_range_y"  value="5.5" /> 
    <param name="grid_map/local_update_range_z"  value="4.5" /> 
    <param name="grid_map/obstacles_inflation"     value="0.299" /> 
    <param name="grid_map/local_map_margin" value="10"/>
    <param name="grid_map/ground_height"        value="-0.01"/>
    <!-- camera parameter -->

    <param name="grid_map/cx" value="376.0"/>
    <param name="grid_map/cy" value="240.0"/>
    <param name="grid_map/fx" value="319.9988245765257"/>
    <param name="grid_map/fy" value="319.9988245765257"/>
    
    <!-- depth filter -->
    <param name="grid_map/use_depth_filter" value="true"/>
    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
    <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
    <param name="grid_map/depth_filter_mindist"   value="0.2"/>
    <param name="grid_map/depth_filter_margin"    value="2"/>
    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
    <param name="grid_map/skip_pixel" value="2"/>
    <!-- local fusion -->
    <param name="grid_map/p_hit"  value="0.65"/>
    <param name="grid_map/p_miss" value="0.35"/>
    <param name="grid_map/p_min"  value="0.12"/>
    <param name="grid_map/p_max"  value="0.90"/>
    <param name="grid_map/p_occ"  value="0.80"/>
    <param name="grid_map/min_ray_length" value="0.3"/>
    <param name="grid_map/max_ray_length" value="5.0"/>

    <param name="grid_map/visualization_truncate_height"   value="1.8"/>
    <param name="grid_map/show_occ_time"  value="false"/>
    <param name="grid_map/pose_type"     value="2"/>  
    <param name="grid_map/frame_id"      value="world"/>
	
 </node>

<node pkg="odom_visualization" name="drone_odom_visualization" type="odom_visualization" output="screen">
    <remap from="~odom" to="/mavros/local_position/odom"/>
    <param name="color/a" value="1.0"/>    
    <param name="color/r" value="0.0"/>        
    <param name="color/g" value="0.0"/>        
    <param name="color/b" value="0.0"/>       
    <param name="covariance_scale" value="100.0"/>       
    <param name="robot_scale" value="1.0"/>
    <param name="tf45" value="false"/>
    <param name="drone_id" value="0"/>
  </node>

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/gridmap_test.rviz" required="true" />

</launch>

最后用gazebo进行仿真
在这里插入图片描述

1. 在整个ego中调用的地方

// 位于/home/Fast-Drone-250/src/planner/plan_manage/src/planner_manager.cpp下的29行 
    grid_map_.reset(new GridMap);
    //GridMap::Ptr grid_map_; 在h文件声明 ， 这里使用了c++ 智能指针 ， 将grid_map_ 指向一个新的对象 原来的对象（如果有的话）将被销毁。
    grid_map_->initMap(nh);//传入nh 开始初始化Gridmap
    
    bspline_optimizer_->a_star_.reset(new AStar);
    bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100));//将该指针传入到a_star_->initGridMap

2. 初始化栅格地图GridMap::initMap(ros::NodeHandle &nh)

初始化地图的相关参数(mp_) 以及数据结构(md_) ， 以及ros的回调函数以及消息发布器

void GridMap::initMap(ros::NodeHandle &nh)
{
  node_ = nh;

  /* get parameter */
  double x_size, y_size, z_size;
  node_.param("grid_map/resolution", mp_.resolution_, -1.0);
  node_.param("grid_map/map_size_x", x_size, -1.0);
  node_.param("grid_map/map_size_y", y_size, -1.0);
  node_.param("grid_map/map_size_z", z_size, -1.0);
  node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0);
  node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0);
  node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0);
  node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0);

  node_.param("grid_map/fx", mp_.fx_, -1.0);
  node_.param("grid_map/fy", mp_.fy_, -1.0);
  node_.param("grid_map/cx", mp_.cx_, -1.0);
  node_.param("grid_map/cy", mp_.cy_, -1.0);

  node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true);
  node_.param("grid_map/depth_filter_tolerance", mp_.depth_filter_tolerance_, -1.0);
  node_.param("grid_map/depth_filter_maxdist", mp_.depth_filter_maxdist_, -1.0);
  node_.param("grid_map/depth_filter_mindist", mp_.depth_filter_mindist_, -1.0);
  node_.param("grid_map/depth_filter_margin", mp_.depth_filter_margin_, -1);
  node_.param("grid_map/k_depth_scaling_factor", mp_.k_depth_scaling_factor_, -1.0);
  node_.param("grid_map/skip_pixel", mp_.skip_pixel_, -1);

  node_.param("grid_map/p_hit", mp_.p_hit_, 0.70);
  node_.param("grid_map/p_miss", mp_.p_miss_, 0.35);
  node_.param("grid_map/p_min", mp_.p_min_, 0.12);
  node_.param("grid_map/p_max", mp_.p_max_, 0.97);
  node_.param("grid_map/p_occ", mp_.p_occ_, 0.80);
  node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1);
  node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1);

  node_.param("grid_map/visualization_truncate_height", mp_.visualization_truncate_height_, -0.1);
  node_.param("grid_map/virtual_ceil_yp", mp_.virtual_ceil_yp_, -0.1);
  node_.param("grid_map/virtual_ceil_yn", mp_.virtual_ceil_yn_, -0.1);

  node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false);
  node_.param("grid_map/pose_type", mp_.pose_type_, 1);

  node_.param("grid_map/frame_id", mp_.frame_id_, string("world"));
  node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1);
  node_.param("grid_map/ground_height", mp_.ground_height_, 1.0);

  node_.param("grid_map/odom_depth_timeout", mp_.odom_depth_timeout_, 1.0);

  mp_.resolution_inv_ = 1 / mp_.resolution_;
  mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);
  mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);
  
//针对地图概率的参数，计算其对应的对数概率值
  mp_.prob_hit_log_ = logit(mp_.p_hit_);
  mp_.prob_miss_log_ = logit(mp_.p_miss_);
  mp_.clamp_min_log_ = logit(mp_.p_min_);
  mp_.clamp_max_log_ = logit(mp_.p_max_);
  mp_.min_occupancy_log_ = logit(mp_.p_occ_);
  mp_.unknown_flag_ = 0.01;

  cout << "hit: " << mp_.prob_hit_log_ << endl;
  cout << "miss: " << mp_.prob_miss_log_ << endl;
  cout << "min log: " << mp_.clamp_min_log_ << endl;
  cout << "max: " << mp_.clamp_max_log_ << endl;
  cout << "thresh log: " << mp_.min_occupancy_log_ << endl;

  for (int i = 0; i < 3; ++i)
    mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);

  mp_.map_min_boundary_ = mp_.map_origin_;
  mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;

  //初始化数据 ， 分配内存

  int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);

  md_.occupancy_buffer_ = vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);
  md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0);

  md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);
  md_.count_hit_ = vector<short>(buffer_size, 0);
  md_.flag_rayend_ = vector<char>(buffer_size, -1);
  md_.flag_traverse_ = vector<char>(buffer_size, -1);

  md_.raycast_num_ = 0;

  md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_);
  md_.proj_points_cnt = 0;

  md_.cam2body_ << 0.0, 0.0, 1.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, -1.0, 0.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

  /* init callback */

  depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "grid_map/depth", 50));
  
  extrinsic_sub_ = node_.subscribe<nav_msgs::Odometry>(
      "/vins_fusion/extrinsic", 10, &GridMap::extrinsicCallback, this); //sub

  if (mp_.pose_type_ == POSE_STAMPED)
  {
    pose_sub_.reset(
        new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "grid_map/pose", 25));

    sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(
        SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));
    sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));
  }
  else if (mp_.pose_type_ == ODOMETRY)
  {
    odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay()));

    sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(
        SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_)); //消息同步器 
        
        /*它使用 message_filters::Synchronizer 类来将两个不同的消息流进行同步。
        这个同步器被配置为接受一个最大队列长度为 100 的消息缓冲区，并且使用 SyncPolicyImageOdom 作为同步策略。*/
        
    sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));
    //用于同步接收深度图像和里程计数据，并在数据匹配时调用 GridMap 类中的 depthOdomCallback 函数进行处理
  }

  // use odometry and point cloud
  indep_cloud_sub_ =
      node_.subscribe<sensor_msgs::PointCloud2>("grid_map/cloud", 10, &GridMap::cloudCallback, this);
  indep_odom_sub_ =
      node_.subscribe<nav_msgs::Odometry>("grid_map/odom", 10, &GridMap::odomCallback, this);
	//定时器 更新地图占据信息
  occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);
//可视化地图  
  vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this);
	//这里就是发布地图
	
  map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/occupancy", 10);
  map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/occupancy_inflate", 10);

  md_.occ_need_update_ = false;
  md_.local_updated_ = false;
  md_.has_first_depth_ = false;
  md_.has_odom_ = false;
  md_.has_cloud_ = false;
  md_.image_cnt_ = 0;
  md_.last_occ_update_time_.fromSec(0);

  md_.fuse_time_ = 0.0;
  md_.update_num_ = 0;
  md_.max_fuse_time_ = 0.0;

  md_.flag_depth_odom_timeout_ = false;
  md_.flag_use_depth_fusion = false;

  // rand_noise_ = uniform_real_distribution<double>(-0.2, 0.2);
  // rand_noise2_ = normal_distribution<double>(0, 0.2);
  // random_device rd;
  // eng_ = default_random_engine(rd());
}

其中在EGO中 使用的是pose_type_ == ODOMETRY 所以在使用的时候只需要订阅odom 以及depth 话题

  if (mp_.pose_type_ == POSE_STAMPED)
  {
    pose_sub_.reset(
        new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "grid_map/pose", 25));

    sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(
        SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));
    sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));
  }
  else if (mp_.pose_type_ == ODOMETRY)
  {
    odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay()));

    sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(
        SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));
    sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));
  }

	<param name="grid_map/pose_type"     value="2"/> 
	
	<arg name="odometry_topic" value="/vins_fusion/imu_propagate"/>
    <arg name="camera_pose_topic" value="nouse1"/>
    <arg name="depth_topic" value="/camera/depth/image_rect_raw"/>
    <arg name="cloud_topic" value="nouse2"/>
        
    <remap from="~grid_map/odom" to="$(arg odometry_topic)"/>
    <remap from="~grid_map/cloud" to="$(arg cloud_topic)"/>
    <remap from="~grid_map/pose"   to = "$(arg camera_pose_topic)"/> 
    <remap from="~grid_map/depth" to = "$(arg depth_topic)"/>

3.最主要的两个定时器

更新地图占据信息

  occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);

主要是更新地图的占据信息 包含的主要函数：
1. 更新深度信息：调用 projectDepthImage() 函数，将深度图像中的像素点投影到世界坐标系中，生成一系列3D点。
2. 射线投影：调用 raycastProcess() 函数，使用射线投影法将投影点与地图相交，更新地图的占据信息。
3. 如果本地地图已经更新，调用 clearAndInflateLocalMap() 函数，用于清除和膨胀本地地图。
（没怎么细看，以后有时间再回来看看）

可视化地图

  vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this);

这个定时器用来发布地图信息和膨胀后的地图信息 其中两个重要的函数是：
1.publishMap()
2.publishMapInflate

3.1 projectDepthImage()

主要是将深度信息中的像素点坐标转换到世界坐标系下


void GridMap::projectDepthImage()
{
  // md_.proj_points_.clear();
  md_.proj_points_cnt = 0; //清空存储投影点数组

  uint16_t *row_ptr;
  // int cols = current_img_.cols, rows = current_img_.rows;
  int cols = md_.depth_image_.cols;
  int rows = md_.depth_image_.rows;
  int skip_pix = mp_.skip_pixel_;

  double depth;
//相机的旋转矩阵 将深度信息中的像素点从相机转到世界坐标系
  Eigen::Matrix3d camera_r = md_.camera_r_m_;

  if (!mp_.use_depth_filter_)
  {		//遍历像素点
    for (int v = 0; v < rows; v+=skip_pix)
    {
      row_ptr = md_.depth_image_.ptr<uint16_t>(v);

      for (int u = 0; u < cols; u+=skip_pix)
      {

        Eigen::Vector3d proj_pt;
        depth = (*row_ptr++) / mp_.k_depth_scaling_factor_;
        proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_;
        proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_;
        proj_pt(2) = depth;

        proj_pt = camera_r * proj_pt + md_.camera_pos_;

        if (u == 320 && v == 240)
          std::cout << "depth: " << depth << std::endl;
        md_.proj_points_[md_.proj_points_cnt++] = proj_pt;
      }
    }
  }
  /* use depth filter */
  else
  {

    if (!md_.has_first_depth_)
      md_.has_first_depth_ = true;
    else
    {
      Eigen::Vector3d pt_cur, pt_world, pt_reproj;

      Eigen::Matrix3d last_camera_r_inv;
      last_camera_r_inv = md_.last_camera_r_m_.inverse();
      const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_;

      for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_)
      {
        row_ptr = md_.depth_image_.ptr<uint16_t>(v) + mp_.depth_filter_margin_;

        for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_;
             u += mp_.skip_pixel_)
        {

          depth = (*row_ptr) * inv_factor;
          row_ptr = row_ptr + mp_.skip_pixel_;

          // filter depth
          // depth += rand_noise_(eng_);
          // if (depth > 0.01) depth += rand_noise2_(eng_);

          if (*row_ptr == 0)
          {
            depth = mp_.max_ray_length_ + 0.1;
          }
          else if (depth < mp_.depth_filter_mindist_)
          {
            continue;
          }
          else if (depth > mp_.depth_filter_maxdist_)
          {
            depth = mp_.max_ray_length_ + 0.1;
          }

          // project to world frame
          pt_cur(0) = (u - mp_.cx_) * depth / mp_.fx_;
          pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_;
          pt_cur(2) = depth;

          pt_world = camera_r * pt_cur + md_.camera_pos_;
          // if (!isInMap(pt_world)) {
          //   pt_world = closetPointInMap(pt_world, md_.camera_pos_);
          // }

          md_.proj_points_[md_.proj_points_cnt++] = pt_world;

          // check consistency with last image, disabled...
          if (false)
          {
            pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_);
            double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_;
            double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_;

            if (uu >= 0 && uu < cols && vv >= 0 && vv < rows)
            {
              if (fabs(md_.last_depth_image_.at<uint16_t>((int)vv, (int)uu) * inv_factor -
                       pt_reproj.z()) < mp_.depth_filter_tolerance_)
              {
                md_.proj_points_[md_.proj_points_cnt++] = pt_world;
              }
            }
            else
            {
              md_.proj_points_[md_.proj_points_cnt++] = pt_world;
            }
          }
        }
      }
    }
  }

  /* maintain camera pose for consistency check */

  md_.last_camera_pos_ = md_.camera_pos_;
  md_.last_camera_r_m_ = md_.camera_r_m_;
  md_.last_depth_image_ = md_.depth_image_;
}

3.2 raycastProcess（）

使用射线投影法将投影点与地图相交，更新地图的占据信息。


void GridMap::raycastProcess()
{
  // if (md_.proj_points_.size() == 0)
  if (md_.proj_points_cnt == 0)
    return;

  ros::Time t1, t2;

  md_.raycast_num_ += 1;

  int vox_idx;
  double length;

  // bounding box of updated region
  double min_x = mp_.map_max_boundary_(0);
  double min_y = mp_.map_max_boundary_(1);
  double min_z = mp_.map_max_boundary_(2);

  double max_x = mp_.map_min_boundary_(0);
  double max_y = mp_.map_min_boundary_(1);
  double max_z = mp_.map_min_boundary_(2);

  RayCaster raycaster;
  Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5);
  Eigen::Vector3d ray_pt, pt_w;

  for (int i = 0; i < md_.proj_points_cnt; ++i)
  {
    pt_w = md_.proj_points_[i];

    // set flag for projected point

    if (!isInMap(pt_w))
    {
      pt_w = closetPointInMap(pt_w, md_.camera_pos_);

      length = (pt_w - md_.camera_pos_).norm();
      if (length > mp_.max_ray_length_)
      {
        pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;
      }
      vox_idx = setCacheOccupancy(pt_w, 0);
    }
    else
    {
      length = (pt_w - md_.camera_pos_).norm();

      if (length > mp_.max_ray_length_)
      {
        pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;
        vox_idx = setCacheOccupancy(pt_w, 0);
      }
      else
      {
        vox_idx = setCacheOccupancy(pt_w, 1);
      }
    }

    max_x = max(max_x, pt_w(0));
    max_y = max(max_y, pt_w(1));
    max_z = max(max_z, pt_w(2));

    min_x = min(min_x, pt_w(0));
    min_y = min(min_y, pt_w(1));
    min_z = min(min_z, pt_w(2));

    // raycasting between camera center and point

    if (vox_idx != INVALID_IDX)
    {
      if (md_.flag_rayend_[vox_idx] == md_.raycast_num_)
      {
        continue;
      }
      else
      {
        md_.flag_rayend_[vox_idx] = md_.raycast_num_;
      }
    }

    raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_);

    while (raycaster.step(ray_pt))
    {
      Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_;
      length = (tmp - md_.camera_pos_).norm();

      // if (length < mp_.min_ray_length_) break;

      vox_idx = setCacheOccupancy(tmp, 0);

      if (vox_idx != INVALID_IDX)
      {
        if (md_.flag_traverse_[vox_idx] == md_.raycast_num_)
        {
          break;
        }
        else
        {
          md_.flag_traverse_[vox_idx] = md_.raycast_num_;
        }
      }
    }
  }

  min_x = min(min_x, md_.camera_pos_(0));
  min_y = min(min_y, md_.camera_pos_(1));
  min_z = min(min_z, md_.camera_pos_(2));

  max_x = max(max_x, md_.camera_pos_(0));
  max_y = max(max_y, md_.camera_pos_(1));
  max_z = max(max_z, md_.camera_pos_(2));
  max_z = max(max_z, mp_.ground_height_);

  posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_);
  posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_);
  boundIndex(md_.local_bound_min_);
  boundIndex(md_.local_bound_max_);

  md_.local_updated_ = true;

  // update occupancy cached in queue
  Eigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_;
  Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_;

  Eigen::Vector3i min_id, max_id;
  posToIndex(local_range_min, min_id);
  posToIndex(local_range_max, max_id);
  boundIndex(min_id);
  boundIndex(max_id);

  // std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl;

  while (!md_.cache_voxel_.empty())
  {

    Eigen::Vector3i idx = md_.cache_voxel_.front();
    int idx_ctns = toAddress(idx);
    md_.cache_voxel_.pop();

    double log_odds_update =
        md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_;

    md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0;

    if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_)
    {
      continue;
    }
    else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_)
    {
      md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;
      continue;
    }

    bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) &&
                    idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2);
    if (!in_local)
    {
      md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;
    }

    md_.occupancy_buffer_[idx_ctns] =
        std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_),
                 mp_.clamp_max_log_);
  }
}

3.3 发布地图和膨胀后的地图


void GridMap::publishMap()
{

  if (map_pub_.getNumSubscribers() <= 0)
 // 如果没有订阅者 直接返回减少不要的计算
    return;

  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;
//地图局部边界信息
  Eigen::Vector3i min_cut = md_.local_bound_min_;
  Eigen::Vector3i max_cut = md_.local_bound_max_;
// 计算局部地图边界的缩小范围
  int lmm = mp_.local_map_margin_ / 2;
  min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
  max_cut += Eigen::Vector3i(lmm, lmm, lmm);
  
//限制边界
  boundIndex(min_cut);
  boundIndex(max_cut);
//遍历地图 将适合的点添加到点云中 
  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y)
      for (int z = min_cut(2); z <= max_cut(2); ++z)
      {
        if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_)
          continue; //占据的概率小于阈值 跳过
          
		//计算点的位置
        Eigen::Vector3d pos;
        indexToPos(Eigen::Vector3i(x, y, z), pos);
        if (pos(2) > mp_.visualization_truncate_height_)
          continue;

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        //符合的点添加到点云中
        cloud.push_back(pt);
      }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = mp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;
	//转换点云为ros消息格式
  pcl::toROSMsg(cloud, cloud_msg);
  map_pub_.publish(cloud_msg);
}

void GridMap::publishMapInflate(bool all_info)
{

  if (map_inf_pub_.getNumSubscribers() <= 0)
    return;

  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;

  Eigen::Vector3i min_cut = md_.local_bound_min_;
  Eigen::Vector3i max_cut = md_.local_bound_max_;

  if (all_info)
  {
    int lmm = mp_.local_map_margin_;
    min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
    max_cut += Eigen::Vector3i(lmm, lmm, lmm);
  }

  boundIndex(min_cut);
  boundIndex(max_cut);

  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y)
      for (int z = min_cut(2); z <= max_cut(2); ++z)
      {
        if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0)
          continue;

        Eigen::Vector3d pos;
        indexToPos(Eigen::Vector3i(x, y, z), pos);
        if (pos(2) > mp_.visualization_truncate_height_)
          continue;

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        cloud.push_back(pt);
      }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = mp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud, cloud_msg);
  map_inf_pub_.publish(cloud_msg);

  // ROS_INFO("pub map");
}

3.4 AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)

void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)
{
    POOL_SIZE_ = pool_size;
    CENTER_IDX_ = pool_size / 2;

    GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)];
    for (int i = 0; i < POOL_SIZE_(0); i++)
    {
        GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)];
        for (int j = 0; j < POOL_SIZE_(1); j++)
        {
            GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)];
            for (int k = 0; k < POOL_SIZE_(2); k++)
            {
                GridNodeMap_[i][j][k] = new GridNode;
            }
        }
    }
    grid_map_ = occ_map;
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/176724.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

PLC 交互逻辑

PLC 交互逻辑

阅读更多...
智能化学习打破资源障碍 成为英语学习新趋势

智能化学习打破资源障碍 成为英语学习新趋势

智能化学习是一种基于互联网和人工智能技术的学习行为,通过网络,学习者可以随时随地进行学习,真正打破了时间和空间的限制。与传统线下学习方式相比,智能化学习更加方便、资源更加丰富,使海量英语学习资源唾手可得,智能化学习正逐渐成为中国孩子习得英语的重要方式。 随着全球…
阅读更多...
通过AX6000路由器，实现外部访问内网的任意主机

通过AX6000路由器，实现外部访问内网的任意主机

概述 这里遇到一个场景，就是需要外部的人员，访问我内网的一台设备，进行内外部的设备联调。 这也是实际环境中，很常见的一种场景。 之前的做法是子设备上运行edge节点，可以直接访问。 但有的设备无法运行edge节点，那么可以参考一下这个方案来实现。 此方案可以摒弃了…
阅读更多...
分享-Spss下载含spss25.spss26.spss27等版本

分享-Spss下载含spss25.spss26.spss27等版本

为了学习spss买的&#xff0c;分享安装程序给大家 SPSS 27是一款用于统计分析和数据挖掘的软件&#xff0c;以下是SPSS 27的功能介绍和配置建议&#xff1a; 功能介绍&#xff1a; 数据管理&#xff1a;SPSS 27可以对数据进行管理和清洗&#xff0c;包括数据输入、缺失值处理…
阅读更多...
什么是开关电源测试系统？如何用它进行测试？

什么是开关电源测试系统？如何用它进行测试？

开关电源测试系统是针对开关电源测试而开发的一种智能自动化测试系统&#xff0c;打破传统测试程序与缺陷&#xff0c;满足客户新的测试需求&#xff0c;助力客户解决测试难点&#xff0c;顺利完成开关电源测试&#xff0c;提高测试效能。那么开关电源自动化测试方案的流程是什…
阅读更多...
【漏洞复现】DPTech VPN存在任意文件读取漏洞

【漏洞复现】DPTech VPN存在任意文件读取漏洞

漏洞描述 DPtech是在网络、安全及应用交付领域集研发、生产、销售于一体的高科技企业。DPtech VPN智能安全网关是迪普科技面向广域互联应用场景推出的专业安全网关产品&#xff0c;集成了IPSec、SSL、L2TP、GRE等多种VPN技术&#xff0c;支持国密算法&#xff0c;实现分支机构…
阅读更多...
监控摄像头连接NAS，实现监控管理一体化

监控摄像头连接NAS，实现监控管理一体化

嗯&#xff1f;你问干嘛要把摄像头连到NAS&#xff1f; 小马给家里安了个监控摄像头 本意是想家里有啥事也能查监控 却没想到这些监控不仅存储回放有限制 要想更多功能还是得多花钱 恰好&#xff0c;我有铁威马NAS 打开Surveillance Manager 轻松搭建网络摄像头管理系统 …
阅读更多...
一键去水印免费网站快速无痕处理图片、视频水印

一键去水印免费网站快速无痕处理图片、视频水印

水印问题往往是一个大麻烦。即使我们只想将这些照片保留在我们的个人相册中以供怀旧&#xff0c;水印也可能像顽固的符号一样刺激我们的眼睛。为了解决这个问题&#xff0c;我们需要不断探索创新的解决方案&#xff0c;让我们深入研究一款强大的一键去水印免费网站“水印云”。…
阅读更多...
ubuntu下docker环境使用GPU配置

ubuntu下docker环境使用GPU配置

本文主要讲述整个命令流程&#xff0c;具体讲解请看官网nvidia-容器工具包和一篇总结得很详细的博文docker使用GPU总结 docker的版本必须安装19.0版本以上的&#xff0c;这里也只讲19.0版本以上的使用方法 首先设置一下网络信息 curl -fsSL https://nvidia.github.io/libnvi…
阅读更多...
Less精简直接上手,纯干货教程

Less精简直接上手,纯干货教程

目录 介绍 安装插件 入门使用测试 ​编辑 less变量 介绍 less作为一门CSS扩展语言&#xff0c;也就是说CSS预处理器。&#xff08;Leaner Style Sheets&#xff09;简称less&#xff0c;它只不过是为css新增这些的功能&#xff0c;比如说&#xff1a;变量、函数、作用域等等…
阅读更多...
【高级网络程序设计】Week3-2 Servlet

【高级网络程序设计】Week3-2 Servlet

一、 What are servlets? 1. 定义 &#xff08;1&#xff09;Servlets are Java’s answer to CGI&#xff1a; programs that run on a web server acting as middle layer between HTTP request and databases or other applications.Used for client requests that cann…
阅读更多...
Tekton — 通过tekton-operator部署tekton组件

Tekton — 通过tekton-operator部署tekton组件

文章目录 版本信息部署准备安装卸载tekton组件 Tektoncd Operator 作为一个 Kubernetes 的扩展&#xff0c;可以方便快捷地在 Kubernetes 集群上安装、升级和管理 Tekton Pipelines、Dashboard、Triggers 等组件。 那么本篇文章介绍在K8S集群中如何通过tekton-operator部署Tekt…
阅读更多...
如何使用ArcGIS Pro进行坐标转换

如何使用ArcGIS Pro进行坐标转换

不同来源的数据坐标系可能是不同的&#xff0c;为了统一使用这些数据就需要进行坐标转换&#xff0c;ArcGIS Pro作为专业的GIS软件&#xff0c;坐标转换功能肯定也是包含的&#xff0c;这里为大家介绍一下ArcGIS Pro如何进行坐标转换&#xff0c;希望能对你有所帮助。 数据来源…
阅读更多...
OFI libfabric原理及应用解析

OFI libfabric原理及应用解析

Agenda 目录/议题 编译通信软件硬件和软件带来的挑战为什么需要libfabriclibfabric架构API分组socket应用 VS libfabric应用区别GPU数据传输示例 编译通信软件 可靠面向连接的TCP和无连接的数据报UDP协议高性能计算HPC或人工智能AI 软硬件复杂性带来的挑战 上千个节点的集群, …
阅读更多...
【算法-哈希表4】 三数之和（去重版）

【算法-哈希表4】 三数之和（去重版）

今天&#xff0c;带来哈希相关算法的讲解。文中不足错漏之处望请斧正&#xff01; 理论基础点这里 三数之和 分析题意 这就是三数之和去重版嘛。 题意转化 求三元组, 满足每个元素相加为0&#xff0c;其中每个元素下标不同&#xff1b;而得到的三元组不能重复。 构成三元…
阅读更多...
【20年扬大真题】删除字符串s中的所有空格

【20年扬大真题】删除字符串s中的所有空格

【20年扬大真题】 删除字符串s中的所有空格 代码思路&#xff1a; 可以定义一个辅助的字符数组tmp&#xff0c;一边遍历字符串s&#xff0c;一边用tmp暂存s中的非空格元素。 遍历完s之后&#xff0c;再把tmp中的元素赋给字符串s即可 #include<stdio.h> #define MaxSize…
阅读更多...
栈和队列【详解】

栈和队列【详解】

目录 一、栈 1.栈的定义 2.栈的初始化 3.入栈 4.出栈 5.获取栈顶元素 6.获取栈元素的个数 7.判断栈是否为空 8.销毁栈 二、队列 1.队列的定义 2.入队 3.出队 4.获取队头元素 5.获取队尾元素 6.判断队列是否为空 7.获取队列的元素个数 8.销毁队列 前言&#xf…
阅读更多...
el-input限制输入整数等分析

el-input限制输入整数等分析

文章目录 前言1、在 Vue 中&#xff0c;可以使用以下几种方式来限制 el-input 只能输入整数1.1 设置input 的 type为number1.2 使用inputmode1.3 使用自定义指令1.4 使用计算属性1.5 使用 onafterpaste ,onkeyup1.6 el-input-number 的precision属性 总结 前言 input 限制输入…
阅读更多...
python命令行交互 引导用户输入一个数字

python命令行交互 引导用户输入一个数字

代码 以下代码将在命令行中&#xff0c;引导用户选择一个数字&#xff0c;并反馈用户输入的值 # -*- coding:UTF-8 -*- """ author: dyy contact: douyaoyuan126.com time: 2023/11/22 15:51 file: 引导用户输入一个数字.py desc: xxxxxx """#…
阅读更多...
D. Absolute Beauty - 思维

D. Absolute Beauty - 思维

题面 分析 补题。配上题解的图&#xff0c;理解了很长时间&#xff0c;思维还需要提高。 对于每一对 a i a_i ai​和 b i b_i bi​&#xff0c;可以看作一个线段的左右端点&#xff0c;这是关键的一步&#xff0c;那么他们的绝对值就是线段的长度&#xff0c;对于线段相对位…
阅读更多...
最新文章

Copyright @ 2022~2023