EGO-planner代码笔记1 grid_map.cpp源码解析
本帖持续更新,先把源码注释贴出来,在grid_map命名空间下的参数作用都能找得到
#include "plan_env/grid_map.h"
// #define current_img_ md_.depth_image_[image_cnt_ & 1]
// #define last_img_ md_.depth_image_[!(image_cnt_ & 1)]
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);
//地图大小(真实世界大小,不是栅格数量,单位m)
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);
//mm转化为m
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_, 999.0);
//在一定高度上添加虚拟地面
node_.param("grid_map/virtual_ceil_height", mp_.virtual_ceil_height_, -0.1);
//源码里被注释了,没用
node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false);
//接受的位姿1为geometry_msgs::Pose类型,2为nav_msgs::Odom类型
node_.param("grid_map/pose_type", mp_.pose_type_, 1);
//发布地图的frame_id
node_.param("grid_map/frame_id", mp_.frame_id_, string("world"));
//局部地图的边界,更新时以local_bound_min和local_bound_max向外扩展margin个栅格作为更新范围
node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1);
//地图坐标系下的地面高度
node_.param("grid_map/ground_height", mp_.ground_height_, 1.0);
//地图原点和地图大小(m)
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);
//#define logit(x) (log((x) / (1 - (x)))),贝叶斯滤波相关,后续介绍
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_);
//地图的最大和最小边界(m)
mp_.map_min_boundary_ = mp_.map_origin_;
mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;
// initialize data buffers
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);
//命中和通过某栅格的计数器count_hit_and_miss_ - count_hit_ 即为miss数
md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);
md_.count_hit_ = vector<short>(buffer_size, 0);
//标记某栅格是第几帧的终点,如果是同一帧,raycast的效果是一样的,不会进行同样操作
md_.flag_rayend_ = vector<char>(buffer_size, -1);
//标记某栅格是第几帧的透过点
md_.flag_traverse_ = vector<char>(buffer_size, -1);
// raycast轮次和上面flag_rayend_一起使用
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.02,
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));
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));
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));
}
//使用深度图的话会自动禁止下面的
// 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.05), &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);
//发布未知点云,源码中实际并未使用
unknown_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/unknown", 10);
//有新数据,举要更新
md_.occ_need_update_ = false;
//raycast后需要重新计算概率
md_.local_updated_ = false;
//是否有第一帧深度图,如果有则odomCallback不生效
md_.has_first_depth_ = false;
//是否有odom,没有会把点云回调直接return
md_.has_odom_ = false;
//是否有点云,目前没用
md_.has_cloud_ = false;
//注释了,没用
md_.image_cnt_ = 0;
//分析时间的没用
md_.fuse_time_ = 0.0;
//这个参数没用到
md_.update_num_ = 0;
md_.max_fuse_time_ = 0.0;
// 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());
}
//重置地图occupancy_buffer_inflate_
void GridMap::resetBuffer()
{
Eigen::Vector3d min_pos = mp_.map_min_boundary_;
Eigen::Vector3d max_pos = mp_.map_max_boundary_;
resetBuffer(min_pos, max_pos);
md_.local_bound_min_ = Eigen::Vector3i::Zero();
md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i::Ones();
}
void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos)
{
Eigen::Vector3i min_id, max_id;
posToIndex(min_pos, min_id);
posToIndex(max_pos, max_id);
boundIndex(min_id);
boundIndex(max_id);
/* reset occ and dist buffer */
for (int x = min_id(0); x <= max_id(0); ++x)
for (int y = min_id(1); y <= max_id(1); ++y)
for (int z = min_id(2); z <= max_id(2); ++z)
{
md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
}
}
//记录某个位置的击中和通过,并缓存,返回该位置的储存地址
int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ)
{
if (occ != 1 && occ != 0)
return INVALID_IDX;
Eigen::Vector3i id;
posToIndex(pos, id);
int idx_ctns = toAddress(id);
md_.count_hit_and_miss_[idx_ctns] += 1;
//初次被通过或被击中时加入缓存队列
if (md_.count_hit_and_miss_[idx_ctns] == 1)
{
md_.cache_voxel_.push(id);
}
//击中次数+1
if (occ == 1)
md_.count_hit_[idx_ctns] += 1;
return idx_ctns;
}
//处理深度图
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;
double depth;
//相机位姿的四元数转旋转矩阵
Eigen::Matrix3d camera_r = md_.camera_q_.toRotationMatrix();
// cout << "rotate: " << md_.camera_q_.toRotationMatrix() << endl;
// std::cout << "pos in proj: " << md_.camera_pos_ << std::endl;
//是否使用深度滤波
if (!mp_.use_depth_filter_)
{
for (int v = 0; v < rows; v++)
{
row_ptr = md_.depth_image_.ptr<uint16_t>(v);
for (int u = 0; u < cols; u++)
{
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_q_.inverse();
const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_;
//去掉边缘,以skip_pixel_降采样
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_q_ = md_.camera_q_;
md_.last_depth_image_ = md_.depth_image_;
}
//光线投射,沿着光路更新路径上的所有栅格概率
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_;
}
//末端标记为miss
vox_idx = setCacheOccupancy(pt_w, 0);
}
else
{
//超过最长光线追踪长度会在末端标记为miss,没超过会在末端标记为hit
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
//标记某栅格是第几帧的终点,如果是同一帧,raycast的效果是一样的,不会进行同样操作
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);
//这里break很巧妙,第一次看一以为应该是continue,但是我们注意到,racast的操作是从点云向相机反向进行的,
//并且相机的所有raycast都会汇聚到同一个点,当raycast进行到某处发现该帧已经被其他点raycast过了,
//则可以保证后续所有的点都是raycast过的
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_);
//所有raycast涉及到的边界
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_)
{
//命中大于log概率clamp_max_log_的不更新
continue;
}
else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_)
{
//命中小于log概率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_);
}
}
//找到一个地图外的点和相机连线和地图边界的交点
Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt)
{
Eigen::Vector3d diff = pt - camera_pt;
Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt;
Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt;
double min_t = 1000000;
for (int i = 0; i < 3; ++i)
{
if (fabs(diff[i]) > 0)
{
double t1 = max_tc[i] / diff[i];
if (t1 > 0 && t1 < min_t)
min_t = t1;
double t2 = min_tc[i] / diff[i];
if (t2 > 0 && t2 < min_t)
min_t = t2;
}
}
return camera_pt + (min_t - 1e-3) * diff;
}
void GridMap::clearAndInflateLocalMap()
{
/*clear outside local*/
const int vec_margin = 5;
// Eigen::Vector3i min_vec_margin = min_vec - Eigen::Vector3i(vec_margin,
// vec_margin, vec_margin); Eigen::Vector3i max_vec_margin = max_vec +
// Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
Eigen::Vector3i min_cut = md_.local_bound_min_ -
Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
Eigen::Vector3i max_cut = md_.local_bound_max_ +
Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
boundIndex(min_cut);
boundIndex(max_cut);
Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
boundIndex(min_cut_m);
boundIndex(max_cut_m);
// clear data outside the local range
//清除局部更新范围外的障碍,
for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
{
for (int z = min_cut_m(2); z < min_cut(2); ++z)
{
int idx = toAddress(x, y, z);
md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
}
for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z)
{
int idx = toAddress(x, y, z);
md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
}
}
for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
{
for (int y = min_cut_m(1); y < min_cut(1); ++y)
{
int idx = toAddress(x, y, z);
md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
}
for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y)
{
int idx = toAddress(x, y, z);
md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
}
}
for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
{
for (int x = min_cut_m(0); x < min_cut(0); ++x)
{
int idx = toAddress(x, y, z);
md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
}
for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x)
{
int idx = toAddress(x, y, z);
md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
}
}
// inflate occupied voxels to compensate robot size
int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
// int inf_step_z = 1;
vector<Eigen::Vector3i> inf_pts(pow(2 * inf_step + 1, 3));
// inf_pts.resize(4 * inf_step + 3);
Eigen::Vector3i inf_pt;
//清除局部更新范围内的障碍
// clear outdated data
for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
{
md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
}
//膨胀局部更新范围内的障碍
// inflate obstacles
for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
{
//如果被占据
if (md_.occupancy_buffer_[toAddress(x, y, z)] > mp_.min_occupancy_log_)
{
//先获取膨胀栅格,再遍历每个栅格赋值
inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts);
for (int k = 0; k < (int)inf_pts.size(); ++k)
{
inf_pt = inf_pts[k];
int idx_inf = toAddress(inf_pt);
if (idx_inf < 0 ||
idx_inf >= mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2))
{
continue;
}
md_.occupancy_buffer_inflate_[idx_inf] = 1;
}
}
}
//添加一个虚拟地面
// add virtual ceiling to limit flight height
if (mp_.virtual_ceil_height_ > -0.5)
{
int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_);
for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
{
md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
}
}
}
//定期发布可视化的地图
void GridMap::visCallback(const ros::TimerEvent & /*event*/)
{
publishMap();
publishMapInflate(true);
}
//定期更新地图
void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/)
{
if (!md_.occ_need_update_)
return;
/* update occupancy */
// ros::Time t1, t2, t3, t4;
// t1 = ros::Time::now();
projectDepthImage();
// t2 = ros::Time::now();
raycastProcess();
// t3 = ros::Time::now();
if (md_.local_updated_)
clearAndInflateLocalMap();
// t4 = ros::Time::now();
// cout << setprecision(7);
// cout << "t2=" << (t2-t1).toSec() << " t3=" << (t3-t2).toSec() << " t4=" << (t4-t3).toSec() << endl;;
// md_.fuse_time_ += (t2 - t1).toSec();
// md_.max_fuse_time_ = max(md_.max_fuse_time_, (t2 - t1).toSec());
// if (mp_.show_occ_time_)
// ROS_WARN("Fusion: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec(),
// md_.fuse_time_ / md_.update_num_, md_.max_fuse_time_);
md_.occ_need_update_ = false;
md_.local_updated_ = false;
}
//深度图和位姿的联合回调函数
void GridMap::depthPoseCallback(const sensor_msgs::ImageConstPtr &img,
const geometry_msgs::PoseStampedConstPtr &pose)
{
/* get depth image */
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(img, img->encoding);
if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
{
(cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_);
}
cv_ptr->image.copyTo(md_.depth_image_);
// std::cout << "depth: " << md_.depth_image_.cols << ", " << md_.depth_image_.rows << std::endl;
/* get pose */
md_.camera_pos_(0) = pose->pose.position.x;
md_.camera_pos_(1) = pose->pose.position.y;
md_.camera_pos_(2) = pose->pose.position.z;
md_.camera_q_ = Eigen::Quaterniond(pose->pose.orientation.w, pose->pose.orientation.x,
pose->pose.orientation.y, pose->pose.orientation.z);
//相机不在地图里了没法更新
if (isInMap(md_.camera_pos_))
{
md_.has_odom_ = true;
md_.update_num_ += 1;
md_.occ_need_update_ = true;
}
else
{
md_.occ_need_update_ = false;
}
}
//odom回调,使用深度图则直接返回
void GridMap::odomCallback(const nav_msgs::OdometryConstPtr &odom)
{
if (md_.has_first_depth_)
return;
md_.camera_pos_(0) = odom->pose.pose.position.x;
md_.camera_pos_(1) = odom->pose.pose.position.y;
md_.camera_pos_(2) = odom->pose.pose.position.z;
md_.has_odom_ = true;
}
//点云回调和odom一起使用
void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img)
{
pcl::PointCloud<pcl::PointXYZ> latest_cloud;
pcl::fromROSMsg(*img, latest_cloud);
md_.has_cloud_ = true;
if (!md_.has_odom_)
{
std::cout << "no odom!" << std::endl;
return;
}
if (latest_cloud.points.size() == 0)
return;
if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2)))
return;
//清除更新范围内的点云
this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_,
md_.camera_pos_ + mp_.local_update_range_);
pcl::PointXYZ pt;
Eigen::Vector3d p3d, p3d_inf;
int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
int inf_step_z = 1;
double max_x, max_y, max_z, min_x, min_y, min_z;
min_x = mp_.map_max_boundary_(0);
min_y = mp_.map_max_boundary_(1);
min_z = mp_.map_max_boundary_(2);
max_x = mp_.map_min_boundary_(0);
max_y = mp_.map_min_boundary_(1);
max_z = mp_.map_min_boundary_(2);
for (size_t i = 0; i < latest_cloud.points.size(); ++i)
{
pt = latest_cloud.points[i];
p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z;
/* point inside update range */
Eigen::Vector3d devi = p3d - md_.camera_pos_;
Eigen::Vector3i inf_pt;
//在局部更新范围内的点就直接膨胀然后更新上去,不算概率了
if (fabs(devi(0)) < mp_.local_update_range_(0) && fabs(devi(1)) < mp_.local_update_range_(1) &&
fabs(devi(2)) < mp_.local_update_range_(2))
{
/* inflate the point */
for (int x = -inf_step; x <= inf_step; ++x)
for (int y = -inf_step; y <= inf_step; ++y)
for (int z = -inf_step_z; z <= inf_step_z; ++z)
{
p3d_inf(0) = pt.x + x * mp_.resolution_;
p3d_inf(1) = pt.y + y * mp_.resolution_;
p3d_inf(2) = pt.z + z * mp_.resolution_;
max_x = max(max_x, p3d_inf(0));
max_y = max(max_y, p3d_inf(1));
max_z = max(max_z, p3d_inf(2));
min_x = min(min_x, p3d_inf(0));
min_y = min(min_y, p3d_inf(1));
min_z = min(min_z, p3d_inf(2));
posToIndex(p3d_inf, inf_pt);
if (!isInMap(inf_pt))
continue;
int idx_inf = toAddress(inf_pt);
md_.occupancy_buffer_inflate_[idx_inf] = 1;
}
}
}
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_);
}
//发布地图点云
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;
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");
}
void GridMap::publishUnknown()
{
pcl::PointXYZ pt;
pcl::PointCloud<pcl::PointXYZ> cloud;
Eigen::Vector3i min_cut = md_.local_bound_min_;
Eigen::Vector3i max_cut = md_.local_bound_max_;
boundIndex(max_cut);
boundIndex(min_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_.clamp_min_log_ - 1e-3)
{
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);
unknown_pub_.publish(cloud_msg);
}
bool GridMap::odomValid() { return md_.has_odom_; }
bool GridMap::hasDepthObservation() { return md_.has_first_depth_; }
Eigen::Vector3d GridMap::getOrigin() { return mp_.map_origin_; }
// int GridMap::getVoxelNum() {
// return mp_.map_voxel_num_[0] * mp_.map_voxel_num_[1] * mp_.map_voxel_num_[2];
// }
void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size)
{
ori = mp_.map_origin_, size = mp_.map_size_;
}
void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img,
const nav_msgs::OdometryConstPtr &odom)
{
/* get pose */
Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w,
odom->pose.pose.orientation.x,
odom->pose.pose.orientation.y,
odom->pose.pose.orientation.z);
Eigen::Matrix3d body_r_m = body_q.toRotationMatrix();
Eigen::Matrix4d body2world;
body2world.block<3, 3>(0, 0) = body_r_m;
body2world(0, 3) = odom->pose.pose.position.x;
body2world(1, 3) = odom->pose.pose.position.y;
body2world(2, 3) = odom->pose.pose.position.z;
body2world(3, 3) = 1.0;
Eigen::Matrix4d cam_T = body2world * md_.cam2body_;
md_.camera_pos_(0) = cam_T(0, 3);
md_.camera_pos_(1) = cam_T(1, 3);
md_.camera_pos_(2) = cam_T(2, 3);
md_.camera_q_ = Eigen::Quaterniond(cam_T.block<3, 3>(0, 0));
/* get depth image */
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(img, img->encoding);
if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
{
(cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_);
}
cv_ptr->image.copyTo(md_.depth_image_);
md_.occ_need_update_ = true;
}
// GridMap