1677 lines
60 KiB
C++
1677 lines
60 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2010, 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_IMPL_GP3_H_
|
|
#define PCL_SURFACE_IMPL_GP3_H_
|
|
|
|
#include <pcl/surface/gp3.h>
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (pcl::PolygonMesh &output)
|
|
{
|
|
output.polygons.clear ();
|
|
output.polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
|
|
if (!reconstructPolygons (output.polygons))
|
|
{
|
|
PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
|
|
output.cloud.width = output.cloud.height = 0;
|
|
output.cloud.data.clear ();
|
|
return;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
|
|
{
|
|
polygons.clear ();
|
|
polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
|
|
if (!reconstructPolygons (polygons))
|
|
{
|
|
PCL_ERROR ("[pcl::%s::performReconstruction] Reconstruction failed. Check parameters: search radius (%f) or mu (%f) before continuing.\n", getClassName ().c_str (), search_radius_, mu_);
|
|
return;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> bool
|
|
pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
|
|
{
|
|
if (search_radius_ <= 0 || mu_ <= 0)
|
|
{
|
|
polygons.clear ();
|
|
return (false);
|
|
}
|
|
const double sqr_mu = mu_*mu_;
|
|
const double sqr_max_edge = search_radius_*search_radius_;
|
|
if (nnn_ > static_cast<int> (indices_->size ()))
|
|
nnn_ = static_cast<int> (indices_->size ());
|
|
|
|
// Variables to hold the results of nearest neighbor searches
|
|
pcl::Indices nnIdx (nnn_);
|
|
std::vector<float> sqrDists (nnn_);
|
|
|
|
// current number of connected components
|
|
int part_index = 0;
|
|
|
|
// 2D coordinates of points
|
|
const Eigen::Vector2f uvn_nn_qp_zero = Eigen::Vector2f::Zero();
|
|
Eigen::Vector2f uvn_current;
|
|
Eigen::Vector2f uvn_prev;
|
|
Eigen::Vector2f uvn_next;
|
|
|
|
// initializing fields
|
|
already_connected_ = false; // see declaration for comments :P
|
|
|
|
// initializing states and fringe neighbors
|
|
part_.clear ();
|
|
state_.clear ();
|
|
source_.clear ();
|
|
ffn_.clear ();
|
|
sfn_.clear ();
|
|
part_.resize(indices_->size (), -1); // indices of point's part
|
|
state_.resize(indices_->size (), FREE);
|
|
source_.resize(indices_->size (), NONE);
|
|
ffn_.resize(indices_->size (), NONE);
|
|
sfn_.resize(indices_->size (), NONE);
|
|
fringe_queue_.clear ();
|
|
int fqIdx = 0; // current fringe's index in the queue to be processed
|
|
|
|
// Avoiding NaN coordinates if needed
|
|
if (!input_->is_dense)
|
|
{
|
|
// Skip invalid points from the indices list
|
|
for (const auto& idx : (*indices_))
|
|
if (!std::isfinite ((*input_)[idx].x) ||
|
|
!std::isfinite ((*input_)[idx].y) ||
|
|
!std::isfinite ((*input_)[idx].z))
|
|
state_[idx] = NONE;
|
|
}
|
|
|
|
// Saving coordinates and point to index mapping
|
|
coords_.clear ();
|
|
coords_.reserve (indices_->size ());
|
|
std::vector<int> point2index (input_->size (), -1);
|
|
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
|
|
{
|
|
coords_.push_back((*input_)[(*indices_)[cp]].getVector3fMap());
|
|
point2index[(*indices_)[cp]] = cp;
|
|
}
|
|
|
|
// Initializing
|
|
int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0, nr_touched = 0;
|
|
angles_.resize(nnn_);
|
|
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > uvn_nn (nnn_);
|
|
Eigen::Vector2f uvn_s;
|
|
|
|
// iterating through fringe points and finishing them until everything is done
|
|
while (is_free != NONE)
|
|
{
|
|
R_ = is_free;
|
|
if (state_[R_] == FREE)
|
|
{
|
|
state_[R_] = NONE;
|
|
part_[R_] = part_index++;
|
|
|
|
// creating starting triangle
|
|
//searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists);
|
|
//tree_->nearestKSearch ((*input_)[(*indices_)[R_]], nnn_, nnIdx, sqrDists);
|
|
tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
|
|
double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]);
|
|
|
|
// Search tree returns indices into the original cloud, but we are working with indices. TODO: make that optional!
|
|
for (int i = 1; i < nnn_; i++)
|
|
{
|
|
//if (point2index[nnIdx[i]] == -1)
|
|
// std::cerr << R_ << " [" << indices_->at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl;
|
|
nnIdx[i] = point2index[nnIdx[i]];
|
|
}
|
|
|
|
// Get the normal estimate at the current point
|
|
const Eigen::Vector3f nc = (*input_)[(*indices_)[R_]].getNormalVector3fMap ();
|
|
|
|
// Get a coordinate system that lies on a plane defined by its normal
|
|
v_ = nc.unitOrthogonal ();
|
|
u_ = nc.cross (v_);
|
|
|
|
// Projecting point onto the surface
|
|
float dist = nc.dot (coords_[R_]);
|
|
proj_qp_ = coords_[R_] - dist * nc;
|
|
|
|
// Converting coords, calculating angles and saving the projected near boundary edges
|
|
int nr_edge = 0;
|
|
std::vector<doubleEdge> doubleEdges;
|
|
for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
|
|
{
|
|
// Transforming coordinates
|
|
tmp_ = coords_[nnIdx[i]] - proj_qp_;
|
|
uvn_nn[i][0] = tmp_.dot(u_);
|
|
uvn_nn[i][1] = tmp_.dot(v_);
|
|
// Computing the angle between each neighboring point and the query point itself
|
|
angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]);
|
|
// initializing angle descriptors
|
|
angles_[i].index = nnIdx[i];
|
|
if (
|
|
(state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
|
|
|| (state_[nnIdx[i]] == NONE) || (nnIdx[i] == UNAVAILABLE) /// NOTE: discarding NaN points and those that are not in indices_
|
|
|| (sqrDists[i] > sqr_dist_threshold)
|
|
)
|
|
angles_[i].visible = false;
|
|
else
|
|
angles_[i].visible = true;
|
|
// Saving the edges between nearby boundary points
|
|
if ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY))
|
|
{
|
|
doubleEdge e;
|
|
e.index = i;
|
|
nr_edge++;
|
|
tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
|
|
e.first[0] = tmp_.dot(u_);
|
|
e.first[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
|
|
e.second[0] = tmp_.dot(u_);
|
|
e.second[1] = tmp_.dot(v_);
|
|
doubleEdges.push_back(e);
|
|
}
|
|
}
|
|
angles_[0].visible = false;
|
|
|
|
// Verify the visibility of each potential new vertex
|
|
for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
|
|
if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
|
|
{
|
|
bool visibility = true;
|
|
for (int j = 0; j < nr_edge; j++)
|
|
{
|
|
if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
|
|
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
|
|
if (!visibility)
|
|
break;
|
|
if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
|
|
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
|
|
if (!visibility)
|
|
break;
|
|
}
|
|
angles_[i].visible = visibility;
|
|
}
|
|
|
|
// Selecting first two visible free neighbors
|
|
bool not_found = true;
|
|
int left = 1;
|
|
do
|
|
{
|
|
while ((left < nnn_) && ((!angles_[left].visible) || (state_[nnIdx[left]] > FREE))) left++;
|
|
if (left >= nnn_)
|
|
break;
|
|
int right = left+1;
|
|
do
|
|
{
|
|
while ((right < nnn_) && ((!angles_[right].visible) || (state_[nnIdx[right]] > FREE))) right++;
|
|
if (right >= nnn_)
|
|
break;
|
|
if ((coords_[nnIdx[left]] - coords_[nnIdx[right]]).squaredNorm () > sqr_max_edge)
|
|
right++;
|
|
else
|
|
{
|
|
addFringePoint (nnIdx[right], R_);
|
|
addFringePoint (nnIdx[left], nnIdx[right]);
|
|
addFringePoint (R_, nnIdx[left]);
|
|
state_[R_] = state_[nnIdx[left]] = state_[nnIdx[right]] = FRINGE;
|
|
ffn_[R_] = nnIdx[left];
|
|
sfn_[R_] = nnIdx[right];
|
|
ffn_[nnIdx[left]] = nnIdx[right];
|
|
sfn_[nnIdx[left]] = R_;
|
|
ffn_[nnIdx[right]] = R_;
|
|
sfn_[nnIdx[right]] = nnIdx[left];
|
|
addTriangle (R_, nnIdx[left], nnIdx[right], polygons);
|
|
nr_parts++;
|
|
not_found = false;
|
|
break;
|
|
}
|
|
}
|
|
while (true);
|
|
left++;
|
|
}
|
|
while (not_found);
|
|
}
|
|
|
|
is_free = NONE;
|
|
for (std::size_t temp = 0; temp < indices_->size (); temp++)
|
|
{
|
|
if (state_[temp] == FREE)
|
|
{
|
|
is_free = temp;
|
|
break;
|
|
}
|
|
}
|
|
|
|
bool is_fringe = true;
|
|
while (is_fringe)
|
|
{
|
|
is_fringe = false;
|
|
|
|
int fqSize = static_cast<int> (fringe_queue_.size ());
|
|
while ((fqIdx < fqSize) && (state_[fringe_queue_[fqIdx]] != FRINGE))
|
|
fqIdx++;
|
|
|
|
// an unfinished fringe point is found
|
|
if (fqIdx >= fqSize)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
R_ = fringe_queue_[fqIdx];
|
|
is_fringe = true;
|
|
|
|
if (ffn_[R_] == sfn_[R_])
|
|
{
|
|
state_[R_] = COMPLETED;
|
|
continue;
|
|
}
|
|
//searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists);
|
|
//tree_->nearestKSearch ((*input_)[(*indices_)[R_]], nnn_, nnIdx, sqrDists);
|
|
tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists);
|
|
|
|
// Search tree returns indices into the original cloud, but we are working with indices TODO: make that optional!
|
|
for (int i = 1; i < nnn_; i++)
|
|
{
|
|
//if (point2index[nnIdx[i]] == -1)
|
|
// std::cerr << R_ << " [" << indices_->at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl;
|
|
nnIdx[i] = point2index[nnIdx[i]];
|
|
}
|
|
|
|
// Locating FFN and SFN to adapt distance threshold
|
|
double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm ();
|
|
double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm ();
|
|
double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm ();
|
|
double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist);
|
|
double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist);
|
|
if (max_sqr_fn_dist > sqrDists[nnn_-1])
|
|
{
|
|
if (0 == increase_nnn4fn)
|
|
PCL_WARN("Not enough neighbors are considered: ffn or sfn out of range! Consider increasing nnn_... Setting R=%d to be BOUNDARY!\n", R_);
|
|
increase_nnn4fn++;
|
|
state_[R_] = BOUNDARY;
|
|
continue;
|
|
}
|
|
double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
|
|
if (max_sqr_fns_dist > sqrDists[nnn_-1])
|
|
{
|
|
if (0 == increase_nnn4s)
|
|
PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_);
|
|
increase_nnn4s++;
|
|
}
|
|
|
|
// Get the normal estimate at the current point
|
|
const Eigen::Vector3f nc = (*input_)[(*indices_)[R_]].getNormalVector3fMap ();
|
|
|
|
// Get a coordinate system that lies on a plane defined by its normal
|
|
v_ = nc.unitOrthogonal ();
|
|
u_ = nc.cross (v_);
|
|
|
|
// Projecting point onto the surface
|
|
float dist = nc.dot (coords_[R_]);
|
|
proj_qp_ = coords_[R_] - dist * nc;
|
|
|
|
// Converting coords, calculating angles and saving the projected near boundary edges
|
|
int nr_edge = 0;
|
|
std::vector<doubleEdge> doubleEdges;
|
|
for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
|
|
{
|
|
tmp_ = coords_[nnIdx[i]] - proj_qp_;
|
|
uvn_nn[i][0] = tmp_.dot(u_);
|
|
uvn_nn[i][1] = tmp_.dot(v_);
|
|
|
|
// Computing the angle between each neighboring point and the query point itself
|
|
angles_[i].angle = std::atan2(uvn_nn[i][1], uvn_nn[i][0]);
|
|
// initializing angle descriptors
|
|
angles_[i].index = nnIdx[i];
|
|
angles_[i].nnIndex = i;
|
|
if (
|
|
(state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY)
|
|
|| (state_[nnIdx[i]] == NONE) || (nnIdx[i] == UNAVAILABLE) /// NOTE: discarding NaN points and those that are not in indices_
|
|
|| (sqrDists[i] > sqr_dist_threshold)
|
|
)
|
|
angles_[i].visible = false;
|
|
else
|
|
angles_[i].visible = true;
|
|
if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i]))
|
|
angles_[i].visible = true;
|
|
bool same_side = true;
|
|
const Eigen::Vector3f neighbor_normal = (*input_)[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); /// NOTE: nnIdx was reset
|
|
double cosine = nc.dot (neighbor_normal);
|
|
if (cosine > 1) cosine = 1;
|
|
if (cosine < -1) cosine = -1;
|
|
double angle = std::acos (cosine);
|
|
if ((!consistent_) && (angle > M_PI/2))
|
|
angle = M_PI - angle;
|
|
if (angle > eps_angle_)
|
|
{
|
|
angles_[i].visible = false;
|
|
same_side = false;
|
|
}
|
|
// Saving the edges between nearby boundary points
|
|
if ((i!=0) && (same_side) && ((state_[nnIdx[i]] == FRINGE) || (state_[nnIdx[i]] == BOUNDARY)))
|
|
{
|
|
doubleEdge e;
|
|
e.index = i;
|
|
nr_edge++;
|
|
tmp_ = coords_[ffn_[nnIdx[i]]] - proj_qp_;
|
|
e.first[0] = tmp_.dot(u_);
|
|
e.first[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
|
|
e.second[0] = tmp_.dot(u_);
|
|
e.second[1] = tmp_.dot(v_);
|
|
doubleEdges.push_back(e);
|
|
// Pruning by visibility criterion
|
|
if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
|
|
{
|
|
double angle1 = std::atan2(e.first[1] - uvn_nn[i][1], e.first[0] - uvn_nn[i][0]);
|
|
double angle2 = std::atan2(e.second[1] - uvn_nn[i][1], e.second[0] - uvn_nn[i][0]);
|
|
double angleMin, angleMax;
|
|
if (angle1 < angle2)
|
|
{
|
|
angleMin = angle1;
|
|
angleMax = angle2;
|
|
}
|
|
else
|
|
{
|
|
angleMin = angle2;
|
|
angleMax = angle1;
|
|
}
|
|
double angleR = angles_[i].angle + M_PI;
|
|
if (angleR >= M_PI)
|
|
angleR -= 2*M_PI;
|
|
if ((source_[nnIdx[i]] == ffn_[nnIdx[i]]) || (source_[nnIdx[i]] == sfn_[nnIdx[i]]))
|
|
{
|
|
if ((angleMax - angleMin) < M_PI)
|
|
{
|
|
if ((angleMin < angleR) && (angleR < angleMax))
|
|
angles_[i].visible = false;
|
|
}
|
|
else
|
|
{
|
|
if ((angleR < angleMin) || (angleMax < angleR))
|
|
angles_[i].visible = false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
tmp_ = coords_[source_[nnIdx[i]]] - proj_qp_;
|
|
uvn_s[0] = tmp_.dot(u_);
|
|
uvn_s[1] = tmp_.dot(v_);
|
|
double angleS = std::atan2(uvn_s[1] - uvn_nn[i][1], uvn_s[0] - uvn_nn[i][0]);
|
|
if ((angleMin < angleS) && (angleS < angleMax))
|
|
{
|
|
if ((angleMin < angleR) && (angleR < angleMax))
|
|
angles_[i].visible = false;
|
|
}
|
|
else
|
|
{
|
|
if ((angleR < angleMin) || (angleMax < angleR))
|
|
angles_[i].visible = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
angles_[0].visible = false;
|
|
|
|
// Verify the visibility of each potential new vertex
|
|
for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
|
|
if ((angles_[i].visible) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
|
|
{
|
|
bool visibility = true;
|
|
for (int j = 0; j < nr_edge; j++)
|
|
{
|
|
if (doubleEdges[j].index != i)
|
|
{
|
|
const auto& f = ffn_[nnIdx[doubleEdges[j].index]];
|
|
if ((f != nnIdx[i]) && (f != R_))
|
|
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
|
|
if (!visibility)
|
|
break;
|
|
|
|
const auto& s = sfn_[nnIdx[doubleEdges[j].index]];
|
|
if ((s != nnIdx[i]) && (s != R_))
|
|
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
|
|
if (!visibility)
|
|
break;
|
|
}
|
|
}
|
|
angles_[i].visible = visibility;
|
|
}
|
|
|
|
// Sorting angles
|
|
std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::nnAngleSortAsc);
|
|
|
|
// Triangulating
|
|
if (angles_[2].visible == false)
|
|
{
|
|
if ( !( (angles_[0].index == ffn_[R_] && angles_[1].index == sfn_[R_]) || (angles_[0].index == sfn_[R_] && angles_[1].index == ffn_[R_]) ) )
|
|
{
|
|
state_[R_] = BOUNDARY;
|
|
}
|
|
else
|
|
{
|
|
if ((source_[R_] == angles_[0].index) || (source_[R_] == angles_[1].index))
|
|
state_[R_] = BOUNDARY;
|
|
else
|
|
{
|
|
if (sqr_max_edge < (coords_[ffn_[R_]] - coords_[sfn_[R_]]).squaredNorm ())
|
|
{
|
|
state_[R_] = BOUNDARY;
|
|
}
|
|
else
|
|
{
|
|
tmp_ = coords_[source_[R_]] - proj_qp_;
|
|
uvn_s[0] = tmp_.dot(u_);
|
|
uvn_s[1] = tmp_.dot(v_);
|
|
double angleS = std::atan2(uvn_s[1], uvn_s[0]);
|
|
double dif = angles_[1].angle - angles_[0].angle;
|
|
if ((angles_[0].angle < angleS) && (angleS < angles_[1].angle))
|
|
{
|
|
if (dif < 2*M_PI - maximum_angle_)
|
|
state_[R_] = BOUNDARY;
|
|
else
|
|
closeTriangle (polygons);
|
|
}
|
|
else
|
|
{
|
|
if (dif >= maximum_angle_)
|
|
state_[R_] = BOUNDARY;
|
|
else
|
|
closeTriangle (polygons);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
|
|
// Finding the FFN and SFN
|
|
int start = -1, end = -1;
|
|
for (int i=0; i<nnn_; i++)
|
|
{
|
|
if (ffn_[R_] == angles_[i].index)
|
|
{
|
|
start = i;
|
|
if (sfn_[R_] == angles_[i+1].index)
|
|
end = i+1;
|
|
else
|
|
if (i==0)
|
|
{
|
|
for (i = i+2; i < nnn_; i++)
|
|
if (sfn_[R_] == angles_[i].index)
|
|
break;
|
|
end = i;
|
|
}
|
|
else
|
|
{
|
|
for (i = i+2; i < nnn_; i++)
|
|
if (sfn_[R_] == angles_[i].index)
|
|
break;
|
|
end = i;
|
|
}
|
|
break;
|
|
}
|
|
if (sfn_[R_] == angles_[i].index)
|
|
{
|
|
start = i;
|
|
if (ffn_[R_] == angles_[i+1].index)
|
|
end = i+1;
|
|
else
|
|
if (i==0)
|
|
{
|
|
for (i = i+2; i < nnn_; i++)
|
|
if (ffn_[R_] == angles_[i].index)
|
|
break;
|
|
end = i;
|
|
}
|
|
else
|
|
{
|
|
for (i = i+2; i < nnn_; i++)
|
|
if (ffn_[R_] == angles_[i].index)
|
|
break;
|
|
end = i;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
// start and end are always set, as we checked if ffn or sfn are out of range before, but let's check anyways if < 0
|
|
if ((start < 0) || (end < 0) || (end == nnn_) || (!angles_[start].visible) || (!angles_[end].visible))
|
|
{
|
|
state_[R_] = BOUNDARY;
|
|
continue;
|
|
}
|
|
|
|
// Finding last visible nn
|
|
int last_visible = end;
|
|
while ((last_visible+1<nnn_) && (angles_[last_visible+1].visible)) last_visible++;
|
|
|
|
// Finding visibility region of R
|
|
bool need_invert = false;
|
|
if ((source_[R_] == ffn_[R_]) || (source_[R_] == sfn_[R_]))
|
|
{
|
|
if ((angles_[end].angle - angles_[start].angle) < M_PI)
|
|
need_invert = true;
|
|
}
|
|
else
|
|
{
|
|
int sourceIdx;
|
|
for (sourceIdx=0; sourceIdx<nnn_; sourceIdx++)
|
|
if (angles_[sourceIdx].index == source_[R_])
|
|
break;
|
|
if (sourceIdx == nnn_)
|
|
{
|
|
int vis_free = NONE, nnCB = NONE; // any free visible and nearest completed or boundary neighbor of R
|
|
for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself
|
|
{
|
|
// NOTE: nnCB is an index in nnIdx
|
|
if ((state_[nnIdx[i]] == COMPLETED) || (state_[nnIdx[i]] == BOUNDARY))
|
|
{
|
|
if (nnCB == NONE)
|
|
{
|
|
nnCB = i;
|
|
if (vis_free != NONE)
|
|
break;
|
|
}
|
|
}
|
|
// NOTE: vis_free is an index in angles
|
|
if (state_[angles_[i].index] <= FREE)
|
|
{
|
|
if (i <= last_visible)
|
|
{
|
|
vis_free = i;
|
|
if (nnCB != NONE)
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
// NOTE: nCB is an index in angles
|
|
int nCB = 0;
|
|
if (nnCB != NONE)
|
|
while (angles_[nCB].index != nnIdx[nnCB]) nCB++;
|
|
else
|
|
nCB = NONE;
|
|
|
|
if (vis_free != NONE)
|
|
{
|
|
if ((vis_free < start) || (vis_free > end))
|
|
need_invert = true;
|
|
}
|
|
else
|
|
{
|
|
if (nCB != NONE)
|
|
{
|
|
if ((nCB == start) || (nCB == end))
|
|
{
|
|
bool inside_CB = false;
|
|
bool outside_CB = false;
|
|
for (int i=0; i<nnn_; i++)
|
|
{
|
|
if (
|
|
((state_[angles_[i].index] == COMPLETED) || (state_[angles_[i].index] == BOUNDARY))
|
|
&& (i != start) && (i != end)
|
|
)
|
|
{
|
|
if ((angles_[start].angle <= angles_[i].angle) && (angles_[i].angle <= angles_[end].angle))
|
|
{
|
|
inside_CB = true;
|
|
if (outside_CB)
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
outside_CB = true;
|
|
if (inside_CB)
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (inside_CB && !outside_CB)
|
|
need_invert = true;
|
|
else if (!(!inside_CB && outside_CB))
|
|
{
|
|
if ((angles_[end].angle - angles_[start].angle) < M_PI)
|
|
need_invert = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if ((angles_[nCB].angle > angles_[start].angle) && (angles_[nCB].angle < angles_[end].angle))
|
|
need_invert = true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (start == end-1)
|
|
need_invert = true;
|
|
}
|
|
}
|
|
}
|
|
else if ((angles_[start].angle < angles_[sourceIdx].angle) && (angles_[sourceIdx].angle < angles_[end].angle))
|
|
need_invert = true;
|
|
}
|
|
|
|
// switching start and end if necessary
|
|
if (need_invert)
|
|
{
|
|
int tmp = start;
|
|
start = end;
|
|
end = tmp;
|
|
}
|
|
|
|
// Arranging visible nnAngles in the order they need to be connected and
|
|
// compute the maximal angle difference between two consecutive visible angles
|
|
bool is_boundary = false, is_skinny = false;
|
|
std::vector<bool> gaps (nnn_, false);
|
|
std::vector<bool> skinny (nnn_, false);
|
|
std::vector<double> dif (nnn_);
|
|
std::vector<int> angleIdx; angleIdx.reserve (nnn_);
|
|
if (start > end)
|
|
{
|
|
for (int j=start; j<last_visible; j++)
|
|
{
|
|
dif[j] = (angles_[j+1].angle - angles_[j].angle);
|
|
if (dif[j] < minimum_angle_)
|
|
{
|
|
skinny[j] = is_skinny = true;
|
|
}
|
|
else if (maximum_angle_ <= dif[j])
|
|
{
|
|
gaps[j] = is_boundary = true;
|
|
}
|
|
if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
|
|
{
|
|
gaps[j] = is_boundary = true;
|
|
}
|
|
angleIdx.push_back(j);
|
|
}
|
|
|
|
dif[last_visible] = (2*M_PI + angles_[0].angle - angles_[last_visible].angle);
|
|
if (dif[last_visible] < minimum_angle_)
|
|
{
|
|
skinny[last_visible] = is_skinny = true;
|
|
}
|
|
else if (maximum_angle_ <= dif[last_visible])
|
|
{
|
|
gaps[last_visible] = is_boundary = true;
|
|
}
|
|
if ((!gaps[last_visible]) && (sqr_max_edge < (coords_[angles_[0].index] - coords_[angles_[last_visible].index]).squaredNorm ()))
|
|
{
|
|
gaps[last_visible] = is_boundary = true;
|
|
}
|
|
angleIdx.push_back(last_visible);
|
|
|
|
for (int j=0; j<end; j++)
|
|
{
|
|
dif[j] = (angles_[j+1].angle - angles_[j].angle);
|
|
if (dif[j] < minimum_angle_)
|
|
{
|
|
skinny[j] = is_skinny = true;
|
|
}
|
|
else if (maximum_angle_ <= dif[j])
|
|
{
|
|
gaps[j] = is_boundary = true;
|
|
}
|
|
if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
|
|
{
|
|
gaps[j] = is_boundary = true;
|
|
}
|
|
angleIdx.push_back(j);
|
|
}
|
|
angleIdx.push_back(end);
|
|
}
|
|
// start < end
|
|
else
|
|
{
|
|
for (int j=start; j<end; j++)
|
|
{
|
|
dif[j] = (angles_[j+1].angle - angles_[j].angle);
|
|
if (dif[j] < minimum_angle_)
|
|
{
|
|
skinny[j] = is_skinny = true;
|
|
}
|
|
else if (maximum_angle_ <= dif[j])
|
|
{
|
|
gaps[j] = is_boundary = true;
|
|
}
|
|
if ((!gaps[j]) && (sqr_max_edge < (coords_[angles_[j+1].index] - coords_[angles_[j].index]).squaredNorm ()))
|
|
{
|
|
gaps[j] = is_boundary = true;
|
|
}
|
|
angleIdx.push_back(j);
|
|
}
|
|
angleIdx.push_back(end);
|
|
}
|
|
|
|
// Set the state of the point
|
|
state_[R_] = is_boundary ? BOUNDARY : COMPLETED;
|
|
|
|
std::vector<int>::iterator first_gap_after = angleIdx.end ();
|
|
std::vector<int>::iterator last_gap_before = angleIdx.begin ();
|
|
int nr_gaps = 0;
|
|
for (std::vector<int>::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; ++it)
|
|
{
|
|
if (gaps[*it])
|
|
{
|
|
nr_gaps++;
|
|
if (first_gap_after == angleIdx.end())
|
|
first_gap_after = it;
|
|
last_gap_before = it+1;
|
|
}
|
|
}
|
|
if (nr_gaps > 1)
|
|
{
|
|
angleIdx.erase(first_gap_after+1, last_gap_before);
|
|
}
|
|
|
|
// Neglecting points that would form skinny triangles (if possible)
|
|
if (is_skinny)
|
|
{
|
|
double angle_so_far = 0, angle_would_be;
|
|
double max_combined_angle = (std::min)(maximum_angle_, M_PI-2*minimum_angle_);
|
|
Eigen::Vector2f X;
|
|
Eigen::Vector2f S1;
|
|
Eigen::Vector2f S2;
|
|
std::vector<int> to_erase;
|
|
for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it)
|
|
{
|
|
if (gaps[*(it-1)])
|
|
angle_so_far = 0;
|
|
else
|
|
angle_so_far += dif[*(it-1)];
|
|
if (gaps[*it])
|
|
angle_would_be = angle_so_far;
|
|
else
|
|
angle_would_be = angle_so_far + dif[*it];
|
|
if (
|
|
(skinny[*it] || skinny[*(it-1)]) &&
|
|
((state_[angles_[*it].index] <= FREE) || (state_[angles_[*(it-1)].index] <= FREE)) &&
|
|
((!gaps[*it]) || (angles_[*it].nnIndex > angles_[*(it-1)].nnIndex)) &&
|
|
((!gaps[*(it-1)]) || (angles_[*it].nnIndex > angles_[*(it+1)].nnIndex)) &&
|
|
(angle_would_be < max_combined_angle)
|
|
)
|
|
{
|
|
if (gaps[*(it-1)])
|
|
{
|
|
gaps[*it] = true;
|
|
to_erase.push_back(*it);
|
|
}
|
|
else if (gaps[*it])
|
|
{
|
|
gaps[*(it-1)] = true;
|
|
to_erase.push_back(*it);
|
|
}
|
|
else
|
|
{
|
|
std::vector<int>::iterator prev_it;
|
|
int erased_idx = static_cast<int> (to_erase.size ()) -1;
|
|
for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); --it)
|
|
if (*it == to_erase[erased_idx])
|
|
erased_idx--;
|
|
else
|
|
break;
|
|
bool can_delete = true;
|
|
for (std::vector<int>::iterator curr_it = prev_it+1; curr_it != it+1; ++curr_it)
|
|
{
|
|
tmp_ = coords_[angles_[*curr_it].index] - proj_qp_;
|
|
X[0] = tmp_.dot(u_);
|
|
X[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[angles_[*prev_it].index] - proj_qp_;
|
|
S1[0] = tmp_.dot(u_);
|
|
S1[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[angles_[*(it+1)].index] - proj_qp_;
|
|
S2[0] = tmp_.dot(u_);
|
|
S2[1] = tmp_.dot(v_);
|
|
// check for inclusions
|
|
if (isVisible(X,S1,S2))
|
|
{
|
|
can_delete = false;
|
|
angle_so_far = 0;
|
|
break;
|
|
}
|
|
}
|
|
if (can_delete)
|
|
{
|
|
to_erase.push_back(*it);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
angle_so_far = 0;
|
|
}
|
|
for (const auto &idx : to_erase)
|
|
{
|
|
for (std::vector<int>::iterator iter = angleIdx.begin(); iter != angleIdx.end(); ++iter)
|
|
if (idx == *iter)
|
|
{
|
|
angleIdx.erase(iter);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Writing edges and updating edge-front
|
|
changed_1st_fn_ = false;
|
|
changed_2nd_fn_ = false;
|
|
new2boundary_ = NONE;
|
|
for (std::vector<int>::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it)
|
|
{
|
|
current_index_ = angles_[*it].index;
|
|
|
|
is_current_free_ = false;
|
|
if (state_[current_index_] <= FREE)
|
|
{
|
|
state_[current_index_] = FRINGE;
|
|
is_current_free_ = true;
|
|
}
|
|
else if (!already_connected_)
|
|
{
|
|
prev_is_ffn_ = (ffn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
|
|
prev_is_sfn_ = (sfn_[current_index_] == angles_[*(it-1)].index) && (!gaps[*(it-1)]);
|
|
next_is_ffn_ = (ffn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
|
|
next_is_sfn_ = (sfn_[current_index_] == angles_[*(it+1)].index) && (!gaps[*it]);
|
|
if (!prev_is_ffn_ && !next_is_sfn_ && !prev_is_sfn_ && !next_is_ffn_)
|
|
{
|
|
nr_touched++;
|
|
}
|
|
}
|
|
|
|
if (gaps[*it])
|
|
if (gaps[*(it-1)])
|
|
{
|
|
if (is_current_free_)
|
|
state_[current_index_] = NONE; /// TODO: document!
|
|
}
|
|
|
|
else // (gaps[*it]) && ^(gaps[*(it-1)])
|
|
{
|
|
addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
|
|
addFringePoint (current_index_, R_);
|
|
new2boundary_ = current_index_;
|
|
if (!already_connected_)
|
|
connectPoint (polygons, angles_[*(it-1)].index, R_,
|
|
angles_[*(it+1)].index,
|
|
uvn_nn[angles_[*it].nnIndex], uvn_nn[angles_[*(it-1)].nnIndex], uvn_nn_qp_zero);
|
|
else already_connected_ = false;
|
|
if (ffn_[R_] == angles_[*(angleIdx.begin())].index)
|
|
{
|
|
ffn_[R_] = new2boundary_;
|
|
}
|
|
else if (sfn_[R_] == angles_[*(angleIdx.begin())].index)
|
|
{
|
|
sfn_[R_] = new2boundary_;
|
|
}
|
|
}
|
|
|
|
else // ^(gaps[*it])
|
|
if (gaps[*(it-1)])
|
|
{
|
|
addFringePoint (current_index_, R_);
|
|
new2boundary_ = current_index_;
|
|
if (!already_connected_) connectPoint (polygons, R_, angles_[*(it+1)].index,
|
|
(it+2) == angleIdx.end() ? -1 : angles_[*(it+2)].index,
|
|
uvn_nn[angles_[*it].nnIndex], uvn_nn_qp_zero,
|
|
uvn_nn[angles_[*(it+1)].nnIndex]);
|
|
else already_connected_ = false;
|
|
if (ffn_[R_] == angles_[*(angleIdx.end()-1)].index)
|
|
{
|
|
ffn_[R_] = new2boundary_;
|
|
}
|
|
else if (sfn_[R_] == angles_[*(angleIdx.end()-1)].index)
|
|
{
|
|
sfn_[R_] = new2boundary_;
|
|
}
|
|
}
|
|
|
|
else // ^(gaps[*it]) && ^(gaps[*(it-1)])
|
|
{
|
|
addTriangle (current_index_, angles_[*(it-1)].index, R_, polygons);
|
|
addFringePoint (current_index_, R_);
|
|
if (!already_connected_) connectPoint (polygons, angles_[*(it-1)].index, angles_[*(it+1)].index,
|
|
(it+2) == angleIdx.end() ? -1 : gaps[*(it+1)] ? R_ : angles_[*(it+2)].index,
|
|
uvn_nn[angles_[*it].nnIndex],
|
|
uvn_nn[angles_[*(it-1)].nnIndex],
|
|
uvn_nn[angles_[*(it+1)].nnIndex]);
|
|
else already_connected_ = false;
|
|
}
|
|
}
|
|
|
|
// Finishing up R_
|
|
if (ffn_[R_] == sfn_[R_])
|
|
{
|
|
state_[R_] = COMPLETED;
|
|
}
|
|
if (!gaps[*(angleIdx.end()-2)])
|
|
{
|
|
addTriangle (angles_[*(angleIdx.end()-2)].index, angles_[*(angleIdx.end()-1)].index, R_, polygons);
|
|
addFringePoint (angles_[*(angleIdx.end()-2)].index, R_);
|
|
if (R_ == ffn_[angles_[*(angleIdx.end()-1)].index])
|
|
{
|
|
if (angles_[*(angleIdx.end()-2)].index == sfn_[angles_[*(angleIdx.end()-1)].index])
|
|
{
|
|
state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
|
|
}
|
|
else
|
|
{
|
|
ffn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
|
|
}
|
|
}
|
|
else if (R_ == sfn_[angles_[*(angleIdx.end()-1)].index])
|
|
{
|
|
if (angles_[*(angleIdx.end()-2)].index == ffn_[angles_[*(angleIdx.end()-1)].index])
|
|
{
|
|
state_[angles_[*(angleIdx.end()-1)].index] = COMPLETED;
|
|
}
|
|
else
|
|
{
|
|
sfn_[angles_[*(angleIdx.end()-1)].index] = angles_[*(angleIdx.end()-2)].index;
|
|
}
|
|
}
|
|
}
|
|
if (!gaps[*(angleIdx.begin())])
|
|
{
|
|
if (R_ == ffn_[angles_[*(angleIdx.begin())].index])
|
|
{
|
|
if (angles_[*(angleIdx.begin()+1)].index == sfn_[angles_[*(angleIdx.begin())].index])
|
|
{
|
|
state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
|
|
}
|
|
else
|
|
{
|
|
ffn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
|
|
}
|
|
}
|
|
else if (R_ == sfn_[angles_[*(angleIdx.begin())].index])
|
|
{
|
|
if (angles_[*(angleIdx.begin()+1)].index == ffn_[angles_[*(angleIdx.begin())].index])
|
|
{
|
|
state_[angles_[*(angleIdx.begin())].index] = COMPLETED;
|
|
}
|
|
else
|
|
{
|
|
sfn_[angles_[*(angleIdx.begin())].index] = angles_[*(angleIdx.begin()+1)].index;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
PCL_DEBUG ("Number of triangles: %lu\n", polygons.size());
|
|
PCL_DEBUG ("Number of unconnected parts: %d\n", nr_parts);
|
|
if (increase_nnn4fn > 0)
|
|
PCL_WARN ("Number of neighborhood size increase requests for fringe neighbors: %d\n", increase_nnn4fn);
|
|
if (increase_nnn4s > 0)
|
|
PCL_WARN ("Number of neighborhood size increase requests for source: %d\n", increase_nnn4s);
|
|
if (increase_dist > 0)
|
|
PCL_WARN ("Number of automatic maximum distance increases: %d\n", increase_dist);
|
|
|
|
// sorting and removing doubles from fringe queue
|
|
std::sort (fringe_queue_.begin (), fringe_queue_.end ());
|
|
fringe_queue_.erase (std::unique (fringe_queue_.begin (), fringe_queue_.end ()), fringe_queue_.end ());
|
|
PCL_DEBUG ("Number of processed points: %lu / %lu\n", fringe_queue_.size(), indices_->size ());
|
|
return (true);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::GreedyProjectionTriangulation<PointInT>::closeTriangle (std::vector<pcl::Vertices> &polygons)
|
|
{
|
|
state_[R_] = COMPLETED;
|
|
addTriangle (angles_[0].index, angles_[1].index, R_, polygons);
|
|
for (int aIdx=0; aIdx<2; aIdx++)
|
|
{
|
|
if (ffn_[angles_[aIdx].index] == R_)
|
|
{
|
|
if (sfn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
|
|
{
|
|
state_[angles_[aIdx].index] = COMPLETED;
|
|
}
|
|
else
|
|
{
|
|
ffn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
|
|
}
|
|
}
|
|
else if (sfn_[angles_[aIdx].index] == R_)
|
|
{
|
|
if (ffn_[angles_[aIdx].index] == angles_[(aIdx+1)%2].index)
|
|
{
|
|
state_[angles_[aIdx].index] = COMPLETED;
|
|
}
|
|
else
|
|
{
|
|
sfn_[angles_[aIdx].index] = angles_[(aIdx+1)%2].index;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::GreedyProjectionTriangulation<PointInT>::connectPoint (
|
|
std::vector<pcl::Vertices> &polygons,
|
|
const pcl::index_t prev_index, const pcl::index_t next_index, const pcl::index_t next_next_index,
|
|
const Eigen::Vector2f &uvn_current,
|
|
const Eigen::Vector2f &uvn_prev,
|
|
const Eigen::Vector2f &uvn_next)
|
|
{
|
|
if (is_current_free_)
|
|
{
|
|
ffn_[current_index_] = prev_index;
|
|
sfn_[current_index_] = next_index;
|
|
}
|
|
else
|
|
{
|
|
if ((prev_is_ffn_ && next_is_sfn_) || (prev_is_sfn_ && next_is_ffn_))
|
|
state_[current_index_] = COMPLETED;
|
|
else if (prev_is_ffn_ && !next_is_sfn_)
|
|
ffn_[current_index_] = next_index;
|
|
else if (next_is_ffn_ && !prev_is_sfn_)
|
|
ffn_[current_index_] = prev_index;
|
|
else if (prev_is_sfn_ && !next_is_ffn_)
|
|
sfn_[current_index_] = next_index;
|
|
else if (next_is_sfn_ && !prev_is_ffn_)
|
|
sfn_[current_index_] = prev_index;
|
|
else
|
|
{
|
|
bool found_triangle = false;
|
|
if ((prev_index != R_) && ((ffn_[current_index_] == ffn_[prev_index]) || (ffn_[current_index_] == sfn_[prev_index])))
|
|
{
|
|
found_triangle = true;
|
|
addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
|
|
state_[prev_index] = COMPLETED;
|
|
state_[ffn_[current_index_]] = COMPLETED;
|
|
ffn_[current_index_] = next_index;
|
|
}
|
|
else if ((prev_index != R_) && ((sfn_[current_index_] == ffn_[prev_index]) || (sfn_[current_index_] == sfn_[prev_index])))
|
|
{
|
|
found_triangle = true;
|
|
addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
|
|
state_[prev_index] = COMPLETED;
|
|
state_[sfn_[current_index_]] = COMPLETED;
|
|
sfn_[current_index_] = next_index;
|
|
}
|
|
else if (state_[next_index] > FREE)
|
|
{
|
|
if ((ffn_[current_index_] == ffn_[next_index]) || (ffn_[current_index_] == sfn_[next_index]))
|
|
{
|
|
found_triangle = true;
|
|
addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
|
|
|
|
if (ffn_[current_index_] == ffn_[next_index])
|
|
{
|
|
ffn_[next_index] = current_index_;
|
|
}
|
|
else
|
|
{
|
|
sfn_[next_index] = current_index_;
|
|
}
|
|
state_[ffn_[current_index_]] = COMPLETED;
|
|
ffn_[current_index_] = prev_index;
|
|
}
|
|
else if ((sfn_[current_index_] == ffn_[next_index]) || (sfn_[current_index_] == sfn_[next_index]))
|
|
{
|
|
found_triangle = true;
|
|
addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
|
|
|
|
if (sfn_[current_index_] == ffn_[next_index])
|
|
{
|
|
ffn_[next_index] = current_index_;
|
|
}
|
|
else
|
|
{
|
|
sfn_[next_index] = current_index_;
|
|
}
|
|
state_[sfn_[current_index_]] = COMPLETED;
|
|
sfn_[current_index_] = prev_index;
|
|
}
|
|
}
|
|
|
|
if (found_triangle)
|
|
{
|
|
}
|
|
else
|
|
{
|
|
tmp_ = coords_[ffn_[current_index_]] - proj_qp_;
|
|
uvn_ffn_[0] = tmp_.dot(u_);
|
|
uvn_ffn_[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[sfn_[current_index_]] - proj_qp_;
|
|
uvn_sfn_[0] = tmp_.dot(u_);
|
|
uvn_sfn_[1] = tmp_.dot(v_);
|
|
bool prev_ffn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_prev, uvn_sfn_, uvn_current, uvn_ffn_);
|
|
bool prev_sfn = isVisible(uvn_prev, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_prev, uvn_ffn_, uvn_current, uvn_sfn_);
|
|
bool next_ffn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_ffn_) && isVisible(uvn_next, uvn_sfn_, uvn_current, uvn_ffn_);
|
|
bool next_sfn = isVisible(uvn_next, uvn_prev, uvn_current, uvn_sfn_) && isVisible(uvn_next, uvn_ffn_, uvn_current, uvn_sfn_);
|
|
int min_dist = -1;
|
|
if (prev_ffn && next_sfn && prev_sfn && next_ffn)
|
|
{
|
|
/* should be never the case */
|
|
double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
if (prev2f < prev2s)
|
|
{
|
|
if (prev2f < next2f)
|
|
{
|
|
if (prev2f < next2s)
|
|
min_dist = 0;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
else
|
|
{
|
|
if (next2f < next2s)
|
|
min_dist = 2;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (prev2s < next2f)
|
|
{
|
|
if (prev2s < next2s)
|
|
min_dist = 1;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
else
|
|
{
|
|
if (next2f < next2s)
|
|
min_dist = 2;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
}
|
|
}
|
|
else if (prev_ffn && next_sfn)
|
|
{
|
|
/* a clear case */
|
|
double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
if (prev2f < next2s)
|
|
min_dist = 0;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
else if (prev_sfn && next_ffn)
|
|
{
|
|
/* a clear case */
|
|
double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
if (prev2s < next2f)
|
|
min_dist = 1;
|
|
else
|
|
min_dist = 2;
|
|
}
|
|
/* straightforward cases */
|
|
else if (prev_ffn && !next_sfn && !prev_sfn && !next_ffn)
|
|
min_dist = 0;
|
|
else if (!prev_ffn && !next_sfn && prev_sfn && !next_ffn)
|
|
min_dist = 1;
|
|
else if (!prev_ffn && !next_sfn && !prev_sfn && next_ffn)
|
|
min_dist = 2;
|
|
else if (!prev_ffn && next_sfn && !prev_sfn && !next_ffn)
|
|
min_dist = 3;
|
|
/* messed up cases */
|
|
else if (prev_ffn)
|
|
{
|
|
double prev2f = (coords_[ffn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
if (prev_sfn)
|
|
{
|
|
double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
if (prev2s < prev2f)
|
|
min_dist = 1;
|
|
else
|
|
min_dist = 0;
|
|
}
|
|
else if (next_ffn)
|
|
{
|
|
double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
if (next2f < prev2f)
|
|
min_dist = 2;
|
|
else
|
|
min_dist = 0;
|
|
}
|
|
}
|
|
else if (next_sfn)
|
|
{
|
|
double next2s = (coords_[sfn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
if (prev_sfn)
|
|
{
|
|
double prev2s = (coords_[sfn_[current_index_]] - coords_[prev_index]).squaredNorm ();
|
|
if (prev2s < next2s)
|
|
min_dist = 1;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
else if (next_ffn)
|
|
{
|
|
double next2f = (coords_[ffn_[current_index_]] - coords_[next_index]).squaredNorm ();
|
|
if (next2f < next2s)
|
|
min_dist = 2;
|
|
else
|
|
min_dist = 3;
|
|
}
|
|
}
|
|
switch (min_dist)
|
|
{
|
|
case 0://prev2f:
|
|
{
|
|
addTriangle (current_index_, ffn_[current_index_], prev_index, polygons);
|
|
|
|
/* updating prev_index */
|
|
if (ffn_[prev_index] == current_index_)
|
|
{
|
|
ffn_[prev_index] = ffn_[current_index_];
|
|
}
|
|
else if (sfn_[prev_index] == current_index_)
|
|
{
|
|
sfn_[prev_index] = ffn_[current_index_];
|
|
}
|
|
else if (ffn_[prev_index] == R_)
|
|
{
|
|
changed_1st_fn_ = true;
|
|
ffn_[prev_index] = ffn_[current_index_];
|
|
}
|
|
else if (sfn_[prev_index] == R_)
|
|
{
|
|
changed_1st_fn_ = true;
|
|
sfn_[prev_index] = ffn_[current_index_];
|
|
}
|
|
else if (prev_index == R_)
|
|
{
|
|
new2boundary_ = ffn_[current_index_];
|
|
}
|
|
|
|
/* updating ffn */
|
|
if (ffn_[ffn_[current_index_]] == current_index_)
|
|
{
|
|
ffn_[ffn_[current_index_]] = prev_index;
|
|
}
|
|
else if (sfn_[ffn_[current_index_]] == current_index_)
|
|
{
|
|
sfn_[ffn_[current_index_]] = prev_index;
|
|
}
|
|
|
|
/* updating current */
|
|
ffn_[current_index_] = next_index;
|
|
|
|
break;
|
|
}
|
|
case 1://prev2s:
|
|
{
|
|
addTriangle (current_index_, sfn_[current_index_], prev_index, polygons);
|
|
|
|
/* updating prev_index */
|
|
if (ffn_[prev_index] == current_index_)
|
|
{
|
|
ffn_[prev_index] = sfn_[current_index_];
|
|
}
|
|
else if (sfn_[prev_index] == current_index_)
|
|
{
|
|
sfn_[prev_index] = sfn_[current_index_];
|
|
}
|
|
else if (ffn_[prev_index] == R_)
|
|
{
|
|
changed_1st_fn_ = true;
|
|
ffn_[prev_index] = sfn_[current_index_];
|
|
}
|
|
else if (sfn_[prev_index] == R_)
|
|
{
|
|
changed_1st_fn_ = true;
|
|
sfn_[prev_index] = sfn_[current_index_];
|
|
}
|
|
else if (prev_index == R_)
|
|
{
|
|
new2boundary_ = sfn_[current_index_];
|
|
}
|
|
|
|
/* updating sfn */
|
|
if (ffn_[sfn_[current_index_]] == current_index_)
|
|
{
|
|
ffn_[sfn_[current_index_]] = prev_index;
|
|
}
|
|
else if (sfn_[sfn_[current_index_]] == current_index_)
|
|
{
|
|
sfn_[sfn_[current_index_]] = prev_index;
|
|
}
|
|
|
|
/* updating current */
|
|
sfn_[current_index_] = next_index;
|
|
|
|
break;
|
|
}
|
|
case 2://next2f:
|
|
{
|
|
addTriangle (current_index_, ffn_[current_index_], next_index, polygons);
|
|
auto neighbor_update = next_index;
|
|
|
|
/* updating next_index */
|
|
if (state_[next_index] <= FREE)
|
|
{
|
|
state_[next_index] = FRINGE;
|
|
ffn_[next_index] = current_index_;
|
|
sfn_[next_index] = ffn_[current_index_];
|
|
}
|
|
else
|
|
{
|
|
if (ffn_[next_index] == R_)
|
|
{
|
|
changed_2nd_fn_ = true;
|
|
ffn_[next_index] = ffn_[current_index_];
|
|
}
|
|
else if (sfn_[next_index] == R_)
|
|
{
|
|
changed_2nd_fn_ = true;
|
|
sfn_[next_index] = ffn_[current_index_];
|
|
}
|
|
else if (next_index == R_)
|
|
{
|
|
new2boundary_ = ffn_[current_index_];
|
|
if (next_next_index == new2boundary_)
|
|
already_connected_ = true;
|
|
}
|
|
else if (ffn_[next_index] == next_next_index)
|
|
{
|
|
already_connected_ = true;
|
|
ffn_[next_index] = ffn_[current_index_];
|
|
}
|
|
else if (sfn_[next_index] == next_next_index)
|
|
{
|
|
already_connected_ = true;
|
|
sfn_[next_index] = ffn_[current_index_];
|
|
}
|
|
else
|
|
{
|
|
tmp_ = coords_[ffn_[next_index]] - proj_qp_;
|
|
uvn_next_ffn_[0] = tmp_.dot(u_);
|
|
uvn_next_ffn_[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[sfn_[next_index]] - proj_qp_;
|
|
uvn_next_sfn_[0] = tmp_.dot(u_);
|
|
uvn_next_sfn_[1] = tmp_.dot(v_);
|
|
|
|
bool ffn_next_ffn = isVisible(uvn_next_ffn_, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_next_ffn_, uvn_next, uvn_next_sfn_, uvn_ffn_);
|
|
bool sfn_next_ffn = isVisible(uvn_next_sfn_, uvn_next, uvn_current, uvn_ffn_) && isVisible(uvn_next_sfn_, uvn_next, uvn_next_ffn_, uvn_ffn_);
|
|
|
|
int connect2ffn = -1;
|
|
if (ffn_next_ffn && sfn_next_ffn)
|
|
{
|
|
double fn2f = (coords_[ffn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
|
|
double sn2f = (coords_[ffn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
|
|
if (fn2f < sn2f) connect2ffn = 0;
|
|
else connect2ffn = 1;
|
|
}
|
|
else if (ffn_next_ffn) connect2ffn = 0;
|
|
else if (sfn_next_ffn) connect2ffn = 1;
|
|
|
|
switch (connect2ffn)
|
|
{
|
|
case 0: // ffn[next]
|
|
{
|
|
addTriangle (next_index, ffn_[current_index_], ffn_[next_index], polygons);
|
|
neighbor_update = ffn_[next_index];
|
|
|
|
/* ffn[next_index] */
|
|
if ((ffn_[ffn_[next_index]] == ffn_[current_index_]) || (sfn_[ffn_[next_index]] == ffn_[current_index_]))
|
|
{
|
|
state_[ffn_[next_index]] = COMPLETED;
|
|
}
|
|
else if (ffn_[ffn_[next_index]] == next_index)
|
|
{
|
|
ffn_[ffn_[next_index]] = ffn_[current_index_];
|
|
}
|
|
else if (sfn_[ffn_[next_index]] == next_index)
|
|
{
|
|
sfn_[ffn_[next_index]] = ffn_[current_index_];
|
|
}
|
|
|
|
ffn_[next_index] = current_index_;
|
|
|
|
break;
|
|
}
|
|
case 1: // sfn[next]
|
|
{
|
|
addTriangle (next_index, ffn_[current_index_], sfn_[next_index], polygons);
|
|
neighbor_update = sfn_[next_index];
|
|
|
|
/* sfn[next_index] */
|
|
if ((ffn_[sfn_[next_index]] == ffn_[current_index_]) || (sfn_[sfn_[next_index]] == ffn_[current_index_]))
|
|
{
|
|
state_[sfn_[next_index]] = COMPLETED;
|
|
}
|
|
else if (ffn_[sfn_[next_index]] == next_index)
|
|
{
|
|
ffn_[sfn_[next_index]] = ffn_[current_index_];
|
|
}
|
|
else if (sfn_[sfn_[next_index]] == next_index)
|
|
{
|
|
sfn_[sfn_[next_index]] = ffn_[current_index_];
|
|
}
|
|
|
|
sfn_[next_index] = current_index_;
|
|
|
|
break;
|
|
}
|
|
default:;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* updating ffn */
|
|
if ((ffn_[ffn_[current_index_]] == neighbor_update) || (sfn_[ffn_[current_index_]] == neighbor_update))
|
|
{
|
|
state_[ffn_[current_index_]] = COMPLETED;
|
|
}
|
|
else if (ffn_[ffn_[current_index_]] == current_index_)
|
|
{
|
|
ffn_[ffn_[current_index_]] = neighbor_update;
|
|
}
|
|
else if (sfn_[ffn_[current_index_]] == current_index_)
|
|
{
|
|
sfn_[ffn_[current_index_]] = neighbor_update;
|
|
}
|
|
|
|
/* updating current */
|
|
ffn_[current_index_] = prev_index;
|
|
|
|
break;
|
|
}
|
|
case 3://next2s:
|
|
{
|
|
addTriangle (current_index_, sfn_[current_index_], next_index, polygons);
|
|
auto neighbor_update = next_index;
|
|
|
|
/* updating next_index */
|
|
if (state_[next_index] <= FREE)
|
|
{
|
|
state_[next_index] = FRINGE;
|
|
ffn_[next_index] = current_index_;
|
|
sfn_[next_index] = sfn_[current_index_];
|
|
}
|
|
else
|
|
{
|
|
if (ffn_[next_index] == R_)
|
|
{
|
|
changed_2nd_fn_ = true;
|
|
ffn_[next_index] = sfn_[current_index_];
|
|
}
|
|
else if (sfn_[next_index] == R_)
|
|
{
|
|
changed_2nd_fn_ = true;
|
|
sfn_[next_index] = sfn_[current_index_];
|
|
}
|
|
else if (next_index == R_)
|
|
{
|
|
new2boundary_ = sfn_[current_index_];
|
|
if (next_next_index == new2boundary_)
|
|
already_connected_ = true;
|
|
}
|
|
else if (ffn_[next_index] == next_next_index)
|
|
{
|
|
already_connected_ = true;
|
|
ffn_[next_index] = sfn_[current_index_];
|
|
}
|
|
else if (sfn_[next_index] == next_next_index)
|
|
{
|
|
already_connected_ = true;
|
|
sfn_[next_index] = sfn_[current_index_];
|
|
}
|
|
else
|
|
{
|
|
tmp_ = coords_[ffn_[next_index]] - proj_qp_;
|
|
uvn_next_ffn_[0] = tmp_.dot(u_);
|
|
uvn_next_ffn_[1] = tmp_.dot(v_);
|
|
tmp_ = coords_[sfn_[next_index]] - proj_qp_;
|
|
uvn_next_sfn_[0] = tmp_.dot(u_);
|
|
uvn_next_sfn_[1] = tmp_.dot(v_);
|
|
|
|
bool ffn_next_sfn = isVisible(uvn_next_ffn_, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_next_ffn_, uvn_next, uvn_next_sfn_, uvn_sfn_);
|
|
bool sfn_next_sfn = isVisible(uvn_next_sfn_, uvn_next, uvn_current, uvn_sfn_) && isVisible(uvn_next_sfn_, uvn_next, uvn_next_ffn_, uvn_sfn_);
|
|
|
|
int connect2sfn = -1;
|
|
if (ffn_next_sfn && sfn_next_sfn)
|
|
{
|
|
double fn2s = (coords_[sfn_[current_index_]] - coords_[ffn_[next_index]]).squaredNorm ();
|
|
double sn2s = (coords_[sfn_[current_index_]] - coords_[sfn_[next_index]]).squaredNorm ();
|
|
if (fn2s < sn2s) connect2sfn = 0;
|
|
else connect2sfn = 1;
|
|
}
|
|
else if (ffn_next_sfn) connect2sfn = 0;
|
|
else if (sfn_next_sfn) connect2sfn = 1;
|
|
|
|
switch (connect2sfn)
|
|
{
|
|
case 0: // ffn[next]
|
|
{
|
|
addTriangle (next_index, sfn_[current_index_], ffn_[next_index], polygons);
|
|
neighbor_update = ffn_[next_index];
|
|
|
|
/* ffn[next_index] */
|
|
if ((ffn_[ffn_[next_index]] == sfn_[current_index_]) || (sfn_[ffn_[next_index]] == sfn_[current_index_]))
|
|
{
|
|
state_[ffn_[next_index]] = COMPLETED;
|
|
}
|
|
else if (ffn_[ffn_[next_index]] == next_index)
|
|
{
|
|
ffn_[ffn_[next_index]] = sfn_[current_index_];
|
|
}
|
|
else if (sfn_[ffn_[next_index]] == next_index)
|
|
{
|
|
sfn_[ffn_[next_index]] = sfn_[current_index_];
|
|
}
|
|
|
|
ffn_[next_index] = current_index_;
|
|
|
|
break;
|
|
}
|
|
case 1: // sfn[next]
|
|
{
|
|
addTriangle (next_index, sfn_[current_index_], sfn_[next_index], polygons);
|
|
neighbor_update = sfn_[next_index];
|
|
|
|
/* sfn[next_index] */
|
|
if ((ffn_[sfn_[next_index]] == sfn_[current_index_]) || (sfn_[sfn_[next_index]] == sfn_[current_index_]))
|
|
{
|
|
state_[sfn_[next_index]] = COMPLETED;
|
|
}
|
|
else if (ffn_[sfn_[next_index]] == next_index)
|
|
{
|
|
ffn_[sfn_[next_index]] = sfn_[current_index_];
|
|
}
|
|
else if (sfn_[sfn_[next_index]] == next_index)
|
|
{
|
|
sfn_[sfn_[next_index]] = sfn_[current_index_];
|
|
}
|
|
|
|
sfn_[next_index] = current_index_;
|
|
|
|
break;
|
|
}
|
|
default:;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* updating sfn */
|
|
if ((ffn_[sfn_[current_index_]] == neighbor_update) || (sfn_[sfn_[current_index_]] == neighbor_update))
|
|
{
|
|
state_[sfn_[current_index_]] = COMPLETED;
|
|
}
|
|
else if (ffn_[sfn_[current_index_]] == current_index_)
|
|
{
|
|
ffn_[sfn_[current_index_]] = neighbor_update;
|
|
}
|
|
else if (sfn_[sfn_[current_index_]] == current_index_)
|
|
{
|
|
sfn_[sfn_[current_index_]] = neighbor_update;
|
|
}
|
|
|
|
sfn_[current_index_] = prev_index;
|
|
|
|
break;
|
|
}
|
|
default:;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> std::vector<std::vector<std::size_t> >
|
|
pcl::GreedyProjectionTriangulation<PointInT>::getTriangleList (const pcl::PolygonMesh &input)
|
|
{
|
|
std::vector<std::vector<std::size_t> > triangleList (input.cloud.width * input.cloud.height);
|
|
|
|
for (std::size_t i=0; i < input.polygons.size (); ++i)
|
|
for (std::size_t j=0; j < input.polygons[i].vertices.size (); ++j)
|
|
triangleList[input.polygons[i].vertices[j]].push_back (i);
|
|
return (triangleList);
|
|
}
|
|
|
|
#define PCL_INSTANTIATE_GreedyProjectionTriangulation(T) \
|
|
template class PCL_EXPORTS pcl::GreedyProjectionTriangulation<T>;
|
|
|
|
#endif // PCL_SURFACE_IMPL_GP3_H_
|
|
|
|
|