00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <octovis/ColorOcTreeDrawer.h>
00027
00028 namespace octomap {
00029
00030 ColorOcTreeDrawer::ColorOcTreeDrawer()
00031 : OcTreeDrawer() {
00032 }
00033
00034 ColorOcTreeDrawer::~ColorOcTreeDrawer() {
00035 }
00036
00037 void ColorOcTreeDrawer::setOcTree(const AbstractOcTree& tree_pnt,
00038 const octomap::pose6d& origin_,
00039 int map_id_) {
00040
00041 const ColorOcTree& tree = ((const ColorOcTree&) tree_pnt);
00042
00043 this->map_id = map_id_;
00044
00045
00046 this->initial_origin = octomap::pose6d(octomap::point3d(0,0,0), origin_.rot());
00047
00048 this->origin = origin_;
00049
00050
00051 bool showAll = (tree.size() < 5 * 1e6);
00052 bool uses_origin = ( (origin_.rot().x() != 0.) && (origin_.rot().y() != 0.)
00053 && (origin_.rot().z() != 0.) && (origin_.rot().u() != 1.) );
00054
00055
00056
00057
00058 unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0);
00059 for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth),
00060 end=tree.end_tree(); it!= end; ++it) {
00061 if (it.isLeaf()) {
00062 if (tree.isNodeOccupied(*it)){
00063 if (tree.isNodeAtThreshold(*it)) ++cnt_occupied_thres;
00064 else ++cnt_occupied;
00065 }
00066 else if (showAll) {
00067 if (tree.isNodeAtThreshold(*it)) ++cnt_free_thres;
00068 else ++cnt_free;
00069 }
00070 }
00071 }
00072
00073 initGLArrays(cnt_occupied , m_occupiedSize , &m_occupiedArray , &m_occupiedColorArray);
00074 initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray);
00075 initGLArrays(cnt_free , m_freeSize , &m_freeArray, NULL);
00076 initGLArrays(cnt_free_thres , m_freeThresSize , &m_freeThresArray, NULL);
00077
00078 std::vector<octomath::Vector3> cube_template;
00079 initCubeTemplate(origin, cube_template);
00080
00081 unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0);
00082 unsigned int color_idx_occupied(0), color_idx_occupied_thres(0);
00083
00084 m_grid_voxels.clear();
00085 OcTreeVolume voxel;
00086 for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth),
00087 end=tree.end_tree(); it!= end; ++it) {
00088
00089 if (it.isLeaf()) {
00090 if (uses_origin)
00091 voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
00092 else
00093 voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
00094
00095 if (tree.isNodeOccupied(*it)){
00096 if (tree.isNodeAtThreshold(*it)) {
00097 idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray);
00098 color_idx_occupied_thres = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b,
00099 (unsigned char) (it->getOccupancy() * 255.),
00100 color_idx_occupied_thres, &m_occupiedThresColorArray);
00101 }
00102 else {
00103 idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray);
00104 color_idx_occupied = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b,
00105 (unsigned char)(it->getOccupancy() * 255.),
00106 color_idx_occupied, &m_occupiedColorArray);
00107 }
00108 }
00109 else if (showAll) {
00110 if (tree.isNodeAtThreshold(*it)) {
00111 idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray);
00112 }
00113 else {
00114 idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray);
00115 }
00116 }
00117
00118
00119 if (showAll) m_grid_voxels.push_back(voxel);
00120 }
00121
00122 else {
00123 if (showAll) {
00124 if (uses_origin)
00125 voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
00126 else
00127 voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
00128 m_grid_voxels.push_back(voxel);
00129 }
00130 }
00131 }
00132
00133 m_octree_grid_vis_initialized = false;
00134
00135 if(m_drawOcTreeGrid)
00136 initOctreeGridVis();
00137 }
00138
00139 }