first commit
This commit is contained in:
222
src/voxel_grid.cpp
Executable file
222
src/voxel_grid.cpp
Executable file
@@ -0,0 +1,222 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* Author: Eitan Marder-Eppstein
|
||||
*********************************************************************/
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
#ifdef HAVE_SYS_TIME_H
|
||||
#include <sys/time.h>
|
||||
#endif
|
||||
|
||||
#include <iostream>
|
||||
#include <cstdio>
|
||||
|
||||
namespace voxel_grid {
|
||||
VoxelGrid::VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z)
|
||||
{
|
||||
size_x_ = size_x;
|
||||
size_y_ = size_y;
|
||||
size_z_ = size_z;
|
||||
|
||||
if(size_z_ > 16){
|
||||
std::cerr << "Error, this implementation can only support up to 16 z values (" << size_z_ << ")" << std::endl;
|
||||
size_z_ = 16;
|
||||
}
|
||||
|
||||
data_ = new uint32_t[size_x_ * size_y_];
|
||||
uint32_t unknown_col = ~((uint32_t)0)>>16;
|
||||
uint32_t* col = data_;
|
||||
for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
|
||||
*col = unknown_col;
|
||||
++col;
|
||||
}
|
||||
}
|
||||
|
||||
void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
|
||||
{
|
||||
//if we're not actually changing the size, we can just reset things
|
||||
if(size_x == size_x_ && size_y == size_y_ && size_z == size_z_){
|
||||
reset();
|
||||
return;
|
||||
}
|
||||
|
||||
delete[] data_;
|
||||
size_x_ = size_x;
|
||||
size_y_ = size_y;
|
||||
size_z_ = size_z;
|
||||
|
||||
if(size_z_ > 16){
|
||||
std::cerr << "Error, this implementation can only support up to 16 z values (" << size_z << ")" << std::endl;
|
||||
size_z_ = 16;
|
||||
}
|
||||
|
||||
data_ = new uint32_t[size_x_ * size_y_];
|
||||
uint32_t unknown_col = ~((uint32_t)0)>>16;
|
||||
uint32_t* col = data_;
|
||||
for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
|
||||
*col = unknown_col;
|
||||
++col;
|
||||
}
|
||||
}
|
||||
|
||||
VoxelGrid::~VoxelGrid()
|
||||
{
|
||||
delete [] data_;
|
||||
}
|
||||
|
||||
void VoxelGrid::reset(){
|
||||
uint32_t unknown_col = ~((uint32_t)0)>>16;
|
||||
uint32_t* col = data_;
|
||||
for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
|
||||
*col = unknown_col;
|
||||
++col;
|
||||
}
|
||||
}
|
||||
|
||||
void VoxelGrid::markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length){
|
||||
if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
|
||||
// Line endpoint out of bounds - silently return
|
||||
return;
|
||||
}
|
||||
|
||||
MarkVoxel mv(data_);
|
||||
raytraceLine(mv, x0, y0, z0, x1, y1, z1, max_length);
|
||||
}
|
||||
|
||||
void VoxelGrid::clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length){
|
||||
if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
|
||||
// Line endpoint out of bounds - silently return
|
||||
return;
|
||||
}
|
||||
|
||||
ClearVoxel cv(data_);
|
||||
raytraceLine(cv, x0, y0, z0, x1, y1, z1, max_length);
|
||||
}
|
||||
|
||||
void VoxelGrid::clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d,
|
||||
unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost, unsigned char unknown_cost, unsigned int max_length){
|
||||
costmap = map_2d;
|
||||
if(map_2d == NULL){
|
||||
clearVoxelLine(x0, y0, z0, x1, y1, z1, max_length);
|
||||
return;
|
||||
}
|
||||
|
||||
if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
|
||||
// Line endpoint out of bounds - silently return
|
||||
return;
|
||||
}
|
||||
|
||||
ClearVoxelInMap cvm(data_, costmap, unknown_threshold, mark_threshold, free_cost, unknown_cost);
|
||||
raytraceLine(cvm, x0, y0, z0, x1, y1, z1, max_length);
|
||||
}
|
||||
|
||||
VoxelStatus VoxelGrid::getVoxel(unsigned int x, unsigned int y, unsigned int z)
|
||||
{
|
||||
if(x >= size_x_ || y >= size_y_ || z >= size_z_){
|
||||
// Voxel out of bounds
|
||||
return UNKNOWN;
|
||||
}
|
||||
uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
|
||||
uint32_t result = data_[y * size_x_ + x] & full_mask;
|
||||
unsigned int bits = numBits(result);
|
||||
|
||||
// known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
|
||||
if(bits < 2){
|
||||
if(bits < 1)
|
||||
return FREE;
|
||||
|
||||
return UNKNOWN;
|
||||
}
|
||||
|
||||
return MARKED;
|
||||
}
|
||||
|
||||
VoxelStatus VoxelGrid::getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold, unsigned int marked_threshold)
|
||||
{
|
||||
if(x >= size_x_ || y >= size_y_){
|
||||
// Voxel out of bounds
|
||||
return UNKNOWN;
|
||||
}
|
||||
|
||||
uint32_t* col = &data_[y * size_x_ + x];
|
||||
|
||||
unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
|
||||
unsigned int marked_bits = *col>>16;
|
||||
|
||||
//check if the number of marked bits qualifies the col as marked
|
||||
if(!bitsBelowThreshold(marked_bits, marked_threshold)){
|
||||
return MARKED;
|
||||
}
|
||||
|
||||
//check if the number of unkown bits qualifies the col as unknown
|
||||
if(!bitsBelowThreshold(unknown_bits, unknown_threshold))
|
||||
return UNKNOWN;
|
||||
|
||||
return FREE;
|
||||
}
|
||||
|
||||
unsigned int VoxelGrid::sizeX(){
|
||||
return size_x_;
|
||||
}
|
||||
|
||||
unsigned int VoxelGrid::sizeY(){
|
||||
return size_y_;
|
||||
}
|
||||
|
||||
unsigned int VoxelGrid::sizeZ(){
|
||||
return size_z_;
|
||||
}
|
||||
|
||||
void VoxelGrid::printVoxelGrid(){
|
||||
for(unsigned int z = 0; z < size_z_; z++){
|
||||
printf("Layer z = %u:\n",z);
|
||||
for(unsigned int y = 0; y < size_y_; y++){
|
||||
for(unsigned int x = 0 ; x < size_x_; x++){
|
||||
printf((getVoxel(x, y, z)) == voxel_grid::MARKED? "#" : " ");
|
||||
}
|
||||
printf("|\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VoxelGrid::printColumnGrid(){
|
||||
printf("Column view:\n");
|
||||
for(unsigned int y = 0; y < size_y_; y++){
|
||||
for(unsigned int x = 0 ; x < size_x_; x++){
|
||||
printf((getVoxelColumn(x, y, 16, 0) == voxel_grid::MARKED)? "#" : " ");
|
||||
}
|
||||
printf("|\n");
|
||||
}
|
||||
}
|
||||
};
|
||||
Reference in New Issue
Block a user