i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.11.2: First Google Tango release

Browse Source
This commit is contained in:
matlabbe
2016-02-15 19:35:24 -05:00
parent 8dc01c491b
commit c6a46a7238
415 changed files with 73300 additions and 3193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/pcl18/readme.txt Normal file
View File

@@ -0,0 +1,2 @@
Copied pcl 1.8 functions to be used in older pcl versions. They are
not included if 1.8 is detected (the originals are used directly).

284
corelib/src/pcl18/surface/impl/organized_fast_mesh.hpp Normal file
View File

@@ -0,0 +1,284 @@
/*
* 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$
*
*/
#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
#define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
#include <pcl18/surface/organized_fast_mesh.h>
#include <rtabmap/utilite/ULogger.h>
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output)
{
reconstructPolygons (output.polygons);
// Get the field names
int x_idx = pcl::getFieldIndex (output.cloud, "x");
int y_idx = pcl::getFieldIndex (output.cloud, "y");
int z_idx = pcl::getFieldIndex (output.cloud, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
return;
// correct all measurements,
// (running over complete image since some rows and columns are left out
// depending on triangle_pixel_size)
// avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
for (unsigned int i = 0; i < input_->points.size (); ++i)
if (!isFinite (input_->points[i]))
resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
{
reconstructPolygons (polygons);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
{
if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
makeRightCutMesh (polygons);
else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
makeLeftCutMesh (polygons);
else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
makeAdaptiveCutMesh (polygons);
else if (triangulation_type_ == QUAD_MESH)
makeQuadMesh (polygons);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
if (isValidQuad (i, index_right, index_down_right, index_down))
if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
addQuad (i, index_right, index_down_right, index_down, idx++, polygons);
}
}
polygons.resize (idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height * 2);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
if (isValidTriangle (i, index_down_right, index_right))
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
addTriangle (i, index_down_right, index_right, idx++, polygons);
if (isValidTriangle (i, index_down, index_down_right))
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
addTriangle (i, index_down, index_down_right, idx++, polygons);
}
}
polygons.resize (idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height * 2);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
if (isValidTriangle (i, index_down, index_right))
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
addTriangle (i, index_down, index_right, idx++, polygons);
if (isValidTriangle (index_right, index_down, index_down_right))
if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
addTriangle (index_right, index_down, index_down_right, idx++, polygons);
}
}
polygons.resize (idx);
}
/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
{
int last_column = input_->width - triangle_pixel_size_columns_;
int last_row = input_->height - triangle_pixel_size_rows_;
int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
int y_big_incr = triangle_pixel_size_rows_ * input_->width,
x_big_incr = y_big_incr + triangle_pixel_size_columns_;
// Reserve enough space
polygons.resize (input_->width * input_->height * 2);
// Go over the rows first
for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
{
// Initialize a new row
i = y * input_->width;
index_right = i + triangle_pixel_size_columns_;
index_down = i + y_big_incr;
index_down_right = i + x_big_incr;
// Go over the columns
for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
i += triangle_pixel_size_columns_,
index_right += triangle_pixel_size_columns_,
index_down += triangle_pixel_size_columns_,
index_down_right += triangle_pixel_size_columns_)
{
const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
{
float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z);
float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z);
if (dist_right_cut >= dist_left_cut)
{
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
addTriangle (i, index_down_right, index_right, idx++, polygons);
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
addTriangle (i, index_down, index_down_right, idx++, polygons);
}
else
{
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
addTriangle (i, index_down, index_right, idx++, polygons);
if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
addTriangle (index_right, index_down, index_down_right, idx++, polygons);
}
}
else
{
if (right_cut_upper)
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
addTriangle (i, index_down_right, index_right, idx++, polygons);
if (right_cut_lower)
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
addTriangle (i, index_down, index_down_right, idx++, polygons);
if (left_cut_upper)
if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
addTriangle (i, index_down, index_right, idx++, polygons);
if (left_cut_lower)
if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
addTriangle (index_right, index_down, index_down_right, idx++, polygons);
}
}
}
polygons.resize (idx);
}
#define PCL_INSTANTIATE_OrganizedFastMesh(T) \
template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
#endif // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_

490
corelib/src/pcl18/surface/organized_fast_mesh.h Normal file
View File

@@ -0,0 +1,490 @@
/*
* 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$
*
*/
#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_H_
#define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
#include <pcl/common/angles.h>
#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 pcl::MeshConstruction<PointInT>
{
public:
typedef boost::shared_ptr<OrganizedFastMesh<PointInT> > Ptr;
typedef boost::shared_ptr<const OrganizedFastMesh<PointInT> > ConstPtr;
using pcl::MeshConstruction<PointInT>::input_;
using pcl::MeshConstruction<PointInT>::check_tree_;
typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
typedef std::vector<pcl::Vertices> Polygons;
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_ (fabsf (cosf (pcl::deg2rad (12.5f))))
, distance_tolerance_ (-1.0f)
, distance_dependent_ (false)
, use_depth_as_distance_(false)
{
check_tree_ = false;
};
/** \brief Destructor. */
virtual ~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_ = fabsf (cosf (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.
*/
virtual void
performReconstruction (std::vector<pcl::Vertices> &polygons);
/** \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);
/** \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 (cos_angle != cos_angle)
cos_angle = 1.0f;
bool check_angle = 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 (fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
dist_thresh += max_edge_length_b_ * dist;
if (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_->points[a])) return (false);
if (!pcl::isFinite (input_->points[b])) return (false);
if (!pcl::isFinite (input_->points[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_->points[a], input_->points[b])) return (true);
if (isShadowed (input_->points[b], input_->points[c])) return (true);
if (isShadowed (input_->points[c], input_->points[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_->points[a])) return (false);
if (!pcl::isFinite (input_->points[b])) return (false);
if (!pcl::isFinite (input_->points[c])) return (false);
if (!pcl::isFinite (input_->points[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_->points[a], input_->points[b])) return (true);
if (isShadowed (input_->points[b], input_->points[c])) return (true);
if (isShadowed (input_->points[c], input_->points[d])) return (true);
if (isShadowed (input_->points[d], input_->points[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);
};
}
#include <pcl18/surface/impl/organized_fast_mesh.hpp>
#endif // PCL_SURFACE_ORGANIZED_FAST_MESH_H_