Bowyer-Watson算法_飞翔——的的博客-CSDN博客
PCL三角化
/*
* 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.
*
* $Id$
*
*/
#ifndef PCL_GP3_H_
#define PCL_GP3_H_
// PCL includes
#include <pcl/surface/reconstruction.h>
#include <pcl/surface/boost.h>
#include <pcl/conversions.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/PolygonMesh.h>
#include <fstream>
#include <iostream>
namespace pcl
{
/** \brief Returns if a point X is visible from point R (or the origin)
* when taking into account the segment between the points S1 and S2
* \param X 2D coordinate of the point
* \param S1 2D coordinate of the segment's first point
* \param S2 2D coordinate of the segment's secont point
* \param R 2D coorddinate of the reference point (defaults to 0,0)
* \ingroup surface
*/
inline bool
isVisible (const Eigen::Vector2f &X, const Eigen::Vector2f &S1, const Eigen::Vector2f &S2,
const Eigen::Vector2f &R = Eigen::Vector2f::Zero ())
{
double a0 = S1[1] - S2[1];
double b0 = S2[0] - S1[0];
double c0 = S1[0]*S2[1] - S2[0]*S1[1];
double a1 = -X[1];
double b1 = X[0];
double c1 = 0;
if (R != Eigen::Vector2f::Zero())
{
a1 += R[1];
b1 -= R[0];
c1 = R[0]*X[1] - X[0]*R[1];
}
double div = a0*b1 - b0*a1;
double x = (b0*c1 - b1*c0) / div;
double y = (a1*c0 - a0*c1) / div;
bool intersection_outside_XR;
if (R == Eigen::Vector2f::Zero())
{
if (X[0] > 0)
intersection_outside_XR = (x <= 0) || (x >= X[0]);
else if (X[0] < 0)
intersection_outside_XR = (x >= 0) || (x <= X[0]);
else if (X[1] > 0)
intersection_outside_XR = (y <= 0) || (y >= X[1]);
else if (X[1] < 0)
intersection_outside_XR = (y >= 0) || (y <= X[1]);
else
intersection_outside_XR = true;
}
else
{
if (X[0] > R[0])
intersection_outside_XR = (x <= R[0]) || (x >= X[0]);
else if (X[0] < R[0])
intersection_outside_XR = (x >= R[0]) || (x <= X[0]);
else if (X[1] > R[1])
intersection_outside_XR = (y <= R[1]) || (y >= X[1]);
else if (X[1] < R[1])
intersection_outside_XR = (y >= R[1]) || (y <= X[1]);
else
intersection_outside_XR = true;
}
if (intersection_outside_XR)
return true;
else
{
if (S1[0] > S2[0])
return (x <= S2[0]) || (x >= S1[0]);
else if (S1[0] < S2[0])
return (x >= S2[0]) || (x <= S1[0]);
else if (S1[1] > S2[1])
return (y <= S2[1]) || (y >= S1[1]);
else if (S1[1] < S2[1])
return (y >= S2[1]) || (y <= S1[1]);
else
return false;
}
}
/** \brief GreedyProjectionTriangulation is an implementation of a greedy triangulation algorithm for 3D points
* based on local 2D projections. It assumes locally smooth surfaces and relatively smooth transitions between
* areas with different point densities.
* \author Zoltan Csaba Marton
* \ingroup surface
*/
template <typename PointInT>
class GreedyProjectionTriangulation : public MeshConstruction<PointInT>
{
public:
typedef boost::shared_ptr<GreedyProjectionTriangulation<PointInT> > Ptr;
typedef boost::shared_ptr<const GreedyProjectionTriangulation<PointInT> > ConstPtr;
using MeshConstruction<PointInT>::tree_;
using MeshConstruction<PointInT>::input_;
using MeshConstruction<PointInT>::indices_;
typedef typename pcl::KdTree<PointInT> KdTree;
typedef typename pcl::KdTree<PointInT>::Ptr KdTreePtr;
typedef pcl::PointCloud<PointInT> PointCloudIn;
typedef typename PointCloudIn::Ptr PointCloudInPtr;
typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
enum GP3Type
{
NONE = -1, // not-defined
FREE = 0,
FRINGE = 1,
BOUNDARY = 2,
COMPLETED = 3
};
/** \brief Empty constructor. */
GreedyProjectionTriangulation () :
mu_ (0),
search_radius_ (0), // must be set by user
nnn_ (100),
minimum_angle_ (M_PI/18), // 10 degrees
maximum_angle_ (2*M_PI/3), // 120 degrees
eps_angle_(M_PI/4), //45 degrees,
consistent_(false),
consistent_ordering_ (false),
triangle_ (),
coords_ (),
angles_ (),
R_ (),
state_ (),
source_ (),
ffn_ (),
sfn_ (),
part_ (),
fringe_queue_ (),
is_current_free_ (false),
current_index_ (),
prev_is_ffn_ (false),
prev_is_sfn_ (false),
next_is_ffn_ (false),
next_is_sfn_ (false),
changed_1st_fn_ (false),
changed_2nd_fn_ (false),
new2boundary_ (),
already_connected_ (false),
proj_qp_ (),
u_ (),
v_ (),
uvn_ffn_ (),
uvn_sfn_ (),
uvn_next_ffn_ (),
uvn_next_sfn_ (),
tmp_ ()
{};
/** \brief Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point
* (this will make the algorithm adapt to different point densities in the cloud).
* \param[in] mu the multiplier
*/
inline void
setMu (double mu) { mu_ = mu; }
/** \brief Get the nearest neighbor distance multiplier. */
inline double
getMu () const { return (mu_); }
/** \brief Set the maximum number of nearest neighbors to be searched for.
* \param[in] nnn the maximum number of nearest neighbors
*/
inline void
setMaximumNearestNeighbors (int nnn) { nnn_ = nnn; }
/** \brief Get the maximum number of nearest neighbors to be searched for. */
inline int
getMaximumNearestNeighbors () const { return (nnn_); }
/** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for triangulating.
* \param[in] radius the sphere radius that is to contain all k-nearest neighbors
* \note This distance limits the maximum edge length!
*/
inline void
setSearchRadius (double radius) { search_radius_ = radius; }
/** \brief Get the sphere radius used for determining the k-nearest neighbors. */
inline double
getSearchRadius () const { return (search_radius_); }
/** \brief Set the minimum angle each triangle should have.
* \param[in] minimum_angle the minimum angle each triangle should have
* \note As this is a greedy approach, this will have to be violated from time to time
*/
inline void
setMinimumAngle (double minimum_angle) { minimum_angle_ = minimum_angle; }
/** \brief Get the parameter for distance based weighting of neighbors. */
inline double
getMinimumAngle () const { return (minimum_angle_); }
/** \brief Set the maximum angle each triangle can have.
* \param[in] maximum_angle the maximum angle each triangle can have
* \note For best results, its value should be around 120 degrees
*/
inline void
setMaximumAngle (double maximum_angle) { maximum_angle_ = maximum_angle; }
/** \brief Get the parameter for distance based weighting of neighbors. */
inline double
getMaximumAngle () const { return (maximum_angle_); }
/** \brief Don't consider points for triangulation if their normal deviates more than this value from the query point's normal.
* \param[in] eps_angle maximum surface angle
* \note As normal estimation methods usually give smooth transitions at sharp edges, this ensures correct triangulation
* by avoiding connecting points from one side to points from the other through forcing the use of the edge points.
*/
inline void
setMaximumSurfaceAngle (double eps_angle) { eps_angle_ = eps_angle; }
/** \brief Get the maximum surface angle. */
inline double
getMaximumSurfaceAngle () const { return (eps_angle_); }
/** \brief Set the flag if the input normals are oriented consistently.
* \param[in] consistent set it to true if the normals are consistently oriented
*/
inline void
setNormalConsistency (bool consistent) { consistent_ = consistent; }
/** \brief Get the flag for consistently oriented normals. */
inline bool
getNormalConsistency () const { return (consistent_); }
/** \brief Set the flag to order the resulting triangle vertices consistently (positive direction around normal).
* @note Assumes consistently oriented normals (towards the viewpoint) -- see setNormalConsistency ()
* \param[in] consistent_ordering set it to true if triangle vertices should be ordered consistently
*/
inline void
setConsistentVertexOrdering (bool consistent_ordering) { consistent_ordering_ = consistent_ordering; }
/** \brief Get the flag signaling consistently ordered triangle vertices. */
inline bool
getConsistentVertexOrdering () const { return (consistent_ordering_); }
/** \brief Get the state of each point after reconstruction.
* \note Options are defined as constants: FREE, FRINGE, COMPLETED, BOUNDARY and NONE
*/
inline std::vector<int>
getPointStates () const { return (state_); }
/** \brief Get the ID of each point after reconstruction.
* \note parts are numbered from 0, a -1 denotes unconnected points
*/
inline std::vector<int>
getPartIDs () const { return (part_); }
/** \brief Get the sfn list. */
inline std::vector<int>
getSFN () const { return (sfn_); }
/** \brief Get the ffn list. */
inline std::vector<int>
getFFN () const { return (ffn_); }
protected:
/** \brief The nearest neighbor distance multiplier to obtain the final search radius. */
double mu_;
/** \brief The nearest neighbors search radius for each point and the maximum edge length. */
double search_radius_;
/** \brief The maximum number of nearest neighbors accepted by searching. */
int nnn_;
/** \brief The preferred minimum angle for the triangles. */
double minimum_angle_;
/** \brief The maximum angle for the triangles. */
double maximum_angle_;
/** \brief Maximum surface angle. */
double eps_angle_;
/** \brief Set this to true if the normals of the input are consistently oriented. */
bool consistent_;
/** \brief Set this to true if the output triangle vertices should be consistently oriented. */
bool consistent_ordering_;
private:
/** \brief Struct for storing the angles to nearest neighbors **/
struct nnAngle
{
double angle;
int index;
int nnIndex;
bool visible;
};
/** \brief Struct for storing the edges starting from a fringe point **/
struct doubleEdge
{
doubleEdge () : index (0), first (), second () {}
int index;
Eigen::Vector2f first;
Eigen::Vector2f second;
};
// Variables made global to decrease the number of parameters to helper functions
/** \brief Temporary variable to store a triangle (as a set of point indices) **/
pcl::Vertices triangle_;
/** \brief Temporary variable to store point coordinates **/
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > coords_;
/** \brief A list of angles to neighbors **/
std::vector<nnAngle> angles_;
/** \brief Index of the current query point **/
int R_;
/** \brief List of point states **/
std::vector<int> state_;
/** \brief List of sources **/
std::vector<int> source_;
/** \brief List of fringe neighbors in one direction **/
std::vector<int> ffn_;
/** \brief List of fringe neighbors in other direction **/
std::vector<int> sfn_;
/** \brief Connected component labels for each point **/
std::vector<int> part_;
/** \brief Points on the outer edge from which the mesh has to be grown **/
std::vector<int> fringe_queue_;
/** \brief Flag to set if the current point is free **/
bool is_current_free_;
/** \brief Current point's index **/
int current_index_;
/** \brief Flag to set if the previous point is the first fringe neighbor **/
bool prev_is_ffn_;
/** \brief Flag to set if the next point is the second fringe neighbor **/
bool prev_is_sfn_;
/** \brief Flag to set if the next point is the first fringe neighbor **/
bool next_is_ffn_;
/** \brief Flag to set if the next point is the second fringe neighbor **/
bool next_is_sfn_;
/** \brief Flag to set if the first fringe neighbor was changed **/
bool changed_1st_fn_;
/** \brief Flag to set if the second fringe neighbor was changed **/
bool changed_2nd_fn_;
/** \brief New boundary point **/
int new2boundary_;
/** \brief Flag to set if the next neighbor was already connected in the previous step.
* To avoid inconsistency it should not be connected again.
*/
bool already_connected_;
/** \brief Point coordinates projected onto the plane defined by the point normal **/
Eigen::Vector3f proj_qp_;
/** \brief First coordinate vector of the 2D coordinate frame **/
Eigen::Vector3f u_;
/** \brief Second coordinate vector of the 2D coordinate frame **/
Eigen::Vector3f v_;
/** \brief 2D coordinates of the first fringe neighbor **/
Eigen::Vector2f uvn_ffn_;
/** \brief 2D coordinates of the second fringe neighbor **/
Eigen::Vector2f uvn_sfn_;
/** \brief 2D coordinates of the first fringe neighbor of the next point **/
Eigen::Vector2f uvn_next_ffn_;
/** \brief 2D coordinates of the second fringe neighbor of the next point **/
Eigen::Vector2f uvn_next_sfn_;
/** \brief Temporary variable to store 3 coordiantes **/
Eigen::Vector3f tmp_;
/** \brief The actual surface reconstruction method.
* \param[out] output the resultant polygonal mesh
*/
void
performReconstruction (pcl::PolygonMesh &output);
/** \brief The actual surface reconstruction method.
* \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);
/** \brief The actual surface reconstruction method.
* \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
*/
bool
reconstructPolygons (std::vector<pcl::Vertices> &polygons);
/** \brief Class get name method. */
std::string
getClassName () const { return ("GreedyProjectionTriangulation"); }
/** \brief Forms a new triangle by connecting the current neighbor to the query point
* and the previous neighbor
* \param[out] polygons the polygon mesh to be updated
* \param[in] prev_index index of the previous point
* \param[in] next_index index of the next point
* \param[in] next_next_index index of the point after the next one
* \param[in] uvn_current 2D coordinate of the current point
* \param[in] uvn_prev 2D coordinates of the previous point
* \param[in] uvn_next 2D coordinates of the next point
*/
void
connectPoint (std::vector<pcl::Vertices> &polygons,
const int prev_index,
const int next_index,
const int next_next_index,
const Eigen::Vector2f &uvn_current,
const Eigen::Vector2f &uvn_prev,
const Eigen::Vector2f &uvn_next);
/** \brief Whenever a query point is part of a boundary loop containing 3 points, that triangle is created
* (called if angle constraints make it possible)
* \param[out] polygons the polygon mesh to be updated
*/
void
closeTriangle (std::vector<pcl::Vertices> &polygons);
/** \brief Get the list of containing triangles for each vertex in a PolygonMesh
* \param[in] polygonMesh the input polygon mesh
*/
std::vector<std::vector<size_t> >
getTriangleList (const pcl::PolygonMesh &input);
/** \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[out] polygons the polygon mesh to be updated
*/
inline void
addTriangle (int a, int b, int c, std::vector<pcl::Vertices> &polygons)
{
triangle_.vertices.resize (3);
if (consistent_ordering_)
{
const PointInT p = input_->at (indices_->at (a));
const Eigen::Vector3f pv = p.getVector3fMap ();
if (p.getNormalVector3fMap ().dot (
(pv - input_->at (indices_->at (b)).getVector3fMap ()).cross (
pv - input_->at (indices_->at (c)).getVector3fMap ()) ) > 0)
{
triangle_.vertices[0] = a;
triangle_.vertices[1] = b;
triangle_.vertices[2] = c;
}
else
{
triangle_.vertices[0] = a;
triangle_.vertices[1] = c;
triangle_.vertices[2] = b;
}
}
else
{
triangle_.vertices[0] = a;
triangle_.vertices[1] = b;
triangle_.vertices[2] = c;
}
polygons.push_back (triangle_);
}
/** \brief Add a new vertex to the advancing edge front and set its source point
* \param[in] v index of the vertex that was connected
* \param[in] s index of the source point
*/
inline void
addFringePoint (int v, int s)
{
source_[v] = s;
part_[v] = part_[s];
fringe_queue_.push_back(v);
}
/** \brief Function for ascending sort of nnAngle, taking visibility into account
* (angles to visible neighbors will be first, to the invisible ones after).
* \param[in] a1 the first angle
* \param[in] a2 the second angle
*/
static inline bool
nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2)
{
if (a1.visible == a2.visible)
return (a1.angle < a2.angle);
else
return a1.visible;
}
};
} // namespace pcl
#ifdef PCL_NO_PRECOMPILE
#include <pcl/surface/impl/gp3.hpp>
#endif
#endif //#ifndef PCL_GP3_H_