三维栅格地图映射到一维vector数组的下标号如何确定问题

三维栅格地图映射到一维vector数组的下标号如何确定问题

在这里插入图片描述
公式(3)如何理解如图:
先zy面开始装,然后一列一列装,最后一列是 z i n d e x z_{index} zindex
在这里插入图片描述

实现代码

// set obstalces into grid map for path planning
void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{
	if (coord_x < gl_xl || coord_y < gl_yl || coord_z < gl_zl ||
		coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu)
		return;

	int idx_x = static_cast<int>((coord_x - gl_xl) * inv_resolution);//把double型的强制转化为int型
	int idx_y = static_cast<int>((coord_y - gl_yl) * inv_resolution);
	int idx_z = static_cast<int>((coord_z - gl_zl) * inv_resolution);

	data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;//为什么公式是这样的???????????三维映射到一维
}
  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值