void ColorOcTree::writeColorHistogram(std::string filename) {
#ifdef _MSC_VER
fprintf(stderr, "The color histogram uses gnuplot, this is not supported under windows.\n");
#else
// build RGB histogram
std::vector<int> histogram_r (256,0);
std::vector<int> histogram_g (256,0);
std::vector<int> histogram_b (256,0);
for(ColorOcTree::tree_iterator it = this->begin_tree(),
end=this->end_tree(); it!= end; ++it) {
if (!it.isLeaf() || !this->isNodeOccupied(*it)) continue;
ColorOcTreeNode::Color& c = it->getColor();
++histogram_r[c.r];
++histogram_g[c.g];
++histogram_b[c.b];
}
// plot data
FILE *gui = popen("gnuplot ", "w");
fprintf(gui, "set term postscript eps enhanced color\n");
fprintf(gui, "set output \"%s\"\n", filename.c_str());
fprintf(gui, "plot [-1:256] ");
fprintf(gui,"'-' w filledcurve lt 1 lc 1 tit \"r\",");
fprintf(gui, "'-' w filledcurve lt 1 lc 2 tit \"g\",");
fprintf(gui, "'-' w filledcurve lt 1 lc 3 tit \"b\",");
fprintf(gui, "'-' w l lt 1 lc 1 tit \"\",");
fprintf(gui, "'-' w l lt 1 lc 2 tit \"\",");
fprintf(gui, "'-' w l lt 1 lc 3 tit \"\"\n");
for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_r[i]);
fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_g[i]);
fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_b[i]);
fprintf(gui,"0 0\n"); fprintf(gui, "e\n");
for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_r[i]);
fprintf(gui, "e\n");
for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_g[i]);
fprintf(gui, "e\n");
for (int i=0; i<256; ++i) fprintf(gui,"%d %d\n", i, histogram_b[i]);
fprintf(gui, "e\n");
fflush(gui);
#endif
}
void ColorOcTreeDrawer::setOcTree(const AbstractOcTree& tree_pnt,
const octomap::pose6d& origin_,
int map_id_) {
const ColorOcTree& tree = ((const ColorOcTree&) tree_pnt);
this->map_id = map_id_;
// save origin used during cube generation
this->initial_origin = octomap::pose6d(octomap::point3d(0,0,0), origin_.rot());
// origin is in global coords
this->origin = origin_;
// maximum size to prevent crashes on large maps: (should be checked in a better way than a constant)
bool showAll = (tree.size() < 5 * 1e6);
bool uses_origin = ( (origin_.rot().x() != 0.) && (origin_.rot().y() != 0.)
&& (origin_.rot().z() != 0.) && (origin_.rot().u() != 1.) );
// walk the tree one to find the number of nodes in each category
// (this is used to set up the OpenGL arrays)
// TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead...
unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0);
for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth),
end=tree.end_tree(); it!= end; ++it) {
if (it.isLeaf()) {
if (tree.isNodeOccupied(*it)){ // occupied voxels
if (tree.isNodeAtThreshold(*it)) ++cnt_occupied_thres;
else ++cnt_occupied;
}
else if (showAll) { // freespace voxels
if (tree.isNodeAtThreshold(*it)) ++cnt_free_thres;
else ++cnt_free;
}
}
}
// setup GL arrays for cube quads and cube colors
initGLArrays(cnt_occupied , m_occupiedSize , &m_occupiedArray , &m_occupiedColorArray);
initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray);
initGLArrays(cnt_free , m_freeSize , &m_freeArray, NULL);
initGLArrays(cnt_free_thres , m_freeThresSize , &m_freeThresArray, NULL);
std::vector<octomath::Vector3> cube_template;
initCubeTemplate(origin, cube_template);
unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0);
unsigned int color_idx_occupied(0), color_idx_occupied_thres(0);
m_grid_voxels.clear();
OcTreeVolume voxel; // current voxel, possibly transformed
for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth),
end=tree.end_tree(); it!= end; ++it) {
if (it.isLeaf()) { // voxels for leaf nodes
if (uses_origin)
voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
else
voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
if (tree.isNodeOccupied(*it)){ // occupied voxels
if (tree.isNodeAtThreshold(*it)) {
idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray);
color_idx_occupied_thres = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b,
(unsigned char) (it->getOccupancy() * 255.),
color_idx_occupied_thres, &m_occupiedThresColorArray);
}
else {
idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray);
color_idx_occupied = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b,
(unsigned char)(it->getOccupancy() * 255.),
color_idx_occupied, &m_occupiedColorArray);
}
}
else if (showAll) { // freespace voxels
if (tree.isNodeAtThreshold(*it)) {
idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray);
}
else {
idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray);
}
}
// grid structure voxel
if (showAll) m_grid_voxels.push_back(voxel);
}
else { // inner node voxels (for grid structure only)
if (showAll) {
if (uses_origin)
voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
else
voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
m_grid_voxels.push_back(voxel);
}
}
} // end for all voxels
m_octree_grid_vis_initialized = false;
if(m_drawOcTreeGrid)
initOctreeGridVis();
}
