//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);
Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
{
Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
if (pt.head<3> () == central_point.head<3> ())
continue;
// Difference between current point and origin
vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
vij (valid_nn_points, 3) = 0;
distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);
// Multiply vij * vij'
cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());
sum += distance;
valid_nn_points++;
}
if (valid_nn_points < 5)
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
cov_m /= sum;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
const double& e1c = solver.eigenvalues ()[0];
const double& e2c = solver.eigenvalues ()[1];
const double& e3c = solver.eigenvalues ()[2];
if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
// Disambiguation
Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
v1.head<3> () = solver.eigenvectors ().col (2);
v3.head<3> () = solver.eigenvectors ().col (0);
int plusNormal = 0, plusTangentDirection1=0;
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij.row (ne).dot (v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij.row (ne).dot (v3);
if (dp >= 0)
plusNormal++;
}
//TANGENT
plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
if (plusTangentDirection1 == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v1) > 0)
plusTangentDirection1 ++;
if (plusTangentDirection1 < points/2+1)
v1 *= - 1;
} else if (plusTangentDirection1 < 0)
v1 *= - 1;
//Normal
plusNormal = 2*plusNormal - valid_nn_points;
if (plusNormal == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v3) > 0)
plusNormal ++;
if (plusNormal < points/2+1)
v3 *= - 1;
} else if (plusNormal < 0)
v3 *= - 1;
rf.row (0) = v1.head<3> ().cast<float> ();
rf.row (2) = v3.head<3> ().cast<float> ();
rf.row (1) = rf.row (2).cross (rf.row (0));
return (0.0f);
}
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
std::vector<int> neighbours_indices;
std::vector<float> neighbours_distances;
int 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 (int 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;
float radius2 = tangent_radius_ * tangent_radius_;
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;
for (int i = 0; i < check_margin_array_size_; i++)
{
check_margin_array_[i] = false;
margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
margin_array_min_angle_normal_[i] = -1.0;
margin_array_max_angle_normal_[i] = -1.0;
}
max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
}
for (int 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> (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 (int 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 (int i = 0; i < check_margin_array_size_; i++)
{
if (!check_margin_array_[i])
{
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;
//find first no border pie
int first_no_border = -1;
if (check_margin_array_[check_margin_array_size_ - 1])
{
first_no_border = 0;
}
else
{
for (int i = 0; i < check_margin_array_size_; i++)
{
if (check_margin_array_[i])
{
first_no_border = i;
break;
}
}
}
//float steep_prob = 0.0;
float max_hole_prob = -std::numeric_limits<float>::max ();
//find holes
for (int ch = first_no_border; ch < 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;
}
else
{
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);
}