关于ego-planner里面的GridMap

news2025/1/19 3:43:21

浙大这套开源的代码写得很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;
}

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

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

cocos2dx ​​Animate3D (一）

cocos2dx ​​Animate3D (一）

3D相关的动画都是继承Grid3DAction 本质上是用GirdBase进行创建动画的小块。 Shaky3D 晃动特效 // 持续时间(时间过后不会回到原来的样子) // 整个屏幕被分成几行几列 // 晃动的范围 // z轴是否晃动 static Shaky3D* create(float initWithDuration, const Size& …
阅读更多...
Centos7安装Cesi（Supervisor集中管理工具）

Centos7安装Cesi（Supervisor集中管理工具）

Background CeSi 是 Supervisor 官方推荐的集中化管理 Supervisor 实例的 Web UI&#xff0c;该工具是用 Python 编写&#xff0c;基于 Flask Web 框架 。Superviosr 自带的 Web UI 不支持跨机器管理Supervisor 进程&#xff0c;功能比较简单&#xff0c;通过 CeSi 可以集中管理…
阅读更多...
对线程的创建

对线程的创建

一&#xff0c;概括 二&#xff0c;线程构建方式一&#xff08;继承Thread类&#xff09; 三&#xff0c;案例 父类&#xff1a; package Duoxiancheng;public abstract class Name {public static void main(String[] args) {//3&#xff0c;创建一个Thread线程类对象Thr…
阅读更多...
python-opencv划痕检测-续

python-opencv划痕检测-续

python-opencv划痕检测-续 这次划痕检测&#xff0c;是上一次划痕检测的续集。 处理的图像如下&#xff1a; 这次划痕检测&#xff0c;我们经过如下几步: 第一步&#xff1a;读取灰度图像 第二步&#xff1a;进行均值滤波 第三步&#xff1a;进行图像差分 第四步&#xff1…
阅读更多...
你了解Postman 变量吗？

你了解Postman 变量吗？

变量是在Postman工具中使用的一种特殊功能&#xff0c;用于存储和管理动态数据。它们可以用于在请求的不同部分、环境或集合之间共享和重复使用值。 Postman变量有以下几种类型&#xff1a; 1、环境变量&#xff08;Environment Variables&#xff09;: 环境变量是在Postman…
阅读更多...
“我，24岁，年薪20万”：选对了行业究竟多重要？

“我，24岁，年薪20万”：选对了行业究竟多重要？

那些在职场上顺风顺水&#xff0c;按部就班拿到高薪的人都有什么特点&#xff1f; 今天的主人公Flee告诉我&#xff0c;是稳。 在她的故事里&#xff0c;我看到一个“别人家的姑娘”&#xff0c;是怎样在职场上稳步晋升&#xff0c;大学毕业仅2年&#xff0c;就拿到18.6K月薪&a…
阅读更多...
数字化建筑工地源码，施工全过程实时监控、数据分析、智能管理和优化调控

数字化建筑工地源码，施工全过程实时监控、数据分析、智能管理和优化调控

智慧工地是指通过信息化技术、物联网、人工智能技术等手段&#xff0c;对建筑工地进行数字化、智能化、网络化升级&#xff0c;实现对施工全过程的实时监控、数据分析、智能管理和优化调控。智慧工地的建设可以提高工地的安全性、效率性和质量&#xff0c;降低施工成本&#xf…
阅读更多...
苍穹外卖项目笔记（4）——菜品管理

苍穹外卖项目笔记（4）——菜品管理

菜品管理 主要功能模块&#xff1a;新建菜品、修改菜品、启用禁用菜品、菜品的分页查询、删除菜品 代码&#xff1a;GitHub - Echo0701/take-out 1 公共字段自动填充 公共字段指的是业务表中有一些相同的字段&#xff0c;比如创建人、创建时间、修改人、修改时间等&#xff…
阅读更多...
易点易动设备管理系统：提升企业设备巡检效率的最佳选择

易点易动设备管理系统：提升企业设备巡检效率的最佳选择

在现代企业运营中&#xff0c;设备管理扮演着至关重要的角色。设备巡检旨在确保设备的正常运行和及时维护&#xff0c;以确保生产线的顺畅运行和业务的高效执行。然而&#xff0c;传统的设备巡检方法常常效率低下、耗时费力。针对这一问题&#xff0c;易点易动设备管理系统应运…
阅读更多...
哈希表之开散列的实现

哈希表之开散列的实现

回顾与引出 我们在上一节用闭散列的开放定址法实现了哈希表。不难看出这种方法有明显的缺点&#xff1a;一旦发生哈希冲突&#xff0c;所有的冲突连在一起&#xff0c;容易产生数据“堆积”&#xff0c;即&#xff1a;不同 关键码占据了可利用的空位置&#xff0c;使得寻找某关…
阅读更多...
【Mysql】[Err] 1293 - Incorrect table definition；

【Mysql】[Err] 1293 - Incorrect table definition；

基本情况 SQL文件描述 /* Navicat MySQL Data TransferSource Server : cm4生产-200 Source Server Version : 50725 Source Host : 192.168.1.200:3306 Source Database : db_wmsTarget Server Type : MYSQL Target Server Version : 50725 File…
阅读更多...
4G5G智能执法记录仪在保险公司车辆保险远程定损中的应用

4G5G智能执法记录仪在保险公司车辆保险远程定损中的应用

4G智能执法记录仪&#xff1a;汽车保险定损的**利器 随着科技的不断进步&#xff0c;越来越多的智能设备应用到日常生活中。而在车辆保险定损领域&#xff0c;4G智能执法记录仪的出现无疑是一大**。它不仅可以实现远程定损&#xff0c;还能实现可视化操作、打印保单以及数据融…
阅读更多...
10年经验之谈 —— 如何做接口测试呢？接口测试有哪些工具？

10年经验之谈 —— 如何做接口测试呢？接口测试有哪些工具？

回想入职测试已经10年时间了&#xff0c;初入职场的我对于接口测试茫然不知。后来因为业务需要&#xff0c;开始慢慢接触接口测试。从最开始使用工具进行接口测试到编写代码实现接口自动化&#xff0c;到最后的测试平台开发。回想这一路走来感触颇深&#xff0c;因此为了避免打…
阅读更多...
几个西门子PLC常见通讯问题的解决方法

几个西门子PLC常见通讯问题的解决方法

1台200SMART 如何控制2台步进电机&#xff1f; S7-200SMART CPU最多可输出3路高速脉冲&#xff08;除ST20外&#xff09;&#xff0c;这意味着可同时控制最多3个步进电机&#xff0c;通过运动向导可配置相应的运动控制子程序&#xff0c;然后通过调用子程序编程可实现对步进电…
阅读更多...
error: ‘ui/ui_uimainwindow.h‘ file not found

error: ‘ui/ui_uimainwindow.h‘ file not found

问题&#xff1a;在刚好创建的Qt Designer Form Class类中&#xff0c;发现类的.cpp文件中有ui头文件未找到 原因&#xff1a;.ui文件没有被识别到&#xff0c;或者.ui文件不存在&#xff0c;导致ui头文件未创建而报错。 解决&#xff1a;若修改了.ui文件&#xff0c;随手ctrls…
阅读更多...
ILI9225 TFT显示屏16位并口方式驱动

ILI9225 TFT显示屏16位并口方式驱动

所用屏及资料如后图&#xff1a; ILI9225&#xff0c;176*220&#xff0c;8位或16位并口屏&#xff0c;IM0接GND&#xff0c;电源及背光接3.3v 主控&#xff1a;CH32V307驱动&#xff08;库文件和STM32基本一样&#xff09; 一、源码 ILI9225.c #include "ILI9225.h&quo…
阅读更多...
innoDB的缓冲池（Buffer Pool）的工作原理

innoDB的缓冲池（Buffer Pool）的工作原理

数据存在磁盘了&#xff0c;总不能次次和磁盘交互吧&#xff0c;所以innoDB有一个缓冲池&#xff08;Buffer Pool&#xff09;&#xff0c;有了缓冲池后&#xff0c;读写就优先在缓冲池了。读先在缓冲池读&#xff0c;没有再去磁盘加载进缓冲池&#xff1b;写也是先写缓冲池&am…
阅读更多...
学习MySQL先有全局观，细说其发展历程及特点

学习MySQL先有全局观，细说其发展历程及特点

学习MySQL先有全局观&#xff0c;细说其发展历程及特点 一、枝繁叶茂的MySQL家族1. 发展历程2. 分支版本 二、特点分析1. 常用数据库2. 选型角度及场景 三、三大组成部分四、总结 相信很多同学在接触编程之初&#xff0c;就接触过数据库&#xff0c;而对于其中关系型数据库中的…
阅读更多...
云备份——初步认识及环境搭建

云备份——初步认识及环境搭建

文章目录 整体功能简介云备份功能实现目标服务器程序负责功能细分服务端模块划分客户端功能细分客户端模块划分 环境搭建gcc安装 jsoncppbundle库 与 httplib库安装 整体功能简介 云备份功能 自动将本地计算机上指定文件夹中需要备份的文件上传备份到服务器中 并且能够通过浏…
阅读更多...
__int128类型movaps指令crash

__int128类型movaps指令crash

结论 在使用__int128时&#xff0c;如果__int128类型的内存起始地址不是按16字节对齐的话&#xff0c;有些汇编指令会抛出SIGSEGV使程序crash。 malloc在64位系统中申请的内存地址&#xff0c;是按16字节对齐的&#xff0c;但一般使用时经常会申请一块内存自己切割使用&#…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023