template <typename PointT, typename PointNT> bool pcl16::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type) { if (!input_ || !normals_) { PCL16_ERROR ("[pcl16::%s::initSACModel] Input data (XYZ or normals) not given! Cannot continue.\n", getClassName ().c_str ()); return (false); } // Check if input is synced with the normals if (input_->points.size () != normals_->points.size ()) { PCL16_ERROR ("[pcl16::%s::initSACModel] The number of points inthe input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ()); return (false); } if (model_) model_.reset (); // Build the model switch (model_type) { case SACMODEL_CYLINDER: { PCL16_DEBUG ("[pcl16::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_)); typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_); // Set the input normals model_cylinder->setInputNormals (normals_); double min_radius, max_radius; model_cylinder->getRadiusLimits (min_radius, max_radius); if (radius_min_ != min_radius && radius_max_ != max_radius) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); model_cylinder->setRadiusLimits (radius_min_, radius_max_); } if (distance_weight_ != model_cylinder->getNormalDistanceWeight ()) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_cylinder->setNormalDistanceWeight (distance_weight_); } if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); model_cylinder->setAxis (axis_); } if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); model_cylinder->setEpsAngle (eps_angle_); } break; } case SACMODEL_NORMAL_PLANE: { PCL16_DEBUG ("[pcl16::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_)); typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_); // Set the input normals model_normals->setInputNormals (normals_); if (distance_weight_ != model_normals->getNormalDistanceWeight ()) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_normals->setNormalDistanceWeight (distance_weight_); } break; } case SACMODEL_NORMAL_PARALLEL_PLANE: { PCL16_DEBUG ("[pcl16::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_)); typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_); // Set the input normals model_normals->setInputNormals (normals_); if (distance_weight_ != model_normals->getNormalDistanceWeight ()) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_normals->setNormalDistanceWeight (distance_weight_); } if (distance_from_origin_ != model_normals->getDistanceFromOrigin ()) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the distance to origin to %f\n", getClassName ().c_str (), distance_from_origin_); model_normals->setDistanceFromOrigin (distance_from_origin_); } if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); model_normals->setAxis (axis_); } if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); model_normals->setEpsAngle (eps_angle_); } break; } case SACMODEL_CONE: { PCL16_DEBUG ("[pcl16::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCone<PointT, PointNT > (input_, *indices_)); typename SampleConsensusModelCone<PointT, PointNT>::Ptr model_cone = boost::static_pointer_cast<SampleConsensusModelCone<PointT, PointNT> > (model_); // Set the input normals model_cone->setInputNormals (normals_); double min_angle, max_angle; model_cone->getMinMaxOpeningAngle(min_angle, max_angle); if (min_angle_ != min_angle && max_angle_ != max_angle) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting minimum and maximum opening angle to %f and %f \n", getClassName ().c_str (), min_angle_, max_angle_); model_cone->setMinMaxOpeningAngle (min_angle_, max_angle_); } if (distance_weight_ != model_cone->getNormalDistanceWeight ()) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_cone->setNormalDistanceWeight (distance_weight_); } if (axis_ != Eigen::Vector3f::Zero () && model_cone->getAxis () != axis_) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); model_cone->setAxis (axis_); } if (eps_angle_ != 0.0 && model_cone->getEpsAngle () != eps_angle_) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); model_cone->setEpsAngle (eps_angle_); } break; } case SACMODEL_NORMAL_SPHERE: { PCL16_DEBUG ("[pcl16::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalSphere<PointT, PointNT> (input_, *indices_)); typename SampleConsensusModelNormalSphere<PointT, PointNT>::Ptr model_normals_sphere = boost::static_pointer_cast<SampleConsensusModelNormalSphere<PointT, PointNT> > (model_); // Set the input normals model_normals_sphere->setInputNormals (normals_); double min_radius, max_radius; model_normals_sphere->getRadiusLimits (min_radius, max_radius); if (radius_min_ != min_radius && radius_max_ != max_radius) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); model_normals_sphere->setRadiusLimits (radius_min_, radius_max_); } if (distance_weight_ != model_normals_sphere->getNormalDistanceWeight ()) { PCL16_DEBUG ("[pcl16::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_normals_sphere->setNormalDistanceWeight (distance_weight_); } break; } // If nothing else, try SACSegmentation default: { return (pcl16::SACSegmentation<PointT>::initSACModel (model_type)); } } if (SACSegmentation<PointT>::samples_radius_ > 0. ) { PCL16_DEBUG ("[pcl16::%s::initSAC] Setting the maximum distance to %f\n", getClassName ().c_str (), SACSegmentation<PointT>::samples_radius_); // Set maximum distance for radius search during random sampling model_->setSamplesMaxDist(SACSegmentation<PointT>::samples_radius_, SACSegmentation<PointT>::samples_radius_search_); } return (true); }
template <typename PointT, typename PointNT> bool pcl::SACSegmentationFromNormals<PointT, PointNT>::initSACModel (const int model_type) { // Check if input is synced with the normals if (input_->points.size () != normals_->points.size ()) { PCL_ERROR ("[pcl::%s::initSACModel] The number of points inthe input point cloud differs than the number of points in the normals!\n", getClassName ().c_str ()); return (false); } if (model_) model_.reset (); // Build the model switch (model_type) { case SACMODEL_CYLINDER: { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCylinder<PointT, PointNT > (input_, *indices_)); typename SampleConsensusModelCylinder<PointT, PointNT>::Ptr model_cylinder = boost::static_pointer_cast<SampleConsensusModelCylinder<PointT, PointNT> > (model_); // Set the input normals model_cylinder->setInputNormals (normals_); double min_radius, max_radius; model_cylinder->getRadiusLimits (min_radius, max_radius); if (radius_min_ != min_radius && radius_max_ != max_radius) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f\n", getClassName ().c_str (), radius_min_, radius_max_); model_cylinder->setRadiusLimits (radius_min_, radius_max_); } if (distance_weight_ != model_cylinder->getNormalDistanceWeight ()) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_cylinder->setNormalDistanceWeight (distance_weight_); } if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); model_cylinder->setAxis (axis_); } if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); model_cylinder->setEpsAngle (eps_angle_); } break; } case SACMODEL_NORMAL_PLANE: { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalPlane<PointT, PointNT> (input_, *indices_)); typename SampleConsensusModelNormalPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalPlane<PointT, PointNT> > (model_); // Set the input normals model_normals->setInputNormals (normals_); if (distance_weight_ != model_normals->getNormalDistanceWeight ()) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_normals->setNormalDistanceWeight (distance_weight_); } break; } case SACMODEL_NORMAL_PARALLEL_PLANE: { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, PointNT> (input_, *indices_)); typename SampleConsensusModelNormalParallelPlane<PointT, PointNT>::Ptr model_normals = boost::static_pointer_cast<SampleConsensusModelNormalParallelPlane<PointT, PointNT> > (model_); // Set the input normals model_normals->setInputNormals (normals_); if (distance_weight_ != model_normals->getNormalDistanceWeight ()) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f\n", getClassName ().c_str (), distance_weight_); model_normals->setNormalDistanceWeight (distance_weight_); } if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); model_normals->setAxis (axis_); } if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)\n", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI); model_normals->setEpsAngle (eps_angle_); } break; } // If nothing else, try SACSegmentation default: { return (pcl::SACSegmentation<PointT>::initSACModel (model_type)); } } return (true); }