492 lines
19 KiB
C++
492 lines
19 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2011, Dirk Holz, University of Bonn.
|
|
* 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.
|
|
*
|
|
* $Id$
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/common/angles.h>
|
|
#include <pcl/common/point_tests.h> // for pcl::isFinite
|
|
#include <pcl/surface/reconstruction.h>
|
|
|
|
|
|
namespace pcl
|
|
{
|
|
|
|
/** \brief Simple triangulation/surface reconstruction for organized point
|
|
* clouds. Neighboring points (pixels in image space) are connected to
|
|
* construct a triangular (or quad) mesh.
|
|
*
|
|
* \note If you use this code in any academic work, please cite:
|
|
* D. Holz and S. Behnke.
|
|
* Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing.
|
|
* In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
|
|
* Jeju Island, Korea, June 26-29 2012.
|
|
* <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
|
|
*
|
|
* \author Dirk Holz, Radu B. Rusu
|
|
* \ingroup surface
|
|
*/
|
|
template <typename PointInT>
|
|
class OrganizedFastMesh : public MeshConstruction<PointInT>
|
|
{
|
|
public:
|
|
using Ptr = shared_ptr<OrganizedFastMesh<PointInT> >;
|
|
using ConstPtr = shared_ptr<const OrganizedFastMesh<PointInT> >;
|
|
|
|
using MeshConstruction<PointInT>::input_;
|
|
using MeshConstruction<PointInT>::check_tree_;
|
|
|
|
using PointCloudPtr = typename pcl::PointCloud<PointInT>::Ptr;
|
|
|
|
using Polygons = std::vector<pcl::Vertices>;
|
|
|
|
enum TriangulationType
|
|
{
|
|
TRIANGLE_RIGHT_CUT, // _always_ "cuts" a quad from top left to bottom right
|
|
TRIANGLE_LEFT_CUT, // _always_ "cuts" a quad from top right to bottom left
|
|
TRIANGLE_ADAPTIVE_CUT, // "cuts" where possible and prefers larger differences in 'z' direction
|
|
QUAD_MESH // create a simple quad mesh
|
|
};
|
|
|
|
/** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
|
|
OrganizedFastMesh ()
|
|
: max_edge_length_a_ (0.0f)
|
|
, max_edge_length_b_ (0.0f)
|
|
, max_edge_length_c_ (0.0f)
|
|
, max_edge_length_set_ (false)
|
|
, max_edge_length_dist_dependent_ (false)
|
|
, triangle_pixel_size_rows_ (1)
|
|
, triangle_pixel_size_columns_ (1)
|
|
, triangulation_type_ (QUAD_MESH)
|
|
, viewpoint_ (Eigen::Vector3f::Zero ())
|
|
, store_shadowed_faces_ (false)
|
|
, cos_angle_tolerance_ (std::abs (std::cos (pcl::deg2rad (12.5f))))
|
|
, distance_tolerance_ (-1.0f)
|
|
, distance_dependent_ (false)
|
|
, use_depth_as_distance_(false)
|
|
{
|
|
check_tree_ = false;
|
|
};
|
|
|
|
/** \brief Destructor. */
|
|
~OrganizedFastMesh () {};
|
|
|
|
/** \brief Set a maximum edge length.
|
|
* Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of:
|
|
* threshold(x) = c*x*x + b*x + a
|
|
* \param[in] a scalar coefficient of the (distance-dependent polynom) threshold
|
|
* \param[in] b linear coefficient of the (distance-dependent polynom) threshold
|
|
* \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold
|
|
*/
|
|
inline void
|
|
setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
|
|
{
|
|
max_edge_length_a_ = a;
|
|
max_edge_length_b_ = b;
|
|
max_edge_length_c_ = c;
|
|
if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min())
|
|
max_edge_length_set_ = true;
|
|
else
|
|
max_edge_length_set_ = false;
|
|
};
|
|
|
|
inline void
|
|
unsetMaxEdgeLength ()
|
|
{
|
|
max_edge_length_set_ = false;
|
|
}
|
|
|
|
/** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
|
|
* \param[in] triangle_size edge length in pixels
|
|
* (Default: 1 = neighboring pixels are connected)
|
|
*/
|
|
inline void
|
|
setTrianglePixelSize (int triangle_size)
|
|
{
|
|
setTrianglePixelSizeRows (triangle_size);
|
|
setTrianglePixelSizeColumns (triangle_size);
|
|
}
|
|
|
|
/** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
|
|
* \param[in] triangle_size edge length in pixels
|
|
* (Default: 1 = neighboring pixels are connected)
|
|
*/
|
|
inline void
|
|
setTrianglePixelSizeRows (int triangle_size)
|
|
{
|
|
triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
|
|
}
|
|
|
|
/** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
|
|
* \param[in] triangle_size edge length in pixels
|
|
* (Default: 1 = neighboring pixels are connected)
|
|
*/
|
|
inline void
|
|
setTrianglePixelSizeColumns (int triangle_size)
|
|
{
|
|
triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
|
|
}
|
|
|
|
/** \brief Set the triangulation type (see \a TriangulationType)
|
|
* \param[in] type quad mesh, triangle mesh with fixed left, right cut,
|
|
* or adaptive cut (splits a quad w.r.t. the depth (z) of the points)
|
|
*/
|
|
inline void
|
|
setTriangulationType (TriangulationType type)
|
|
{
|
|
triangulation_type_ = type;
|
|
}
|
|
|
|
/** \brief Set the viewpoint from where the input point cloud has been acquired.
|
|
* \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data)
|
|
*/
|
|
inline void setViewpoint (const Eigen::Vector3f& viewpoint)
|
|
{
|
|
viewpoint_ = viewpoint;
|
|
}
|
|
|
|
/** \brief Get the viewpoint from where the input point cloud has been acquired. */
|
|
const inline Eigen::Vector3f& getViewpoint () const
|
|
{
|
|
return viewpoint_;
|
|
}
|
|
|
|
/** \brief Store shadowed faces or not.
|
|
* \param[in] enable set to true to store shadowed faces
|
|
*/
|
|
inline void
|
|
storeShadowedFaces (bool enable)
|
|
{
|
|
store_shadowed_faces_ = enable;
|
|
}
|
|
|
|
/** \brief Set the angle tolerance used for checking whether or not an edge is occluded.
|
|
* Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to
|
|
* disable the check for shadowed edges.
|
|
* \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable.
|
|
*/
|
|
inline void
|
|
setAngleTolerance(float angle_tolerance)
|
|
{
|
|
if (angle_tolerance > 0)
|
|
cos_angle_tolerance_ = std::abs (std::cos (angle_tolerance));
|
|
else
|
|
cos_angle_tolerance_ = -1.0f;
|
|
}
|
|
|
|
|
|
inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
|
|
{
|
|
distance_tolerance_ = distance_tolerance;
|
|
if (distance_tolerance_ < 0)
|
|
return;
|
|
|
|
distance_dependent_ = depth_dependent;
|
|
if (!distance_dependent_)
|
|
distance_tolerance_ *= distance_tolerance_;
|
|
}
|
|
|
|
/** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint).
|
|
* \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */
|
|
inline void useDepthAsDistance(bool enable)
|
|
{
|
|
use_depth_as_distance_ = enable;
|
|
}
|
|
|
|
protected:
|
|
/** \brief max length of edge, scalar component */
|
|
float max_edge_length_a_;
|
|
/** \brief max length of edge, scalar component */
|
|
float max_edge_length_b_;
|
|
/** \brief max length of edge, scalar component */
|
|
float max_edge_length_c_;
|
|
/** \brief flag whether or not edges are limited in length */
|
|
bool max_edge_length_set_;
|
|
|
|
/** \brief flag whether or not max edge length is distance dependent. */
|
|
bool max_edge_length_dist_dependent_;
|
|
|
|
/** \brief size of triangle edges (in pixels) for iterating over rows. */
|
|
int triangle_pixel_size_rows_;
|
|
|
|
/** \brief size of triangle edges (in pixels) for iterating over columns*/
|
|
int triangle_pixel_size_columns_;
|
|
|
|
/** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
|
|
TriangulationType triangulation_type_;
|
|
|
|
/** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */
|
|
Eigen::Vector3f viewpoint_;
|
|
|
|
/** \brief Whether or not shadowed faces are stored, e.g., for exploration */
|
|
bool store_shadowed_faces_;
|
|
|
|
/** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
|
|
float cos_angle_tolerance_;
|
|
|
|
/** \brief distance tolerance for filtering out shadowed/occluded edges */
|
|
float distance_tolerance_;
|
|
|
|
/** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */
|
|
bool distance_dependent_;
|
|
|
|
/** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint).
|
|
This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */
|
|
bool use_depth_as_distance_;
|
|
|
|
|
|
/** \brief Perform the actual polygonal reconstruction.
|
|
* \param[out] polygons the resultant polygons
|
|
*/
|
|
void
|
|
reconstructPolygons (std::vector<pcl::Vertices>& polygons);
|
|
|
|
/** \brief Create the surface.
|
|
* \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
|
|
*/
|
|
void
|
|
performReconstruction (std::vector<pcl::Vertices> &polygons) override;
|
|
|
|
/** \brief Create the surface.
|
|
*
|
|
* Simply uses image indices to create an initial polygonal mesh for organized point clouds.
|
|
* \a indices_ are ignored!
|
|
*
|
|
* \param[out] output the resultant polygonal mesh
|
|
*/
|
|
void
|
|
performReconstruction (pcl::PolygonMesh &output) override;
|
|
|
|
/** \brief Add a new triangle to the current polygon mesh
|
|
* \param[in] a index of the first vertex
|
|
* \param[in] b index of the second vertex
|
|
* \param[in] c index of the third vertex
|
|
* \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
|
|
* \param[out] polygons the polygon mesh to be updated
|
|
*/
|
|
inline void
|
|
addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
|
|
{
|
|
assert (idx < static_cast<int> (polygons.size ()));
|
|
polygons[idx].vertices.resize (3);
|
|
polygons[idx].vertices[0] = a;
|
|
polygons[idx].vertices[1] = b;
|
|
polygons[idx].vertices[2] = c;
|
|
}
|
|
|
|
/** \brief Add a new quad to the current polygon mesh
|
|
* \param[in] a index of the first vertex
|
|
* \param[in] b index of the second vertex
|
|
* \param[in] c index of the third vertex
|
|
* \param[in] d index of the fourth vertex
|
|
* \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
|
|
* \param[out] polygons the polygon mesh to be updated
|
|
*/
|
|
inline void
|
|
addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
|
|
{
|
|
assert (idx < static_cast<int> (polygons.size ()));
|
|
polygons[idx].vertices.resize (4);
|
|
polygons[idx].vertices[0] = a;
|
|
polygons[idx].vertices[1] = b;
|
|
polygons[idx].vertices[2] = c;
|
|
polygons[idx].vertices[3] = d;
|
|
}
|
|
|
|
/** \brief Set (all) coordinates of a particular point to the specified value
|
|
* \param[in] point_index index of point
|
|
* \param[out] mesh to modify
|
|
* \param[in] value value to use when re-setting
|
|
* \param[in] field_x_idx the X coordinate of the point
|
|
* \param[in] field_y_idx the Y coordinate of the point
|
|
* \param[in] field_z_idx the Z coordinate of the point
|
|
*/
|
|
inline void
|
|
resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
|
|
int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
|
|
{
|
|
float new_value = value;
|
|
memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
|
|
memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
|
|
memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
|
|
}
|
|
|
|
/** \brief Check if a point is shadowed by another point
|
|
* \param[in] point_a the first point
|
|
* \param[in] point_b the second point
|
|
*/
|
|
inline bool
|
|
isShadowed (const PointInT& point_a, const PointInT& point_b)
|
|
{
|
|
bool valid = true;
|
|
|
|
Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
|
|
Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
|
|
float distance_to_points = dir_a.norm ();
|
|
float distance_between_points = dir_b.norm ();
|
|
|
|
if (cos_angle_tolerance_ > 0)
|
|
{
|
|
float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
|
|
if (std::isnan(cos_angle))
|
|
cos_angle = 1.0f;
|
|
bool check_angle = std::fabs (cos_angle) >= cos_angle_tolerance_;
|
|
|
|
bool check_distance = true;
|
|
if (check_angle && (distance_tolerance_ > 0))
|
|
{
|
|
float dist_thresh = distance_tolerance_;
|
|
if (distance_dependent_)
|
|
{
|
|
float d = distance_to_points;
|
|
if (use_depth_as_distance_)
|
|
d = std::max(point_a.z, point_b.z);
|
|
dist_thresh *= d*d;
|
|
dist_thresh *= dist_thresh; // distance_tolerance_ is already squared if distance_dependent_ is false.
|
|
}
|
|
check_distance = (distance_between_points > dist_thresh);
|
|
}
|
|
valid = !(check_angle && check_distance);
|
|
}
|
|
|
|
// check if max. edge length is not exceeded
|
|
if (max_edge_length_set_)
|
|
{
|
|
float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
|
|
float dist_thresh = max_edge_length_a_;
|
|
if (std::fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
|
|
dist_thresh += max_edge_length_b_ * dist;
|
|
if (std::fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
|
|
dist_thresh += max_edge_length_c_ * dist * dist;
|
|
valid = (distance_between_points <= dist_thresh);
|
|
}
|
|
|
|
return !valid;
|
|
}
|
|
|
|
/** \brief Check if a triangle is valid.
|
|
* \param[in] a index of the first vertex
|
|
* \param[in] b index of the second vertex
|
|
* \param[in] c index of the third vertex
|
|
*/
|
|
inline bool
|
|
isValidTriangle (const int& a, const int& b, const int& c)
|
|
{
|
|
if (!pcl::isFinite ((*input_)[a])) return (false);
|
|
if (!pcl::isFinite ((*input_)[b])) return (false);
|
|
if (!pcl::isFinite ((*input_)[c])) return (false);
|
|
return (true);
|
|
}
|
|
|
|
/** \brief Check if a triangle is shadowed.
|
|
* \param[in] a index of the first vertex
|
|
* \param[in] b index of the second vertex
|
|
* \param[in] c index of the third vertex
|
|
*/
|
|
inline bool
|
|
isShadowedTriangle (const int& a, const int& b, const int& c)
|
|
{
|
|
if (isShadowed ((*input_)[a], (*input_)[b])) return (true);
|
|
if (isShadowed ((*input_)[b], (*input_)[c])) return (true);
|
|
if (isShadowed ((*input_)[c], (*input_)[a])) return (true);
|
|
return (false);
|
|
}
|
|
|
|
/** \brief Check if a quad is valid.
|
|
* \param[in] a index of the first vertex
|
|
* \param[in] b index of the second vertex
|
|
* \param[in] c index of the third vertex
|
|
* \param[in] d index of the fourth vertex
|
|
*/
|
|
inline bool
|
|
isValidQuad (const int& a, const int& b, const int& c, const int& d)
|
|
{
|
|
if (!pcl::isFinite ((*input_)[a])) return (false);
|
|
if (!pcl::isFinite ((*input_)[b])) return (false);
|
|
if (!pcl::isFinite ((*input_)[c])) return (false);
|
|
if (!pcl::isFinite ((*input_)[d])) return (false);
|
|
return (true);
|
|
}
|
|
|
|
/** \brief Check if a triangle is shadowed.
|
|
* \param[in] a index of the first vertex
|
|
* \param[in] b index of the second vertex
|
|
* \param[in] c index of the third vertex
|
|
* \param[in] d index of the fourth vertex
|
|
*/
|
|
inline bool
|
|
isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
|
|
{
|
|
if (isShadowed ((*input_)[a], (*input_)[b])) return (true);
|
|
if (isShadowed ((*input_)[b], (*input_)[c])) return (true);
|
|
if (isShadowed ((*input_)[c], (*input_)[d])) return (true);
|
|
if (isShadowed ((*input_)[d], (*input_)[a])) return (true);
|
|
return (false);
|
|
}
|
|
|
|
/** \brief Create a quad mesh.
|
|
* \param[out] polygons the resultant mesh
|
|
*/
|
|
void
|
|
makeQuadMesh (std::vector<pcl::Vertices>& polygons);
|
|
|
|
/** \brief Create a right cut mesh.
|
|
* \param[out] polygons the resultant mesh
|
|
*/
|
|
void
|
|
makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
|
|
|
|
/** \brief Create a left cut mesh.
|
|
* \param[out] polygons the resultant mesh
|
|
*/
|
|
void
|
|
makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
|
|
|
|
/** \brief Create an adaptive cut mesh.
|
|
* \param[out] polygons the resultant mesh
|
|
*/
|
|
void
|
|
makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
|
|
};
|
|
}
|
|
|
|
#ifdef PCL_NO_PRECOMPILE
|
|
#include <pcl/surface/impl/organized_fast_mesh.hpp>
|
|
#endif
|