Loading...
Searching...
No Matches
voxel_grid_occlusion_estimation.hpp
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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 Willow Garage, Inc. 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 : Christian Potthast
* Email : potthast@usc.edu
*
*/
#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
#define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
#include <pcl/filters/voxel_grid_occlusion_estimation.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::VoxelGridOcclusionEstimation<PointT>::initializeVoxelGrid ()
{
// initialization set to true
initialized_ = true;
// create the voxel grid and store the output cloud
this->filter (filtered_cloud_);
// Get the minimum and maximum bounding box dimensions
b_min_[0] = (static_cast<float> ( min_b_[0]) * leaf_size_[0]);
b_min_[1] = (static_cast<float> ( min_b_[1]) * leaf_size_[1]);
b_min_[2] = (static_cast<float> ( min_b_[2]) * leaf_size_[2]);
b_max_[0] = (static_cast<float> ( (max_b_[0]) + 1) * leaf_size_[0]);
b_max_[1] = (static_cast<float> ( (max_b_[1]) + 1) * leaf_size_[1]);
b_max_[2] = (static_cast<float> ( (max_b_[2]) + 1) * leaf_size_[2]);
// set the sensor origin and sensor orientation
sensor_origin_ = filtered_cloud_.sensor_origin_;
sensor_orientation_ = filtered_cloud_.sensor_orientation_;
Eigen::Vector3i ijk = this->getGridCoordinates(sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
// first check whether the sensor origin is within the voxel grid bounding box, then whether it is occupied
if((ijk[0]>=min_b_[0] && ijk[1]>=min_b_[1] && ijk[2]>=min_b_[2] && ijk[0]<=max_b_[0] && ijk[1]<=max_b_[1] && ijk[2]<=max_b_[2]) && this->getCentroidIndexAt (ijk) != -1) {
PCL_WARN ("[VoxelGridOcclusionEstimation::initializeVoxelGrid] The voxel containing the sensor origin (%g, %g, %g) is marked as occupied. We will instead assume that it is free, to be able to perform the occlusion estimation.\n", sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
this->leaf_layout_[((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (this->divb_mul_)] = -1;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimation (int& out_state,
const Eigen::Vector3i& in_target_voxel)
{
if (!initialized_)
{
PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
return -1;
}
// estimate direction to target voxel
Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
Eigen::Vector4f direction = p - sensor_origin_;
direction.normalize ();
// estimate entry point into the voxel grid
float tmin = rayBoxIntersection (sensor_origin_, direction);
if (tmin == -1)
{
PCL_ERROR ("The ray does not intersect with the bounding box \n");
return -1;
}
// ray traversal
out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin);
return 0;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimation (int& out_state,
std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& in_target_voxel)
{
if (!initialized_)
{
PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
return -1;
}
// estimate direction to target voxel
Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
Eigen::Vector4f direction = p - sensor_origin_;
direction.normalize ();
// estimate entry point into the voxel grid
float tmin = rayBoxIntersection (sensor_origin_, direction);
if (tmin == -1)
{
PCL_ERROR ("The ray does not intersect with the bounding box \n");
return -1;
}
// ray traversal
out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin);
return 0;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels)
{
if (!initialized_)
{
PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
return -1;
}
// reserve space for the ray vector
int reserve_size = div_b_[0] * div_b_[1] * div_b_[2];
occluded_voxels.reserve (reserve_size);
// iterate over the entire voxel grid
for (int kk = min_b_.z (); kk <= max_b_.z (); ++kk)
for (int jj = min_b_.y (); jj <= max_b_.y (); ++jj)
for (int ii = min_b_.x (); ii <= max_b_.x (); ++ii)
{
Eigen::Vector3i ijk (ii, jj, kk);
// process all free voxels
int index = this->getCentroidIndexAt (ijk);
if (index == -1)
{
// estimate direction to target voxel
Eigen::Vector4f p = getCentroidCoordinate (ijk);
Eigen::Vector4f direction = p - sensor_origin_;
direction.normalize ();
// estimate entry point into the voxel grid
float tmin = rayBoxIntersection (sensor_origin_, direction);
// ray traversal
int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
// if voxel is occluded
if (state == 1)
occluded_voxels.push_back (ijk);
}
}
return 0;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> float
pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction)
{
float tmin, tmax, tymin, tymax, tzmin, tzmax;
if (direction[0] >= 0)
{
tmin = (b_min_[0] - origin[0]) / direction[0];
tmax = (b_max_[0] - origin[0]) / direction[0];
}
else
{
tmin = (b_max_[0] - origin[0]) / direction[0];
tmax = (b_min_[0] - origin[0]) / direction[0];
}
if (direction[1] >= 0)
{
tymin = (b_min_[1] - origin[1]) / direction[1];
tymax = (b_max_[1] - origin[1]) / direction[1];
}
else
{
tymin = (b_max_[1] - origin[1]) / direction[1];
tymax = (b_min_[1] - origin[1]) / direction[1];
}
if ((tmin > tymax) || (tymin > tmax))
{
PCL_ERROR ("no intersection with the bounding box \n");
tmin = -1.0f;
return tmin;
}
if (tymin > tmin)
tmin = tymin;
if (tymax < tmax)
tmax = tymax;
if (direction[2] >= 0)
{
tzmin = (b_min_[2] - origin[2]) / direction[2];
tzmax = (b_max_[2] - origin[2]) / direction[2];
}
else
{
tzmin = (b_max_[2] - origin[2]) / direction[2];
tzmax = (b_min_[2] - origin[2]) / direction[2];
}
if ((tmin > tzmax) || (tzmin > tmax))
{
PCL_ERROR ("no intersection with the bounding box \n");
tmin = -1.0f;
return tmin;
}
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
return std::max<float>(tmin, 0.0f); // We only want to go in "positive" direction here, not in negative. This is relevant if the origin is inside the bounding box
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i& target_voxel,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction,
const float t_min)
{
// coordinate of the boundary of the voxel grid. To avoid numerical imprecision
// causing the start voxel index to be off by one, we add the machine epsilon
Eigen::Vector4f start = origin + (t_min > 0.0f ? (t_min + std::numeric_limits<float>::epsilon()) : t_min) * direction;
// i,j,k coordinate of the voxel were the ray enters the voxel grid
Eigen::Vector3i ijk = this->getGridCoordinates(start[0], start[1], start[2]);
// steps in which direction we have to travel in the voxel grid
int step_x, step_y, step_z;
// centroid coordinate of the entry voxel
Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
if (direction[0] >= 0)
{
voxel_max[0] += leaf_size_[0] * 0.5f;
step_x = 1;
}
else
{
voxel_max[0] -= leaf_size_[0] * 0.5f;
step_x = -1;
}
if (direction[1] >= 0)
{
voxel_max[1] += leaf_size_[1] * 0.5f;
step_y = 1;
}
else
{
voxel_max[1] -= leaf_size_[1] * 0.5f;
step_y = -1;
}
if (direction[2] >= 0)
{
voxel_max[2] += leaf_size_[2] * 0.5f;
step_z = 1;
}
else
{
voxel_max[2] -= leaf_size_[2] * 0.5f;
step_z = -1;
}
float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
(ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
{
// check if we reached target voxel
if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
return 0;
// index of the point in the point cloud
int index = this->getCentroidIndexAt (ijk);
// check if voxel is occupied, if yes return 1 for occluded
if (index != -1)
return 1;
// estimate next voxel
if(t_max_x <= t_max_y && t_max_x <= t_max_z)
{
t_max_x += t_delta_x;
ijk[0] += step_x;
}
else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
{
t_max_y += t_delta_y;
ijk[1] += step_y;
}
else
{
t_max_z += t_delta_z;
ijk[2] += step_z;
}
}
return 0;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& target_voxel,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction,
const float t_min)
{
// reserve space for the ray vector
int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
out_ray.reserve (reserve_size);
// coordinate of the boundary of the voxel grid. To avoid numerical imprecision
// causing the start voxel index to be off by one, we add the machine epsilon
Eigen::Vector4f start = origin + (t_min > 0.0f ? (t_min + std::numeric_limits<float>::epsilon()) : t_min) * direction;
// i,j,k coordinate of the voxel were the ray enters the voxel grid
Eigen::Vector3i ijk = this->getGridCoordinates (start[0], start[1], start[2]);
// steps in which direction we have to travel in the voxel grid
int step_x, step_y, step_z;
// centroid coordinate of the entry voxel
Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
if (direction[0] >= 0)
{
voxel_max[0] += leaf_size_[0] * 0.5f;
step_x = 1;
}
else
{
voxel_max[0] -= leaf_size_[0] * 0.5f;
step_x = -1;
}
if (direction[1] >= 0)
{
voxel_max[1] += leaf_size_[1] * 0.5f;
step_y = 1;
}
else
{
voxel_max[1] -= leaf_size_[1] * 0.5f;
step_y = -1;
}
if (direction[2] >= 0)
{
voxel_max[2] += leaf_size_[2] * 0.5f;
step_z = 1;
}
else
{
voxel_max[2] -= leaf_size_[2] * 0.5f;
step_z = -1;
}
float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
// the index of the cloud (-1 if empty)
int result = 0;
while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
(ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
{
// add voxel to ray
out_ray.push_back (ijk);
// check if we reached target voxel
if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
break;
// check if voxel is occupied
int index = this->getCentroidIndexAt (ijk);
if (index != -1)
result = 1;
// estimate next voxel
if(t_max_x <= t_max_y && t_max_x <= t_max_z)
{
t_max_x += t_delta_x;
ijk[0] += step_x;
}
else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
{
t_max_y += t_delta_y;
ijk[1] += step_y;
}
else
{
t_max_z += t_delta_z;
ijk[2] += step_z;
}
}
return result;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation<T>;
#endif // PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
pcl::VoxelGridOcclusionEstimation::initializeVoxelGrid
void initializeVoxelGrid()
Initialize the voxel grid, needs to be called first Builts the voxel grid and computes additional val...
Definition voxel_grid_occlusion_estimation.hpp:46
pcl::VoxelGridOcclusionEstimation::occlusionEstimationAll
int occlusionEstimationAll(std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &occluded_voxels)
Computes the voxel coordinates (i, j, k) of all occluded voxels in the voxel grid.
Definition voxel_grid_occlusion_estimation.hpp:139
pcl::VoxelGridOcclusionEstimation::rayBoxIntersection
float rayBoxIntersection(const Eigen::Vector4f &origin, const Eigen::Vector4f &direction)
Returns the scaling value (tmin) were the ray intersects with the voxel grid bounding box.
Definition voxel_grid_occlusion_estimation.hpp:182
pcl::VoxelGridOcclusionEstimation::rayTraversal
int rayTraversal(const Eigen::Vector3i &target_voxel, const Eigen::Vector4f &origin, const Eigen::Vector4f &direction, const float t_min)
Returns the state of the target voxel (0 = visible, 1 = occupied) using a ray traversal algorithm.
Definition voxel_grid_occlusion_estimation.hpp:248
pcl::VoxelGridOcclusionEstimation::occlusionEstimation
int occlusionEstimation(int &out_state, const Eigen::Vector3i &in_target_voxel)
Computes the state (free = 0, occluded = 1) of the voxel after utilizing a ray traversal algorithm to...
Definition voxel_grid_occlusion_estimation.hpp:76