world_model.h
/*
* 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)
*/
#pragma once
#include <pcl/common/impl/common.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
//#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
//#include <boost/graph/buffer_concepts.hpp>
namespace pcl
{
namespace kinfuLS
{
/** \brief WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.\n
* The world is represented as a point cloud.\n
* When new points are added to the world, we replace old ones by the newest ones.
* This is achieved by setting old points to nan (for speed)
* \author Raphael Favier
*/
template <typename PointT>
class WorldModel
{
public:
using Ptr = shared_ptr<WorldModel<PointT> >;
using ConstPtr = shared_ptr<const WorldModel<PointT> >;
using PointCloud = pcl::PointCloud<PointT>;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
using ConditionAndPtr = typename pcl::ConditionAnd<PointT>::Ptr;
using ConditionOrPtr = typename pcl::ConditionOr<PointT>::Ptr;
using FieldComparisonConstPtr = typename pcl::FieldComparison<PointT>::ConstPtr;
using FieldList = typename pcl::traits::fieldList<PointT>::type;
/** \brief Default constructor for the WorldModel.
*/
WorldModel() :
world_ (new PointCloud)
{
world_->is_dense = false;
}
/** \brief Clear the world.
*/
void reset()
{
if(!world_->points.empty ())
{
PCL_WARN("Clearing world model\n");
world_->points.clear ();
}
}
/** \brief Append a new point cloud (slice) to the world.
* \param[in] new_cloud the point cloud to add to the world
*/
void addSlice (const PointCloudPtr new_cloud);
/** \brief Retrieve existing data from the world model, after a shift
* \param[in] previous_origin_x global origin of the cube on X axis, before the shift
* \param[in] previous_origin_y global origin of the cube on Y axis, before the shift
* \param[in] previous_origin_z global origin of the cube on Z axis, before the shift
* \param[in] offset_x shift on X, in indices
* \param[in] offset_y shift on Y, in indices
* \param[in] offset_z shift on Z, in indices
* \param[in] volume_x size of the cube, X axis, in indices
* \param[in] volume_y size of the cube, Y axis, in indices
* \param[in] volume_z size of the cube, Z axis, in indices
* \param[out] existing_slice the extracted point cloud representing the slice
*/
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);
/** \brief Give nan values to the slice of the world
* \param[in] origin_x global origin of the cube on X axis, before the shift
* \param[in] origin_y global origin of the cube on Y axis, before the shift
* \param[in] origin_z global origin of the cube on Z axis, before the shift
* \param[in] offset_x shift on X, in indices
* \param[in] offset_y shift on Y, in indices
* \param[in] offset_z shift on Z, in indices
* \param[in] size_x size of the cube, X axis, in indices
* \param[in] size_y size of the cube, Y axis, in indices
* \param[in] size_z size of the cube, Z axis, in indices
*/
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);
/** \brief Remove points with nan values from the world.
*/
void cleanWorldFromNans ()
{
world_->is_dense = false;
pcl::Indices indices;
pcl::removeNaNFromPointCloud (*world_, *world_, indices);
}
/** \brief Returns the world as a point cloud.
*/
PointCloudPtr getWorld ()
{
return (world_);
}
/** \brief Returns the number of points contained in the world.
*/
std::size_t getWorldSize ()
{
return (world_->size () );
}
/** \brief Returns the world as two vectors of cubes of size "size" (pointclouds) and transforms
* \param[in] size the size of a 3D cube.
* \param[out] cubes a vector of point clouds representing each cube (in their original world coordinates).
* \param[out] transforms a vector containing the xyz position of each cube in world coordinates.
* \param[in] overlap optional overlap (in percent) between each cube (useful to create overlapped meshes).
*/
void getWorldAsCubes (double size, std::vector<PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap = 0.0);
private:
/** \brief cloud containing our world */
PointCloudPtr world_;
/** \brief set the points which index is in the indices vector to nan
* \param[in] cloud the cloud that contains the point to be set to nan
* \param[in] indices the vector of indices to set to nan
*/
inline void setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices);
};
}
}
pcl::ConditionAnd::Ptr
shared_ptr< ConditionAnd< PointT > > Ptr
Definition: conditional_removal.h:509
pcl::ConditionOr::Ptr
shared_ptr< ConditionOr< PointT > > Ptr
Definition: conditional_removal.h:537
pcl::FieldComparison::ConstPtr
shared_ptr< const FieldComparison< PointT > > ConstPtr
Definition: conditional_removal.h:137
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
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::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: world_model.h:71
pcl::kinfuLS::WorldModel::Ptr
shared_ptr< WorldModel< PointT > > Ptr
Definition: world_model.h:66
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::getWorldSize
std::size_t getWorldSize()
Returns the number of points contained in the world.
Definition: world_model.h:153
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::cleanWorldFromNans
void cleanWorldFromNans()
Remove points with nan values from the world.
Definition: world_model.h:137
pcl::kinfuLS::WorldModel::ConditionAndPtr
typename pcl::ConditionAnd< PointT >::Ptr ConditionAndPtr
Definition: world_model.h:73
pcl::kinfuLS::WorldModel::reset
void reset()
Clear the world.
Definition: world_model.h:89
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::WorldModel
WorldModel()
Default constructor for the WorldModel.
Definition: world_model.h:81
pcl::kinfuLS::WorldModel::FieldList
typename pcl::traits::fieldList< PointT >::type FieldList
Definition: world_model.h:77
pcl::kinfuLS::WorldModel::ConditionOrPtr
typename pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
Definition: world_model.h:74
pcl::kinfuLS::WorldModel::getWorld
PointCloudPtr getWorld()
Returns the world as a point cloud.
Definition: world_model.h:146
pcl::kinfuLS::WorldModel::ConstPtr
shared_ptr< const WorldModel< PointT > > ConstPtr
Definition: world_model.h:67
point_types.h
Defines all the PCL implemented PointT point type structures.
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
Definition: convolution.h:46
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