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
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
00038 #define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
00039
00040 #include <algorithm>
00041 #include <cmath>
00042 #include <Eigen/Core>
00043 #include <moveit/macros/declare_ptr.h>
00044
00045 namespace distance_field
00046 {
00048 enum Dimension
00049 {
00050 DIM_X = 0,
00051 DIM_Y = 1,
00052 DIM_Z = 2
00053 };
00054
00061 template <typename T>
00062 class VoxelGrid
00063 {
00064 public:
00065 MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid);
00066
00092 VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
00093 double origin_z, T default_object);
00094 virtual ~VoxelGrid();
00095
00102 VoxelGrid();
00103
00121 void resize(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
00122 double origin_z, T default_object);
00123
00137 const T& operator()(double x, double y, double z) const;
00138 const T& operator()(const Eigen::Vector3d& pos) const;
00139
00157 T& getCell(int x, int y, int z);
00158 T& getCell(const Eigen::Vector3i& pos);
00159 const T& getCell(int x, int y, int z) const;
00160 const T& getCell(const Eigen::Vector3i& pos) const;
00161
00180 void setCell(int x, int y, int z, const T& obj);
00181 void setCell(const Eigen::Vector3i& pos, const T& obj);
00182
00188 void reset(const T& initial);
00189
00197 double getSize(Dimension dim) const;
00198
00204 double getResolution() const;
00205
00207 double getResolution(Dimension dim) const;
00208
00216 double getOrigin(Dimension dim) const;
00217
00225 int getNumCells(Dimension dim) const;
00226
00245 void gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00246 void gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3i& world) const;
00247
00264 bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00265 bool worldToGrid(const Eigen::Vector3i& world, Eigen::Vector3i& grid) const;
00266
00276 bool isCellValid(int x, int y, int z) const;
00277 bool isCellValid(const Eigen::Vector3i& pos) const;
00278
00287 bool isCellValid(Dimension dim, int cell) const;
00288
00289 protected:
00290 T* data_;
00291 T default_object_;
00292 T*** data_ptrs_;
00293 double size_[3];
00294 double resolution_;
00295 double oo_resolution_;
00296 double origin_[3];
00297 double origin_minus_[3];
00298 int num_cells_[3];
00299 int num_cells_total_;
00300 int stride1_;
00301 int stride2_;
00312 int ref(int x, int y, int z) const;
00313
00326 int getCellFromLocation(Dimension dim, double loc) const;
00327
00337 double getLocationFromCell(Dimension dim, int cell) const;
00338 };
00339
00341
00342 template <typename T>
00343 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x,
00344 double origin_y, double origin_z, T default_object)
00345 : data_(NULL)
00346 {
00347 resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
00348 }
00349
00350 template <typename T>
00351 VoxelGrid<T>::VoxelGrid() : data_(NULL)
00352 {
00353 for (int i = DIM_X; i <= DIM_Z; ++i)
00354 {
00355 size_[i] = 0;
00356 origin_[i] = 0;
00357 origin_minus_[i] = 0;
00358 num_cells_[i] = 0;
00359 }
00360 resolution_ = 1.0;
00361 oo_resolution_ = 1.0 / resolution_;
00362 num_cells_total_ = 0;
00363 stride1_ = 0;
00364 stride2_ = 0;
00365 }
00366
00367 template <typename T>
00368 void VoxelGrid<T>::resize(double size_x, double size_y, double size_z, double resolution, double origin_x,
00369 double origin_y, double origin_z, T default_object)
00370 {
00371 delete[] data_;
00372 data_ = NULL;
00373
00374 size_[DIM_X] = size_x;
00375 size_[DIM_Y] = size_y;
00376 size_[DIM_Z] = size_z;
00377 origin_[DIM_X] = origin_x;
00378 origin_[DIM_Y] = origin_y;
00379 origin_[DIM_Z] = origin_z;
00380 origin_minus_[DIM_X] = origin_x - 0.5 * resolution;
00381 origin_minus_[DIM_Y] = origin_y - 0.5 * resolution;
00382 origin_minus_[DIM_Z] = origin_z - 0.5 * resolution;
00383 num_cells_total_ = 1;
00384 resolution_ = resolution;
00385 oo_resolution_ = 1.0 / resolution_;
00386 for (int i = DIM_X; i <= DIM_Z; ++i)
00387 {
00388 num_cells_[i] = size_[i] * oo_resolution_;
00389 num_cells_total_ *= num_cells_[i];
00390 }
00391
00392 default_object_ = default_object;
00393
00394 stride1_ = num_cells_[DIM_Y] * num_cells_[DIM_Z];
00395 stride2_ = num_cells_[DIM_Z];
00396
00397
00398 if (num_cells_total_ > 0)
00399 data_ = new T[num_cells_total_];
00400 }
00401
00402 template <typename T>
00403 VoxelGrid<T>::~VoxelGrid()
00404 {
00405 delete[] data_;
00406 }
00407
00408 template <typename T>
00409 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
00410 {
00411 return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]);
00412 }
00413
00414 template <typename T>
00415 inline bool VoxelGrid<T>::isCellValid(const Eigen::Vector3i& pos) const
00416 {
00417 return isCellValid(pos.x(), pos.y(), pos.z());
00418 }
00419
00420 template <typename T>
00421 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
00422 {
00423 return cell >= 0 && cell < num_cells_[dim];
00424 }
00425
00426 template <typename T>
00427 inline int VoxelGrid<T>::ref(int x, int y, int z) const
00428 {
00429 return x * stride1_ + y * stride2_ + z;
00430 }
00431
00432 template <typename T>
00433 inline double VoxelGrid<T>::getSize(Dimension dim) const
00434 {
00435 return size_[dim];
00436 }
00437
00438 template <typename T>
00439 inline double VoxelGrid<T>::getResolution() const
00440 {
00441 return resolution_;
00442 }
00443
00444 template <typename T>
00445 inline double VoxelGrid<T>::getResolution(Dimension dim) const
00446 {
00447 return resolution_;
00448 }
00449
00450 template <typename T>
00451 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
00452 {
00453 return origin_[dim];
00454 }
00455
00456 template <typename T>
00457 inline int VoxelGrid<T>::getNumCells(Dimension dim) const
00458 {
00459 return num_cells_[dim];
00460 }
00461
00462 template <typename T>
00463 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
00464 {
00465 int cellX = getCellFromLocation(DIM_X, x);
00466 int cellY = getCellFromLocation(DIM_Y, y);
00467 int cellZ = getCellFromLocation(DIM_Z, z);
00468 if (!isCellValid(cellX, cellY, cellZ))
00469 return default_object_;
00470 return getCell(cellX, cellY, cellZ);
00471 }
00472
00473 template <typename T>
00474 inline const T& VoxelGrid<T>::operator()(const Eigen::Vector3d& pos) const
00475 {
00476 return this->operator()(pos.x(), pos.y(), pos.z());
00477 }
00478
00479 template <typename T>
00480 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
00481 {
00482 return data_[ref(x, y, z)];
00483 }
00484
00485 template <typename T>
00486 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
00487 {
00488 return data_[ref(x, y, z)];
00489 }
00490
00491 template <typename T>
00492 inline T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos)
00493 {
00494 return data_[ref(pos.x(), pos.y(), pos.z())];
00495 }
00496
00497 template <typename T>
00498 inline const T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos) const
00499 {
00500 return data_[ref(pos.x(), pos.y(), pos.z())];
00501 }
00502
00503 template <typename T>
00504 inline void VoxelGrid<T>::setCell(int x, int y, int z, const T& obj)
00505 {
00506 data_[ref(x, y, z)] = obj;
00507 }
00508
00509 template <typename T>
00510 inline void VoxelGrid<T>::setCell(const Eigen::Vector3i& pos, const T& obj)
00511 {
00512 data_[ref(pos.x(), pos.y(), pos.z())] = obj;
00513 }
00514
00515 template <typename T>
00516 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
00517 {
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532 return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
00533 }
00534
00535 template <typename T>
00536 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
00537 {
00538 return origin_[dim] + resolution_ * (double(cell));
00539 }
00540
00541 template <typename T>
00542 inline void VoxelGrid<T>::reset(const T& initial)
00543 {
00544 std::fill(data_, data_ + num_cells_total_, initial);
00545 }
00546
00547 template <typename T>
00548 inline void VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00549 {
00550 world_x = getLocationFromCell(DIM_X, x);
00551 world_y = getLocationFromCell(DIM_Y, y);
00552 world_z = getLocationFromCell(DIM_Z, z);
00553 }
00554
00555 template <typename T>
00556 inline void VoxelGrid<T>::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3i& world) const
00557 {
00558 world.x() = getLocationFromCell(DIM_X, grid.x());
00559 world.y() = getLocationFromCell(DIM_Y, grid.y());
00560 world.z() = getLocationFromCell(DIM_Z, grid.z());
00561 }
00562
00563 template <typename T>
00564 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00565 {
00566 x = getCellFromLocation(DIM_X, world_x);
00567 y = getCellFromLocation(DIM_Y, world_y);
00568 z = getCellFromLocation(DIM_Z, world_z);
00569 return isCellValid(x, y, z);
00570 }
00571
00572 template <typename T>
00573 inline bool VoxelGrid<T>::worldToGrid(const Eigen::Vector3i& world, Eigen::Vector3i& grid) const
00574 {
00575 grid.x() = getCellFromLocation(DIM_X, world.x());
00576 grid.y() = getCellFromLocation(DIM_Y, world.y());
00577 grid.z() = getCellFromLocation(DIM_Z, world.z());
00578 return isCellValid(grid.x(), grid.y(), grid.z());
00579 }
00580
00581 }
00582 #endif