1、参数解读
一、MappingData md_中的参数含义:
local_bound_min_, local_bound_max_
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;
二、MappingParameters mp_中的参数含义:
for (int i = 0; i < 3; ++i)
{
mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);
}
int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);
mp_.map_min_boundary_ = mp_.map_origin_;
mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;
mp_.local_map_margin_
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;
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
三、grid_map.cpp文件中其他参数含义:
x_size, y_size, z_size
mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);
mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);
2、主要函数解读
1. indep_cloud_sub_ =
node_.subscribe<sensor_msgs::PointCloud2>(“grid_map/cloud”, 10, &GridMap::cloudCallback, this);
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;
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))
{
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_);
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::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);
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;
}
}
inline void GridMap::boundIndex(Eigen::Vector3i& id) {
Eigen::Vector3i id1;
id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
id = id1;
}
inline int GridMap::toAddress(int& x, int& y, int& z) {
return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
}
2. void GridMap::raycastProcess()
void GridMap::raycastProcess()
{
if (md_.proj_points_cnt == 0)
return;
ros::Time t1, t2;
md_.raycast_num_ += 1;
int vox_idx;
double length;
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];
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));
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();
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;
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);
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_);
}
}
inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
return false;
}
if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
return false;
}
return true;
}
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;
}
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);
}
if (occ == 1)
md_.count_hit_[idx_ctns] += 1;
return idx_ctns;
}
3. void GridMap::clearAndInflateLocalMap()
void GridMap::clearAndInflateLocalMap()
{
const int vec_margin = 5;
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);
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_;
}
}
int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
vector<Eigen::Vector3i> inf_pts(pow(2 * inf_step + 1, 3));
Eigen::Vector3i inf_pt;
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;
}
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;
}
}
}
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;
}
}
}
inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
int num = 0;
for (int x = -step; x <= step; ++x)
for (int y = -step; y <= step; ++y)
for (int z = -step; z <= step; ++z) {
pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
}
}
1.图中红色阴影部分的体素被设为空闲,对应代码中的 //clear data outside the local range.
2.黑色框内的占据体素会被膨胀,左右各膨胀1*0.1=0.1m,而黑色框与蓝色框的边边距离为local_map_margin_*0.1=1m.
//local_bound_min_表示更新栅格的范围,local_map_margin_ = 10 表示局部栅格地图的更新和清除
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_);
4. vis_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::visCallback, this);
void GridMap::visCallback(const ros::TimerEvent & )
{
publishMapInflate(true);
publishMap();
}
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);
}
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);
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)