jerryzeng/thirdParty
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
thirdParty/PCL 1.12.0/include/pcl-1.12/pcl/features/impl/board.hpp

605 lines
23 KiB
C++
Raw Permalink Blame History

/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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.
*
*
*/
#ifndef PCL_FEATURES_IMPL_BOARD_H_
#define PCL_FEATURES_IMPL_BOARD_H_
#include <pcl/features/board.h>
#include <utility>
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::directedOrthogonalAxis (
Eigen::Vector3f const &axis,
Eigen::Vector3f const &axis_origin,
Eigen::Vector3f const &point,
Eigen::Vector3f &directed_ortho_axis)
{
Eigen::Vector3f projection;
projectPointOnPlane (point, axis_origin, axis, projection);
directed_ortho_axis = projection - axis_origin;
directed_ortho_axis.normalize ();
// check if the computed x axis is orthogonal to the normal
//assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::projectPointOnPlane (
Eigen::Vector3f const &point,
Eigen::Vector3f const &origin_point,
Eigen::Vector3f const &plane_normal,
Eigen::Vector3f &projected_point)
{
float t;
Eigen::Vector3f xo;
xo = point - origin_point;
t = plane_normal.dot (xo);
projected_point = point - (t * plane_normal);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> float
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::getAngleBetweenUnitVectors (
Eigen::Vector3f const &v1,
Eigen::Vector3f const &v2,
Eigen::Vector3f const &axis)
{
Eigen::Vector3f angle_orientation;
angle_orientation = v1.cross (v2);
float angle_radians = std::acos (std::max (-1.0f, std::min (1.0f, v1.dot (v2))));
angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast<float> (M_PI) - angle_radians) : angle_radians;
return (angle_radians);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::randomOrthogonalAxis (
Eigen::Vector3f const &axis,
Eigen::Vector3f &rand_ortho_axis)
{
if (!areEquals (axis.z (), 0.0f))
{
rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
}
else if (!areEquals (axis.y (), 0.0f))
{
rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
}
else if (!areEquals (axis.x (), 0.0f))
{
rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
}
rand_ortho_axis.normalize ();
// check if the computed x axis is orthogonal to the normal
//assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::planeFitting (
Eigen::Matrix<float,
Eigen::Dynamic, 3> const &points,
Eigen::Vector3f &center,
Eigen::Vector3f &norm)
{
// -----------------------------------------------------
// Plane Fitting using Singular Value Decomposition (SVD)
// -----------------------------------------------------
const auto n_points = points.rows ();
if (n_points == 0)
{
return;
}
//find the center by averaging the points positions
center = points.colwise().mean().transpose();
//copy points - average (center)
const Eigen::Matrix<float, Eigen::Dynamic, 3> A = points.rowwise() - center.transpose();
Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
norm = svd.matrixV ().col (2);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::normalDisambiguation (
pcl::PointCloud<PointNT> const &normal_cloud,
pcl::Indices const &normal_indices,
Eigen::Vector3f &normal)
{
Eigen::Vector3f normal_mean;
normal_mean.setZero ();
for (const auto &normal_index : normal_indices)
{
const PointNT& curPt = normal_cloud[normal_index];
normal_mean += curPt.getNormalVector3fMap ();
}
normal_mean.normalize ();
if (normal.dot (normal_mean) < 0)
{
normal = -normal;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> float
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computePointLRF (const int &index,
Eigen::Matrix3f &lrf)
{
//find Z axis
//extract support points for Rz radius
pcl::Indices neighbours_indices;
std::vector<float> neighbours_distances;
std::size_t n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
//check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
if (n_neighbours < 6)
{
//PCL_WARN(
// "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n",
// getClassName().c_str(), index);
//setting lrf to NaN
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//copy neighbours coordinates into eigen matrix
Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
for (std::size_t i = 0; i < n_neighbours; ++i)
{
neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
}
Eigen::Vector3f x_axis, y_axis;
//plane fitting to find direction of Z axis
Eigen::Vector3f fitted_normal; //z_axis
Eigen::Vector3f centroid;
planeFitting (neigh_points_mat, centroid, fitted_normal);
//disambiguate Z axis with normal mean
normalDisambiguation (*normals_, neighbours_indices, fitted_normal);
//setting LRF Z axis
lrf.row (2).matrix () = fitted_normal;
/////////////////////////////////////////////////////////////////////////////////////////
//find X axis
//extract support points for Rx radius
if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
{
n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
}
//find point with the "most different" normal (with respect to fittedNormal)
float min_normal_cos = std::numeric_limits<float>::max ();
int min_normal_index = -1;
bool margin_point_found = false;
Eigen::Vector3f best_margin_point;
bool best_point_found_on_margins = false;
const float radius2 = tangent_radius_ * tangent_radius_;
const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
float max_boundary_angle = 0;
if (find_holes_)
{
randomOrthogonalAxis (fitted_normal, x_axis);
lrf.row (0).matrix () = x_axis;
check_margin_array_.assign(check_margin_array_size_, false);
margin_array_min_angle_.assign(check_margin_array_size_, std::numeric_limits<float>::max ());
margin_array_max_angle_.assign(check_margin_array_size_, -std::numeric_limits<float>::max ());
margin_array_min_angle_normal_.assign(check_margin_array_size_, -1.0);
margin_array_max_angle_normal_.assign(check_margin_array_size_, -1.0);
max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
}
for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
{
const int& curr_neigh_idx = neighbours_indices[curr_neigh];
const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
if (neigh_distance_sqr <= margin_distance2)
{
continue;
}
//point normalIndex is inside the ring between marginThresh and Radius
margin_point_found = true;
Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
float normal_cos = fitted_normal.dot (normal_mean);
if (normal_cos < min_normal_cos)
{
min_normal_index = curr_neigh_idx;
min_normal_cos = normal_cos;
best_point_found_on_margins = false;
}
if (find_holes_)
{
//find angle with respect to random axis previously calculated
Eigen::Vector3f indicating_normal_vect;
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);
int check_margin_array_idx = std::min (static_cast<int> (std::floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
check_margin_array_[check_margin_array_idx] = true;
if (angle < margin_array_min_angle_[check_margin_array_idx])
{
margin_array_min_angle_[check_margin_array_idx] = angle;
margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
}
if (angle > margin_array_max_angle_[check_margin_array_idx])
{
margin_array_max_angle_[check_margin_array_idx] = angle;
margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
}
}
} //for each neighbor
if (!margin_point_found)
{
//find among points with neighDistance <= marginThresh*radius
for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
{
const int& curr_neigh_idx = neighbours_indices[curr_neigh];
const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
if (neigh_distance_sqr > margin_distance2)
continue;
Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
float normal_cos = fitted_normal.dot (normal_mean);
if (normal_cos < min_normal_cos)
{
min_normal_index = curr_neigh_idx;
min_normal_cos = normal_cos;
}
}//for each neighbor
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
if (!find_holes_)
{
if (best_point_found_on_margins)
{
//if most inclined normal is on support margin
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}// if(!find_holes_)
//check if there is at least a hole
bool is_hole_present = false;
for (const auto check_margin: check_margin_array_)
{
if (!check_margin)
{
is_hole_present = true;
break;
}
}
if (!is_hole_present)
{
if (best_point_found_on_margins)
{
//if most inclined normal is on support margin
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}//if (!is_hole_present)
//case hole found
//find missing region
float angle = 0.0;
int hole_end;
int hole_first;
const auto find_first_no_border_pie = [](const auto& array) -> std::size_t {
if (array.back())
{
return 0;
}
const auto result = std::find_if(array.cbegin (), array.cend (),
[](const auto& x) -> bool { return x;});
return std::distance(array.cbegin (), result);
};
const auto first_no_border = find_first_no_border_pie(check_margin_array_);
//float steep_prob = 0.0;
float max_hole_prob = -std::numeric_limits<float>::max ();
//find holes
for (auto ch = first_no_border; ch < static_cast<std::size_t>(check_margin_array_size_); ch++)
{
if (!check_margin_array_[ch])
{
//border beginning found
hole_first = ch;
hole_end = hole_first + 1;
while (!check_margin_array_[hole_end % check_margin_array_size_])
{
++hole_end;
}
//border end found, find angle
if ((hole_end - hole_first) > 0)
{
//check if hole can be a shapeness hole
int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
% check_margin_array_size_;
int following_hole = (hole_end) % check_margin_array_size_;
float normal_begin = margin_array_max_angle_normal_[previous_hole];
float normal_end = margin_array_min_angle_normal_[following_hole];
normal_begin -= min_normal_cos;
normal_end -= min_normal_cos;
normal_begin = normal_begin / (1.0f - min_normal_cos);
normal_end = normal_end / (1.0f - min_normal_cos);
normal_begin = 1.0f - normal_begin;
normal_end = 1.0f - normal_end;
//evaluate P(Hole);
float hole_width = 0.0f;
if (following_hole < previous_hole)
{
hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI)
- margin_array_max_angle_[previous_hole];
}
else
{
hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
}
float hole_prob = hole_width / (2 * static_cast<float> (M_PI));
//evaluate P(zmin|Hole)
float steep_prob = (normal_end + normal_begin) / 2.0f;
//check hole prob and after that, check steepThresh
if (hole_prob > hole_size_prob_thresh_)
{
if (steep_prob > steep_thresh_)
{
if (hole_prob > max_hole_prob)
{
max_hole_prob = hole_prob;
float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
if (following_hole < previous_hole)
{
angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
* static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
}
else
{
angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
- margin_array_max_angle_[previous_hole]) * angle_weight;
}
}
}
}
} //(hole_end-hole_first) > 0
if (hole_end >= check_margin_array_size_)
{
break;
}
ch = hole_end - 1;
}
}
if (max_hole_prob > -std::numeric_limits<float>::max ())
{
//hole found
Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
x_axis = rotation * x_axis;
min_normal_cos -= 10.0f;
}
else
{
if (best_point_found_on_margins)
{
//if most inclined normal is on support margin
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
}
else
{
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
}
}
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str());
return;
}
this->resetData ();
for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
{
Eigen::Matrix3f currentLrf;
PointOutT &rf = output[point_idx];
//rf.confidence = computePointLRF (*indices_[point_idx], currentLrf);
//if (rf.confidence == std::numeric_limits<float>::max ())
if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
for (int d = 0; d < 3; ++d)
{
rf.x_axis[d] = currentLrf (0, d);
rf.y_axis[d] = currentLrf (1, d);
rf.z_axis[d] = currentLrf (2, d);
}
}
}
#define PCL_INSTANTIATE_BOARDLocalReferenceFrameEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::BOARDLocalReferenceFrameEstimation<T,NT,OutT>;
#endif // PCL_FEATURES_IMPL_BOARD_H_
Reference in New Issue View Git Blame Copy Permalink