sac_segmentation.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: sac_segmentation.hpp 35810 2011-02-08 00:03:46Z rusu $
 *
 */

#ifndef PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_
#define PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_

#include "pcl/segmentation/sac_segmentation.h"

#include "pcl/io/io.h"

// Sample Consensus methods
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/sac.h"
#include "pcl/sample_consensus/lmeds.h"
#include "pcl/sample_consensus/mlesac.h"
#include "pcl/sample_consensus/msac.h"
#include "pcl/sample_consensus/ransac.h"
#include "pcl/sample_consensus/rmsac.h"
#include "pcl/sample_consensus/rransac.h"

// Sample Consensus models
#include "pcl/sample_consensus/model_types.h"
#include "pcl/sample_consensus/sac_model.h"
#include "pcl/sample_consensus/sac_model_circle.h"
#include "pcl/sample_consensus/sac_model_cylinder.h"
#include "pcl/sample_consensus/sac_model_line.h"
#include "pcl/sample_consensus/sac_model_normal_plane.h"
#include "pcl/sample_consensus/sac_model_parallel_plane.h"
#include "pcl/sample_consensus/sac_model_normal_parallel_plane.h"
#include "pcl/sample_consensus/sac_model_parallel_line.h"
#include "pcl/sample_consensus/sac_model_perpendicular_plane.h"
#include "pcl/sample_consensus/sac_model_plane.h"
#include "pcl/sample_consensus/sac_model_sphere.h"


template <typename PointT> void
pcl::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_))
  {
    ROS_ERROR ("[pcl::%s::segment] Error initializing the SAC model!", 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))
  {
    ROS_ERROR ("[pcl::%s::segment] Error segmenting the model! No solution found.", 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 ();
}

template <typename PointT> bool
pcl::SACSegmentation<PointT>::initSACModel (const int model_type)
{
  if (model_)
    model_.reset ();

  // Build the model
  switch (model_type)
  {
    case SACMODEL_PLANE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelPlane<PointT> (input_, *indices_));
      break;
    }
    case SACMODEL_LINE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelLine<PointT> (input_, *indices_));
      break;
    }
    case SACMODEL_CIRCLE2D:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelCircle2D<PointT> (input_, *indices_));
      typename SampleConsensusModelCircle2D<PointT>::Ptr model_circle = boost::static_pointer_cast<SampleConsensusModelCircle2D<PointT> > (model_);
      double min_radius, max_radius;
      model_circle->getRadiusLimits (min_radius, max_radius);
      if (radius_min_ != min_radius && radius_max_ != max_radius)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f", getClassName ().c_str (), radius_min_, radius_max_);
        model_circle->setRadiusLimits (radius_min_, radius_max_);
      }
      break;
    }
    case SACMODEL_SPHERE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelSphere<PointT> (input_, *indices_));
      typename SampleConsensusModelSphere<PointT>::Ptr model_sphere = boost::static_pointer_cast<SampleConsensusModelSphere<PointT> > (model_);
      double min_radius, max_radius;
      model_sphere->getRadiusLimits (min_radius, max_radius);
      if (radius_min_ != min_radius && radius_max_ != max_radius)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f", getClassName ().c_str (), radius_min_, radius_max_);
        model_sphere->setRadiusLimits (radius_min_, radius_max_);
      }
      break;
    }
    case SACMODEL_PARALLEL_LINE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelParallelLine<PointT> (input_, *indices_));
      typename SampleConsensusModelParallelLine<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelLine<PointT> > (model_);
      if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
        model_parallel->setAxis (axis_);
      }
      if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
        model_parallel->setEpsAngle (eps_angle_);
      }
      break;
    }
    case SACMODEL_PERPENDICULAR_PLANE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_, *indices_));
      typename SampleConsensusModelPerpendicularPlane<PointT>::Ptr model_perpendicular = boost::static_pointer_cast<SampleConsensusModelPerpendicularPlane<PointT> > (model_);
      if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
        model_perpendicular->setAxis (axis_);
      }
      if (eps_angle_ != 0.0 && model_perpendicular->getEpsAngle () != eps_angle_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
        model_perpendicular->setEpsAngle (eps_angle_);
      }
      break;
    }
    case SACMODEL_PARALLEL_PLANE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE", getClassName ().c_str ());
      model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_, *indices_));
      typename SampleConsensusModelParallelPlane<PointT>::Ptr model_parallel = boost::static_pointer_cast<SampleConsensusModelParallelPlane<PointT> > (model_);
      if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
        model_parallel->setAxis (axis_);
      }
      if (eps_angle_ != 0.0 && model_parallel->getEpsAngle () != eps_angle_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
        model_parallel->setEpsAngle (eps_angle_);
      }
      break;
    }
    default:
    {
      ROS_ERROR ("[pcl::%s::initSACModel] No valid model given!", getClassName ().c_str ());
      return (false);
    }
  }
  return (true);
}

template <typename PointT> void
pcl::SACSegmentation<PointT>::initSAC (const int method_type)
{
  if (sac_)
    sac_.reset ();
  // Build the sample consensus method
  switch (method_type)
  {
    case SAC_RANSAC:
    default:
    {
      ROS_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RANSAC with a model threshold of %f", getClassName ().c_str (), threshold_);
      sac_.reset (new RandomSampleConsensus<PointT> (model_, threshold_));
      break;
    }
    case SAC_LMEDS:
    {
      ROS_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_LMEDS with a model threshold of %f", getClassName ().c_str (), threshold_);
      sac_.reset (new LeastMedianSquares<PointT> (model_, threshold_));
      break;
    }
    case SAC_MSAC:
    {
      ROS_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MSAC with a model threshold of %f", getClassName ().c_str (), threshold_);
      sac_.reset (new MEstimatorSampleConsensus<PointT> (model_, threshold_));
      break;
    }
    case SAC_RRANSAC:
    {
      ROS_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RRANSAC with a model threshold of %f", getClassName ().c_str (), threshold_);
      sac_.reset (new RandomizedRandomSampleConsensus<PointT> (model_, threshold_));
      break;
    }
    case SAC_RMSAC:
    {
      ROS_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_RMSAC with a model threshold of %f", getClassName ().c_str (), threshold_);
      sac_.reset (new RandomizedMEstimatorSampleConsensus<PointT> (model_, threshold_));
      break;
    }
    case SAC_MLESAC:
    {
      ROS_DEBUG ("[pcl::%s::initSAC] Using a method of type: SAC_MLESAC with a model threshold of %f", getClassName ().c_str (), threshold_);
      sac_.reset (new MaximumLikelihoodSampleConsensus<PointT> (model_, threshold_));
      break;
    }
  }
  // Set the Sample Consensus parameters if they are given/changed
  if (sac_->getProbability () != probability_)
  {
    ROS_DEBUG ("[pcl::%s::initSAC] Setting the desired probability to %f", getClassName ().c_str (), probability_);
    sac_->setProbability (probability_);
  }
  if (max_iterations_ != -1 && sac_->getMaxIterations () != max_iterations_)
  {
    ROS_DEBUG ("[pcl::%s::initSAC] Setting the maximum number of iterations to %d", getClassName ().c_str (), max_iterations_);
    sac_->setMaxIterations (max_iterations_);
  }
}

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 ())
  {
    ROS_ERROR ("[pcl::%s::initSACModel] The number of points inthe input point cloud differs than the number of points in the normals!", getClassName ().c_str ());
    return (false);
  }

  if (model_)
    model_.reset ();

  // Build the model
  switch (model_type)
  {
    case SACMODEL_CYLINDER:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER", 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)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting radius limits to %f/%f", getClassName ().c_str (), radius_min_, radius_max_);
        model_cylinder->setRadiusLimits (radius_min_, radius_max_);
      }
      if (distance_weight_ != model_cylinder->getNormalDistanceWeight ())
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f", getClassName ().c_str (), distance_weight_);
        model_cylinder->setNormalDistanceWeight (distance_weight_);
      }
      if (axis_ != Eigen::Vector3f::Zero () && model_cylinder->getAxis () != axis_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
        model_cylinder->setAxis (axis_);
      }
      if (eps_angle_ != 0.0 && model_cylinder->getEpsAngle () != eps_angle_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
        model_cylinder->setEpsAngle (eps_angle_);
      }
      break;
    }
    case SACMODEL_NORMAL_PLANE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE", 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 ())
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f", getClassName ().c_str (), distance_weight_);
        model_normals->setNormalDistanceWeight (distance_weight_);
      }
      if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
        model_normals->setAxis (axis_);
      }
      if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)", getClassName ().c_str (), eps_angle_, eps_angle_ * 180.0 / M_PI);
        model_normals->setEpsAngle (eps_angle_);
      }
      break;
    }
    case SACMODEL_NORMAL_PARALLEL_PLANE:
    {
      ROS_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE", 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 ())
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting normal distance weight to %f", getClassName ().c_str (), distance_weight_);
        model_normals->setNormalDistanceWeight (distance_weight_);
      }
      if (axis_ != Eigen::Vector3f::Zero () && model_normals->getAxis () != axis_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]);
        model_normals->setAxis (axis_);
      }
      if (eps_angle_ != 0.0 && model_normals->getEpsAngle () != eps_angle_)
      {
        ROS_DEBUG ("[pcl::%s::initSACModel] Setting the epsilon angle to %f (%f degrees)", 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);
}

#define PCL_INSTANTIATE_SACSegmentation(T) template class pcl::SACSegmentation<T>;
#define PCL_INSTANTIATE_SACSegmentationFromNormals(T,NT) template class pcl::SACSegmentationFromNormals<T,NT>;

#endif        // PCL_SEGMENTATION_IMPL_SAC_SEGMENTATION_H_

---

pcl
Author(s): See http://pcl.ros.org/authors for the complete list of authors.
autogenerated on Fri Jan 11 09:55:23 2013