本文整理汇总了C++中OcTree::updateInnerOccupancy方法的典型用法代码示例。如果您正苦于以下问题：C++ OcTree::updateInnerOccupancy方法的具体用法？C++ OcTree::updateInnerOccupancy怎么用？C++ OcTree::updateInnerOccupancy使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类OcTree的用法示例。
在下文中一共展示了OcTree::updateInnerOccupancy方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: execute
void execute(const fremen::informationGoalConstPtr& goal, Server* as)
{
/* Octmap Estimation and Visualization */
octomap_msgs::Octomap bmap_msg;
OcTree octree (resolution);
geometry_msgs::Point initialPt, finalPt;
//Create pointcloud:
octomap::Pointcloud octoCloud;
sensor_msgs::PointCloud fremenCloud;
float x = 0*gridPtr->positionX;
float y = 0*gridPtr->positionY;
geometry_msgs::Point32 test_point;
int cnt = 0;
int cell_x, cell_y, cell_z;
cell_x = (int)(goal->x/resolution);
cell_y = (int)(goal->y/resolution);
cell_z = (int)(head_height/resolution);
for(double i = LIM_MIN_X; i < LIM_MAX_X; i+=resolution){
for(double j = LIM_MIN_Y; j < LIM_MAX_Y; j+=resolution){
for(double w = LIM_MIN_Z; w < LIM_MAX_Z; w+=resolution){
point3d ptt(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
int s = goal->stamp;
if(gridPtr->retrieve(cnt, goal->stamp)>0)
{
//finalPt.z = (int)((w+resolution/2)/resolution)-cell_z;
//finalPt.y = (int)((j+resolution/2)/resolution)-cell_y;
//finalPt.x = (int)((i+resolution/2)/resolution)-cell_x;
//int cnta = ((cell_x+finalPt.x-LIM_MIN_X/resolution)*dim_y + (finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (finalPt.z + cell_z-LIM_MIN_Z/resolution);
//ROS_INFO("something %d %d",cnt,cnta);
octoCloud.push_back(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
octree.updateNode(ptt,true,true);
}
cnt++;
}
}
}
//Update grid
octree.updateInnerOccupancy();
//init visualization markers:
visualization_msgs::MarkerArray occupiedNodesVis;
unsigned int m_treeDepth = octree.getTreeDepth();
//each array stores all cubes of a different size, one for each depth level:
occupiedNodesVis.markers.resize(m_treeDepth + 1);
geometry_msgs::Point cubeCenter;
std_msgs::ColorRGBA m_color;
m_color.r = 0.0;
m_color.g = 0.0;
m_color.b = 1.0;
m_color.a = 0.5;
for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i)
{
double size = octree.getNodeSize(i);
occupiedNodesVis.markers[i].header.frame_id = "/map";
occupiedNodesVis.markers[i].header.stamp = ros::Time::now();
occupiedNodesVis.markers[i].ns = "map";
occupiedNodesVis.markers[i].id = i;
occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
occupiedNodesVis.markers[i].scale.x = size;
occupiedNodesVis.markers[i].scale.y = size;
occupiedNodesVis.markers[i].scale.z = size;
occupiedNodesVis.markers[i].color = m_color;
}
ROS_INFO("s %i",cnt++);
x = gridPtr->positionX;
y = gridPtr->positionY;
for(OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it)
{
if(it != NULL && octree.isNodeOccupied(*it))
{
unsigned idx = it.getDepth();
cubeCenter.x = x+it.getX();
cubeCenter.y = y+it.getY();
cubeCenter.z = it.getZ();
occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
double minX, minY, minZ, maxX, maxY, maxZ;
octree.getMetricMin(minX, minY, minZ);
octree.getMetricMax(maxX, maxY, maxZ);
double h = (1.0 - fmin(fmax((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor;
occupiedNodesVis.markers[idx].colors.push_back(heightMapColorA(h));
}
}
/**** Robot Head Marker ****/
//Robot Position
//.........这里部分代码省略.........