Loading...
Searching...
No Matches
world_model.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, 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: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
*/
#ifndef PCL_WORLD_MODEL_IMPL_HPP_
#define PCL_WORLD_MODEL_IMPL_HPP_
#include <pcl/gpu/kinfu_large_scale/world_model.h>
#include <pcl/common/transforms.h> // for transformPointCloud
template <typename PointT>
void
pcl::kinfuLS::WorldModel<PointT>::addSlice ( PointCloudPtr new_cloud)
{
PCL_DEBUG("Adding new cloud. Current world contains %zu points.\n",
static_cast<std::size_t>(world_->size()));
PCL_DEBUG("New slice contains %zu points.\n",
static_cast<std::size_t>(new_cloud->size()));
*world_ += *new_cloud;
PCL_DEBUG("World now contains %zu points.\n",
static_cast<std::size_t>(world_->size()));
}
template <typename PointT>
void
pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
{
double newOriginX = previous_origin_x + offset_x;
double newOriginY = previous_origin_y + offset_y;
double newOriginZ = previous_origin_z + offset_z;
double newLimitX = newOriginX + volume_x;
double newLimitY = newOriginY + volume_y;
double newLimitZ = newOriginZ + volume_z;
// filter points in the space of the new cube
PointCloudPtr newCube (new pcl::PointCloud<PointT>);
// condition
ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
// build the filter
pcl::ConditionalRemoval<PointT> condremAND (true);
condremAND.setCondition (range_condAND);
condremAND.setInputCloud (world_);
condremAND.setKeepOrganized (false);
// apply filter
condremAND.filter (*newCube);
// filter points that belong to the new slice
ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
if(offset_x >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
if(offset_y >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
if(offset_z >= 0)
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
else
range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
// build the filter
pcl::ConditionalRemoval<PointT> condrem (true);
condrem.setCondition (range_condOR);
condrem.setInputCloud (newCube);
condrem.setKeepOrganized (false);
// apply filter
condrem.filter (existing_slice);
if(!existing_slice.empty ())
{
//transform the slice in new cube coordinates
Eigen::Affine3f transformation;
transformation.translation ()[0] = newOriginX;
transformation.translation ()[1] = newOriginY;
transformation.translation ()[2] = newOriginZ;
transformation.linear ().setIdentity ();
transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
}
}
template <typename PointT>
void
pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap)
{
if(world_->points.empty ())
{
PCL_INFO("The world is empty, returning nothing\n");
return;
}
PCL_INFO("Getting world as cubes. World contains %zu points.\n",
static_cast<std::size_t>(world_->size()));
// remove nans from world cloud
world_->is_dense = false;
pcl::Indices indices;
pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
PCL_INFO("World contains %zu points after nan removal.\n",
static_cast<std::size_t>(world_->size()));
// check cube size value
double cubeSide = size;
if (cubeSide <= 0.0f)
{
PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
cubeSide = 512.0f;
}
std::cout << "cube size is set to " << cubeSide << std::endl;
// check overlap value
double step_increment = 1.0f - overlap;
if (overlap < 0.0)
{
PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
step_increment = 1.0f;
}
if (overlap > 1.0)
{
PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
step_increment = 0.1f;
}
// get world's bounding values on XYZ
PointT min, max;
pcl::getMinMax3D(*world_, min, max);
PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);
PointT origin = min;
// clear returned vectors
cubes.clear();
transforms.clear();
// iterate with box filter
while (origin.x < max.x)
{
origin.y = min.y;
while (origin.y < max.y)
{
origin.z = min.z;
while (origin.z < max.z)
{
// extract cube here
PCL_INFO ("Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
// pointcloud for current cube.
PointCloudPtr box (new pcl::PointCloud<PointT>);
// set conditional filter
ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));
// build the filter
pcl::ConditionalRemoval<PointT> condrem;
condrem.setCondition (range_cond);
condrem.setInputCloud (world_);
condrem.setKeepOrganized(false);
// apply filter
condrem.filter (*box);
// also push transform along with points.
if(!box->points.empty ())
{
Eigen::Vector3f transform;
transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
transforms.push_back(transform);
cubes.push_back(box);
}
else
{
PCL_INFO ("Extracted cube was empty, skipping this one.\n");
}
origin.z += cubeSide * step_increment;
}
origin.y += cubeSide * step_increment;
}
origin.x += cubeSide * step_increment;
}
/* for(int c = 0 ; c < cubes.size() ; ++c)
{
std::stringstream name;
name << "cloud" << c+1 << ".pcd";
pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
}*/
std::cout << "returning " << cubes.size() << " cubes" << std::endl;
}
template <typename PointT>
inline void
pcl::kinfuLS::WorldModel<PointT>::setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices)
{
std::vector<pcl::PCLPointField> fields;
pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
float my_nan = std::numeric_limits<float>::quiet_NaN ();
for (int rii = 0; rii < static_cast<int> (indices->size ()); ++rii) // rii = removed indices iterator
{
std::uint8_t* pt_data = reinterpret_cast<std::uint8_t*> (&(*cloud)[(*indices)[rii]]);
for (const auto &field : fields)
memcpy (pt_data + field.offset, &my_nan, sizeof (float));
}
}
template <typename PointT>
void
pcl::kinfuLS::WorldModel<PointT>::setSliceAsNans (const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
{
// PCL_DEBUG ("IN SETSLICE AS NANS\n");
PointCloudPtr slice (new pcl::PointCloud<PointT>);
// prepare filter limits on all dimensions
double previous_origin_x = origin_x;
double previous_limit_x = origin_x + size_x - 1;
double new_origin_x = origin_x + offset_x;
double new_limit_x = previous_limit_x + offset_x;
double previous_origin_y = origin_y;
double previous_limit_y = origin_y + size_y - 1;
double new_origin_y = origin_y + offset_y;
double new_limit_y = previous_limit_y + offset_y;
double previous_origin_z = origin_z;
double previous_limit_z = origin_z + size_z - 1;
double new_origin_z = origin_z + offset_z;
double new_limit_z = previous_limit_z + offset_z;
// get points of slice on X (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
double lower_limit_x, upper_limit_x;
if(offset_x >=0)
{
lower_limit_x = previous_origin_x;
upper_limit_x = new_origin_x;
}
else
{
lower_limit_x = new_limit_x;
upper_limit_x = previous_limit_x;
}
// PCL_DEBUG ("Limit X: [%f - %f]\n", lower_limit_x, upper_limit_x);
ConditionOrPtr range_cond_OR_x (new pcl::ConditionOr<PointT> ());
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, upper_limit_x ))); // filtered dimension
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, lower_limit_x ))); // filtered dimension
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
pcl::ConditionalRemoval<PointT> condrem_x (true);
condrem_x.setCondition (range_cond_OR_x);
condrem_x.setInputCloud (world_);
condrem_x.setKeepOrganized (false);
// apply filter
condrem_x.filter (*slice);
IndicesConstPtr indices_x = condrem_x.getRemovedIndices ();
//set outliers (so our slice points) to nan
setIndicesAsNans(world_, indices_x);
// PCL_DEBUG("%d points set to nan on X\n", indices_x->size ());
// get points of slice on Y (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
double lower_limit_y, upper_limit_y;
if(offset_y >=0)
{
lower_limit_y = previous_origin_y;
upper_limit_y = new_origin_y;
}
else
{
lower_limit_y = new_limit_y;
upper_limit_y = previous_limit_y;
}
// PCL_DEBUG ("Limit Y: [%f - %f]\n", lower_limit_y, upper_limit_y);
ConditionOrPtr range_cond_OR_y (new pcl::ConditionOr<PointT> ());
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, upper_limit_y))); // filtered dimension
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, lower_limit_y ))); // filtered dimension
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
pcl::ConditionalRemoval<PointT> condrem_y (true);
condrem_y.setCondition (range_cond_OR_y);
condrem_y.setInputCloud (world_);
condrem_y.setKeepOrganized (false);
// apply filter
condrem_y.filter (*slice);
IndicesConstPtr indices_y = condrem_y.getRemovedIndices ();
//set outliers (so our slice points) to nan
setIndicesAsNans(world_, indices_y);
// PCL_DEBUG ("%d points set to nan on Y\n", indices_y->size ());
// get points of slice on Z (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
double lower_limit_z, upper_limit_z;
if(offset_z >=0)
{
lower_limit_z = previous_origin_z;
upper_limit_z = new_origin_z;
}
else
{
lower_limit_z = new_limit_z;
upper_limit_z = previous_limit_z;
}
// PCL_DEBUG ("Limit Z: [%f - %f]\n", lower_limit_z, upper_limit_z);
ConditionOrPtr range_cond_OR_z (new pcl::ConditionOr<PointT> ());
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, upper_limit_z))); // filtered dimension
range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, lower_limit_z ))); // filtered dimension
pcl::ConditionalRemoval<PointT> condrem_z (true);
condrem_z.setCondition (range_cond_OR_z);
condrem_z.setInputCloud (world_);
condrem_z.setKeepOrganized (false);
// apply filter
condrem_z.filter (*slice);
IndicesConstPtr indices_z = condrem_z.getRemovedIndices ();
//set outliers (so our slice points) to nan
setIndicesAsNans(world_, indices_z);
// PCL_DEBUG("%d points set to nan on Z\n", indices_z->size ());
}
#define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
#endif // PCL_WORLD_MODEL_IMPL_HPP_
pcl::ConditionAnd
AND condition.
Definition conditional_removal.h:505
pcl::ConditionOr
OR condition.
Definition conditional_removal.h:533
pcl::ConditionalRemoval
ConditionalRemoval filters data that satisfies certain conditions.
Definition conditional_removal.h:592
pcl::ConditionalRemoval::setCondition
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
Definition conditional_removal.hpp:617
pcl::ConditionalRemoval::setKeepOrganized
void setKeepOrganized(bool val)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
Definition conditional_removal.h:635
pcl::FieldComparison
The field-based specialization of the comparison object.
Definition conditional_removal.h:130
pcl::Filter::filter
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition filter.h:121
pcl::Filter::getRemovedIndices
IndicesConstPtr const getRemovedIndices() const
Get the point indices being removed.
Definition filter.h:103
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::empty
bool empty() const
Definition point_cloud.h:446
pcl::kinfuLS::WorldModel
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
Definition world_model.h:63
pcl::kinfuLS::WorldModel::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition world_model.h:70
pcl::kinfuLS::WorldModel::setSliceAsNans
void setSliceAsNans(const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
Give nan values to the slice of the world.
Definition world_model.hpp:275
pcl::kinfuLS::WorldModel::getWorldAsCubes
void getWorldAsCubes(double size, std::vector< PointCloudPtr > &cubes, std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &transforms, double overlap=0.0)
Returns the world as two vectors of cubes of size "size" (pointclouds) and transforms.
Definition world_model.hpp:136
pcl::kinfuLS::WorldModel::FieldComparisonConstPtr
typename pcl::FieldComparison< PointT >::ConstPtr FieldComparisonConstPtr
Definition world_model.h:75
pcl::kinfuLS::WorldModel::getExistingData
void getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud< PointT > &existing_slice)
Retrieve existing data from the world model, after a shift.
Definition world_model.hpp:63
pcl::kinfuLS::WorldModel::ConditionAndPtr
typename pcl::ConditionAnd< PointT >::Ptr ConditionAndPtr
Definition world_model.h:73
pcl::kinfuLS::WorldModel::addSlice
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
Definition world_model.hpp:47
pcl::kinfuLS::WorldModel::ConditionOrPtr
typename pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
Definition world_model.h:74
pcl::getMinMax3D
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition common.hpp:295
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition transforms.hpp:221
pcl::removeNaNFromPointCloud
void removeNaNFromPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, Indices &index)
Removes points with x, y, or z equal to NaN.
Definition filter.hpp:46
pcl::ComparisonOps::GE
@ GE
Definition conditional_removal.h:54
pcl::ComparisonOps::LT
@ LT
Definition conditional_removal.h:54
pcl::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623
pcl::detail::FieldAdder
Definition conversions.h:64