template <typename PointInT, typename PointOutT> void
pcl::SHOTLocalReferenceFrameEstimation<PointInT, 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;
}
tree_->setSortedResults (true);
for (size_t i = 0; i < indices_->size (); ++i)
{
// point result
Eigen::Matrix3f rf;
PointOutT& output_rf = output[i];
//output_rf.confidence = getLocalRF ((*indices_)[i], rf);
//if (output_rf.confidence == std::numeric_limits<float>::max ())
if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output_rf.x_axis.getNormalVector3fMap () = rf.row (0);
output_rf.y_axis.getNormalVector3fMap () = rf.row (1);
output_rf.z_axis.getNormalVector3fMap () = rf.row (2);
}
}
void
pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
if (threads_ < 0)
threads_ = 1;
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeatureEigen] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str ());
return;
}
tree_->setSortedResults (true);
int data_size = static_cast<int> (indices_->size ());
// Set up the output channels
output.channels["shot_lrf"].name = "shot_lrf";
output.channels["shot_lrf"].offset = 0;
output.channels["shot_lrf"].size = 4;
output.channels["shot_lrf"].count = 9;
output.channels["shot_lrf"].datatype = sensor_msgs::PointField::FLOAT32;
//output.points.resize (indices_->size (), 10);
output.points.resize (data_size, 9);
#pragma omp parallel for num_threads(threads_)
for (int i = 0; i < data_size; ++i)
{
// point result
Eigen::Matrix3f rf;
//output.points (i, 9) = getLocalRF ((*indices_)[i], rf);
//if (output.points (i, 9) == std::numeric_limits<float>::max ())
if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output.points.block<1, 3> (i, 0) = rf.row (0);
output.points.block<1, 3> (i, 3) = rf.row (1);
output.points.block<1, 3> (i, 6) = rf.row (2);
}
}
void
pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
if (threads_ < 0)
threads_ = 1;
//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;
}
tree_->setSortedResults (true);
int data_size = static_cast<int> (indices_->size ());
#pragma omp parallel for num_threads(threads_)
for (int i = 0; i < data_size; ++i)
{
// point result
Eigen::Matrix3f rf;
PointOutT& output_rf = output[i];
//output_rf.confidence = getLocalRF ((*indices_)[i], rf);
//if (output_rf.confidence == std::numeric_limits<float>::max ())
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
this->searchForNeighbors ((*indices_)[i], search_parameter_, n_indices, n_sqr_distances);
if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output_rf.x_axis.getNormalVector3fMap () = rf.row (0);
output_rf.y_axis.getNormalVector3fMap () = rf.row (1);
output_rf.z_axis.getNormalVector3fMap () = rf.row (2);
}
}
