voxel_grid.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <voxel_grid/voxel_grid.h>
00038 #include <sys/time.h>
00039 #include <ros/console.h>
00040 
00041 namespace voxel_grid {
00042   VoxelGrid::VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z)
00043   {
00044     size_x_ = size_x; 
00045     size_y_ = size_y; 
00046     size_z_ = size_z; 
00047 
00048     if(size_z_ > 16){
00049       ROS_INFO("Error, this implementation can only support up to 16 z values"); 
00050       size_z_ = 16;
00051     }
00052 
00053     data_ = new uint32_t[size_x_ * size_y_];
00054     uint32_t unknown_col = ~((uint32_t)0)>>16;
00055     uint32_t* col = data_;
00056     for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
00057       *col = unknown_col;
00058       ++col;
00059     }
00060   }
00061 
00062   void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
00063   {
00064     //if we're not actually changing the size, we can just reset things
00065     if(size_x == size_x_ && size_y == size_y_ && size_z == size_z_){
00066       reset();
00067       return;
00068     }
00069 
00070     delete[] data_;
00071     size_x_ = size_x; 
00072     size_y_ = size_y; 
00073     size_z_ = size_z; 
00074 
00075     if(size_z_ > 16){
00076       ROS_INFO("Error, this implementation can only support up to 16 z values"); 
00077       size_z_ = 16;
00078     }
00079 
00080     data_ = new uint32_t[size_x_ * size_y_];
00081     uint32_t unknown_col = ~((uint32_t)0)>>16;
00082     uint32_t* col = data_;
00083     for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
00084       *col = unknown_col;
00085       ++col;
00086     }
00087   }
00088 
00089   VoxelGrid::~VoxelGrid()
00090   {
00091     delete [] data_;
00092   }
00093 
00094   void VoxelGrid::reset(){
00095     uint32_t unknown_col = ~((uint32_t)0)>>16;
00096     uint32_t* col = data_;
00097     for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
00098       *col = unknown_col;
00099       ++col;
00100     }
00101   }
00102 
00103   void VoxelGrid::markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length){
00104     if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
00105       ROS_DEBUG("Error, line endpoint out of bounds. (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f),  size: (%d, %d, %d)", x0, y0, z0, x1, y1, z1, 
00106           size_x_, size_y_, size_z_);
00107       return;
00108     }
00109 
00110     MarkVoxel mv(data_);
00111     raytraceLine(mv, x0, y0, z0, x1, y1, z1, max_length);
00112   }
00113 
00114   void VoxelGrid::clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length){
00115     if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
00116       ROS_DEBUG("Error, line endpoint out of bounds. (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f),  size: (%d, %d, %d)", x0, y0, z0, x1, y1, z1, 
00117           size_x_, size_y_, size_z_);
00118       return;
00119     }
00120 
00121     ClearVoxel cv(data_);
00122     raytraceLine(cv, x0, y0, z0, x1, y1, z1, max_length);
00123   }
00124 
00125   void VoxelGrid::clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d, 
00126       unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost, unsigned char unknown_cost, unsigned int max_length){
00127     costmap = map_2d;
00128     if(map_2d == NULL){
00129       clearVoxelLine(x0, y0, z0, x1, y1, z1, max_length);
00130       return;
00131     }
00132 
00133     if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
00134       ROS_DEBUG("Error, line endpoint out of bounds. (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f),  size: (%d, %d, %d)", x0, y0, z0, x1, y1, z1, 
00135           size_x_, size_y_, size_z_);
00136       return;
00137     }
00138 
00139     ClearVoxelInMap cvm(data_, costmap, unknown_threshold, mark_threshold, free_cost, unknown_cost);
00140     raytraceLine(cvm, x0, y0, z0, x1, y1, z1, max_length);
00141   }
00142 
00143   VoxelStatus VoxelGrid::getVoxel(unsigned int x, unsigned int y, unsigned int z)
00144   {
00145     if(x >= size_x_ || y >= size_y_ || z >= size_z_){
00146       ROS_DEBUG("Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
00147       return UNKNOWN;
00148     }
00149     uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00150     uint32_t result = data_[y * size_x_ + x] & full_mask; 
00151     unsigned int bits = numBits(result);
00152 
00153     // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
00154     if(bits < 2){
00155       if(bits < 1)
00156         return FREE;
00157 
00158       return UNKNOWN;
00159     }
00160 
00161     return MARKED;
00162   }
00163 
00164   VoxelStatus VoxelGrid::getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold, unsigned int marked_threshold)
00165   {
00166     if(x >= size_x_ || y >= size_y_){
00167       ROS_DEBUG("Error, voxel out of bounds. (%d, %d)\n", x, y);
00168       return UNKNOWN;
00169     }
00170     
00171     uint32_t* col = &data_[y * size_x_ + x];
00172 
00173     unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00174     unsigned int marked_bits = *col>>16;
00175 
00176     //check if the number of marked bits qualifies the col as marked
00177     if(!bitsBelowThreshold(marked_bits, marked_threshold)){
00178       return MARKED;
00179     }
00180 
00181     //check if the number of unkown bits qualifies the col as unknown
00182     if(!bitsBelowThreshold(unknown_bits, unknown_threshold))
00183       return UNKNOWN;
00184 
00185     return FREE;
00186   }
00187 
00188   unsigned int VoxelGrid::sizeX(){
00189     return size_x_;
00190   }
00191 
00192   unsigned int VoxelGrid::sizeY(){
00193     return size_y_;
00194   }
00195 
00196   unsigned int VoxelGrid::sizeZ(){
00197     return size_z_;
00198   }
00199 
00200   void VoxelGrid::printVoxelGrid(){
00201     for(unsigned int z = 0; z < size_z_; z++){
00202       printf("Layer z = %d:\n",z);
00203       for(unsigned int y = 0; y < size_y_; y++){
00204         for(unsigned int x = 0 ; x < size_x_; x++){
00205           printf((getVoxel(x, y, z)) == voxel_grid::MARKED? "#" : " ");
00206         }
00207         printf("|\n");
00208       } 
00209     }
00210   }
00211 
00212   void VoxelGrid::printColumnGrid(){
00213     printf("Column view:\n");
00214     for(unsigned int y = 0; y < size_y_; y++){
00215       for(unsigned int x = 0 ; x < size_x_; x++){
00216         printf((getVoxelColumn(x, y, 16, 0) == voxel_grid::MARKED)? "#" : " ");
00217       }
00218       printf("|\n");
00219     } 
00220   }
00221 };


voxel_grid
Author(s): Eitan Marder-Eppstein, Eric Berger
autogenerated on Mon Oct 6 2014 02:44:13