pcl::ihs::ICP::CloudNormalConstPtr
pcl::ihs::ICP::selectModelPoints (const CloudModelConstPtr& cloud_model,
const Transformation& T_init_inv) const
{
const CloudNormalPtr cloud_model_out (new CloudNormal ());
cloud_model_out->reserve (cloud_model->size ());
CloudModel::const_iterator it_in = cloud_model->begin ();
for (; it_in!=cloud_model->end (); ++it_in)
{
// Don't consider points that are facing away from the cameara.
if ((T_init_inv * it_in->getNormalVector4fMap ()).z () < 0.f)
{
PointNormal pt;
pt.getVector4fMap () = it_in->getVector4fMap ();
pt.getNormalVector4fMap () = it_in->getNormalVector4fMap ();
// NOTE: Not the transformed points!!
cloud_model_out->push_back (pt);
}
}
// Shrink to fit ("Scott Meyers swap trick")
CloudNormal (*cloud_model_out).swap (*cloud_model_out);
return (cloud_model_out);
}
pcl::ihs::ICP::CloudNormalConstPtr
pcl::ihs::ICP::selectDataPoints (const CloudProcessedConstPtr& cloud_data) const
{
const CloudNormalPtr cloud_data_out (new CloudNormal ());
cloud_data_out->reserve (cloud_data->size ());
CloudProcessed::const_iterator it_in = cloud_data->begin ();
for (; it_in!=cloud_data->end (); ++it_in)
{
if (pcl::isFinite (*it_in))
{
PointNormal pt;
pt.getVector4fMap () = it_in->getVector4fMap ();
pt.getNormalVector4fMap () = it_in->getNormalVector4fMap ();
cloud_data_out->push_back (pt);
}
}
// Shrink to fit ("Scott Meyers swap trick")
CloudNormal (*cloud_data_out).swap (*cloud_data_out);
return (cloud_data_out);
}
