39 #include <octomap_msgs/Octomap.h>
42 #include <OgreSceneNode.h>
43 #include <OgreSceneManager.h>
49 typedef std::vector<rviz::PointCloud::Point>
VPoint;
50 typedef std::vector<VPoint>
VVPoint;
54 std::size_t max_octree_depth, Ogre::SceneNode* parent_node)
55 : octree_(octree), colorFactor_(0.8)
57 if (!max_octree_depth)
59 octree_depth_ = octree->getTreeDepth();
63 octree_depth_ = std::min(max_octree_depth, (std::size_t)octree->getTreeDepth());
66 scene_node_ = parent_node->createChildSceneNode();
68 cloud_.resize(octree_depth_);
70 for (std::size_t i = 0; i < octree_depth_; ++i)
72 std::stringstream sname;
73 sname <<
"PointCloud Nr." << i;
75 cloud_[i]->setName(sname.str());
77 scene_node_->attachObject(cloud_[i]);
80 octreeDecoding(octree, octree_voxel_rendering, octree_color_mode);
105 scene_node_->setOrientation(orientation);
118 double h = (1.0 - std::min(std::max((z_pos - min_z) / (max_z - min_z), 0.0), 1.0)) * color_factor;
136 point->setColor(n, v, m);
139 point->setColor(m, v, n);
142 point->setColor(m, n, v);
145 point->setColor(n, m, v);
148 point->setColor(v, m, n);
151 point->setColor(1, 0.5, 0.5);
163 double min_x, min_y, min_z, max_x, max_y, max_z;
164 octree->getMetricMin(min_x, min_y, min_z);
165 octree->getMetricMax(max_x, max_y, max_z);
167 unsigned int render_mode_mask =
static_cast<unsigned int>(octree_voxel_rendering);
170 int step_size = 1 << (octree->getTreeDepth() -
octree_depth_);
175 bool display_voxel =
false;
178 if (((
int)octree->isNodeOccupied(*it) + 1) & render_mode_mask)
181 bool all_neighbors_found =
true;
188 int diff_base = 1 << (octree->getTreeDepth() - it.getDepth());
189 int diff[2] = { -1, diff_base };
192 for (
unsigned int idx_case = 0; idx_case < 3; ++idx_case)
194 int idx_0 = idx_case % 3;
195 int idx_1 = (idx_case + 1) % 3;
196 int idx_2 = (idx_case + 2) % 3;
198 for (
int i = 0; all_neighbors_found && i < 2; ++i)
200 key[idx_0] = n_key[idx_0] + diff[i];
203 for (key[idx_1] = n_key[idx_1] + diff[0] + 1; all_neighbors_found && key[idx_1] < n_key[idx_1] + diff[1];
204 key[idx_1] += step_size)
206 for (key[idx_2] = n_key[idx_2] + diff[0] + 1; all_neighbors_found && key[idx_2] < n_key[idx_2] + diff[1];
207 key[idx_2] += step_size)
212 if (!(node && ((((
int)octree->isNodeOccupied(node)) + 1) & render_mode_mask)))
215 all_neighbors_found =
false;
222 display_voxel |= !all_neighbors_found;
233 float cell_probability;
235 switch (octree_color_mode)
241 cell_probability = it->getOccupancy();
242 new_point.
setColor((1.0f - cell_probability), cell_probability, 0.0);
249 unsigned int depth = it.getDepth();
250 point_buf[depth - 1].push_back(new_point);
257 double size = octree->getNodeSize(i + 1);
260 cloud_[i]->setDimensions(size, size, size);
262 cloud_[i]->addPoints(&point_buf[i].front(), point_buf[i].size());
263 point_buf[i].clear();