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); }