template <typename PointT> void
pcl::ProjectInliers<PointT>::applyFilter (PointCloud &output)
{
if (indices_->empty ())
{
PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
//Eigen::Map<Eigen::VectorXf, Eigen::Aligned> model_coefficients (&model_->values[0], model_->values.size ());
// More expensive than a map but safer (32bit architectures seem to complain)
Eigen::VectorXf model_coefficients (model_->values.size ());
for (size_t i = 0; i < model_->values.size (); ++i)
model_coefficients[i] = model_->values[i];
// Initialize the Sample Consensus model and set its parameters
if (!initSACModel (model_type_))
{
PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (copy_all_data_)
sacmodel_->projectPoints (*indices_, model_coefficients, output, true);
else
sacmodel_->projectPoints (*indices_, model_coefficients, output, false);
}
template <typename PointT> void
pcl16::SACSegmentation<PointT>::segment (PointIndices &inliers, ModelCoefficients &model_coefficients)
{
// Copy the header information
inliers.header = model_coefficients.header = input_->header;
if (!initCompute ())
{
inliers.indices.clear (); model_coefficients.values.clear ();
return;
}
// Initialize the Sample Consensus model and set its parameters
if (!initSACModel (model_type_))
{
PCL16_ERROR ("[pcl16::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
deinitCompute ();
inliers.indices.clear (); model_coefficients.values.clear ();
return;
}
// Initialize the Sample Consensus method and set its parameters
initSAC (method_type_);
if (!sac_->computeModel (0))
{
PCL16_ERROR ("[pcl16::%s::segment] Error segmenting the model! No solution found.\n", getClassName ().c_str ());
deinitCompute ();
inliers.indices.clear (); model_coefficients.values.clear ();
return;
}
// Get the model inliers
sac_->getInliers (inliers.indices);
// Get the model coefficients
Eigen::VectorXf coeff;
sac_->getModelCoefficients (coeff);
// If the user needs optimized coefficients
if (optimize_coefficients_)
{
Eigen::VectorXf coeff_refined;
model_->optimizeModelCoefficients (inliers.indices, coeff, coeff_refined);
model_coefficients.values.resize (coeff_refined.size ());
memcpy (&model_coefficients.values[0], &coeff_refined[0], coeff_refined.size () * sizeof (float));
// Refine inliers
model_->selectWithinDistance (coeff_refined, threshold_, inliers.indices);
}
else
{
model_coefficients.values.resize (coeff.size ());
memcpy (&model_coefficients.values[0], &coeff[0], coeff.size () * sizeof (float));
}
deinitCompute ();
}
void
pcl::ProjectInliers<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
if (indices_->empty ())
{
PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
//Eigen::Map<Eigen::VectorXf, Eigen::Aligned> model_coefficients (&model_->values[0], model_->values.size ());
// More expensive than a map but safer (32bit architectures seem to complain)
Eigen::VectorXf model_coefficients (model_->values.size ());
for (size_t i = 0; i < model_->values.size (); ++i)
model_coefficients[i] = model_->values[i];
// Construct the model and project
if (!initSACModel (model_type_))
{
PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
pcl::PointCloud<pcl::PointXYZ> cloud_out;
if (!copy_all_fields_)
sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, false);
else
sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, true);
if (copy_all_data_)
{
output.height = input_->height;
output.width = input_->width;
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.row_step = input_->row_step;
output.data = input_->data;
output.is_dense = input_->is_dense;
// Get the distance field index
int x_idx = -1, y_idx = -1, z_idx = -1;
for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d)
{
if (output.fields[d].name == "x") x_idx = d;
if (output.fields[d].name == "y") y_idx = d;
if (output.fields[d].name == "z") z_idx = d;
}
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
{
PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx);
output.width = output.height = 0;
output.data.clear ();
return;
}
// Copy the projected points
for (size_t i = 0; i < indices_->size (); ++i)
{
memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
}
}
else
{
if (!copy_all_fields_)
{
pcl::toPCLPointCloud2<pcl::PointXYZ> (cloud_out, output);
}
else
{
// Copy everything
output.height = 1;
output.width = static_cast<uint32_t> (indices_->size ());
output.point_step = input_->point_step;
output.data.resize (output.width * output.point_step);
output.is_bigendian = input_->is_bigendian;
output.row_step = output.point_step * output.width;
// All projections should return valid data, so is_dense = true
output.is_dense = true;
// Get the distance field index
int x_idx = -1, y_idx = -1, z_idx = -1;
for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d)
{
if (output.fields[d].name == "x") x_idx = d;
if (output.fields[d].name == "y") y_idx = d;
if (output.fields[d].name == "z") z_idx = d;
}
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
{
PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx);
output.width = output.height = 0;
output.data.clear ();
return;
}
// Copy the projected points
for (size_t i = 0; i < indices_->size (); ++i)
{
memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * input_->point_step], output.point_step);
memcpy (&output.data[i * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
memcpy (&output.data[i * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
memcpy (&output.data[i * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
}
}
}
}
template <typename PointT> void
pcl::ModelOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
//The arrays to be used
indices.resize (indices_->size ());
removed_indices_->resize (indices_->size ());
int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
//is the filtersetup correct?
bool valid_setup = true;
valid_setup &= initSACModel (model_type_);
typedef SampleConsensusModelFromNormals<PointT, pcl::Normal> SACModelFromNormals;
// Returns NULL if cast isn't possible
SACModelFromNormals *model_from_normals = dynamic_cast<SACModelFromNormals *> (& (*model_));
if (model_from_normals)
{
if (!cloud_normals_)
{
valid_setup = false;
PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n");
}
else
{
model_from_normals->setNormalDistanceWeight (normals_distance_weight_);
model_from_normals->setInputNormals (cloud_normals_);
}
}
//if the filter setup is invalid filter for nan and return;
if (!valid_setup)
{
for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
{
// Non-finite entries are always passed to removed indices
if (!isFinite (input_->points[ (*indices_)[iii]]))
{
if (extract_removed_indices_)
(*removed_indices_)[rii++] = (*indices_)[iii];
continue;
}
indices[oii++] = (*indices_)[iii];
}
return;
}
// check distance of pointcloud to model
std::vector<double> distances;
//TODO: get signed distances !
model_->setIndices(indices_); // added to reduce computation and arrange distances with indices
model_->getDistancesToModel (model_coefficients_, distances);
bool thresh_result;
// Filter for non-finite entries and the specified field limits
for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
{
// Non-finite entries are always passed to removed indices
if (!isFinite (input_->points[ (*indices_)[iii]]))
{
if (extract_removed_indices_)
(*removed_indices_)[rii++] = (*indices_)[iii];
continue;
}
// use threshold function to separate outliers from inliers:
thresh_result = threshold_function_ (distances[iii]);
// in normal mode: define outliers as false thresh_result
if (!negative_ && !thresh_result)
{
if (extract_removed_indices_)
(*removed_indices_)[rii++] = (*indices_)[iii];
continue;
}
// in negative_ mode: define outliers as true thresh_result
if (negative_ && thresh_result)
{
if (extract_removed_indices_)
(*removed_indices_)[rii++] = (*indices_)[iii];
continue;
}
// Otherwise it was a normal point for output (inlier)
indices[oii++] = (*indices_)[iii];
}
// Resize the output arrays
indices.resize (oii);
removed_indices_->resize (rii);
}
