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