template <typename PointT> void
pcl16::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_))
  {
    PCL16_ERROR ("[pcl16::%s::segment] Error initializing the SAC model!\n", 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))
  {
    PCL16_ERROR ("[pcl16::%s::segment] Error segmenting the model! No solution found.\n", 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> void
pcl::SupervoxelClustering<PointT>::extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
{
  //timer_.reset ();
  //double t_start = timer_.getTime ();
  //std::cout << "Init compute  \n";
  bool segmentation_is_possible = initCompute ();
  if ( !segmentation_is_possible )
  {
    deinitCompute ();
    return;
  }
  
  //std::cout << "Preparing for segmentation \n";
  segmentation_is_possible = prepareForSegmentation ();
  if ( !segmentation_is_possible )
  {
    deinitCompute ();
    return;
  }
  
  //double t_prep = timer_.getTime ();
  //std::cout << "Placing Seeds" << std::endl;
  std::vector<int> seed_indices;
  selectInitialSupervoxelSeeds (seed_indices);
  //std::cout << "Creating helpers "<<std::endl;
  createSupervoxelHelpers (seed_indices);
  //double t_seeds = timer_.getTime ();
  
  
  //std::cout << "Expanding the supervoxels" << std::endl;
  int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
  expandSupervoxels (max_depth);
  //double t_iterate = timer_.getTime ();
    
  //std::cout << "Making Supervoxel structures" << std::endl;
  makeSupervoxels (supervoxel_clusters);
  //double t_supervoxels = timer_.getTime ();
  
 // std::cout << "--------------------------------- Timing Report --------------------------------- \n";
 // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n";
 // std::cout << "Time to seed clusters                          ="<<t_seeds-t_prep<<" ms\n";
 // std::cout << "Time to expand clusters                        ="<<t_iterate-t_seeds<<" ms\n";
 // std::cout << "Time to create supervoxel structures           ="<<t_supervoxels-t_iterate<<" ms\n";
 // std::cout << "Total run time                                 ="<<t_supervoxels-t_start<<" ms\n";
 // std::cout << "--------------------------------------------------------------------------------- \n";
  
  deinitCompute ();
}

template <typename PointT> void 
pcl16::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clusters)
{
  if (!initCompute () || 
      (input_ != 0   && input_->points.empty ()) ||
      (indices_ != 0 && indices_->empty ()))
  {
    clusters.clear ();
    return;
  }

  // Initialize the spatial locator
  if (!tree_)
  {
    if (input_->isOrganized ())
      tree_.reset (new pcl16::search::OrganizedNeighbor<PointT> ());
    else
      tree_.reset (new pcl16::search::KdTree<PointT> (false));
  }

  // Send the input dataset to the spatial locator
  tree_->setInputCloud (input_, indices_);
  extractEuclideanClusters (*input_, *indices_, tree_, static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);

  //tree_->setInputCloud (input_);
  //extractEuclideanClusters (*input_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);

  // Sort the clusters based on their size (largest one first)
  std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);

  deinitCompute ();
}

void 
pcl::SeededHueSegmentation::segment (PointIndices &indices_in, PointIndices &indices_out)
{
  if (!initCompute () || 
      (input_ != 0   && input_->points.empty ()) ||
      (indices_ != 0 && indices_->empty ()))
  {
    indices_out.indices.clear ();
    return;
  }

  // Initialize the spatial locator
  if (!tree_)
  {
    if (input_->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<PointXYZRGB> ());
    else
      tree_.reset (new pcl::search::KdTree<PointXYZRGB> (false));
  }

  // Send the input dataset to the spatial locator
  tree_->setInputCloud (input_);
  seededHueSegmentation (*input_, tree_, static_cast<float> (cluster_tolerance_), indices_in, indices_out, delta_hue_);
  deinitCompute ();
}

template <typename PointInT, typename PointOutT> void
pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
{
  if (!initCompute ())
  {
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  // Copy the header
  output.header = input_->header;

  // Resize the output dataset
  if (output.points.size () != indices_->size ())
    output.points.resize (indices_->size ());
  // Check if the output will be computed for all points or only a subset
  if (indices_->size () != input_->points.size ())
  {
    output.width = static_cast<int> (indices_->size ());
    output.height = 1;
  }
  else
  {
    output.width = input_->width;
    output.height = input_->height;
  }
  output.is_dense = input_->is_dense;

  // Perform the actual feature computation
  computeFeature (output);

  deinitCompute ();
}

template <typename PointInT> void
pcl::MeshConstruction<PointInT>::reconstruct (std::vector<pcl::Vertices> &polygons)
{
  if (!initCompute ()) 
  {
    polygons.clear ();
    return;
  }

  // Check if a space search locator was given
  if (check_tree_)
  {
    if (!tree_)
    {
      if (input_->isOrganized ())
        tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
      else
        tree_.reset (new pcl::search::KdTree<PointInT> (false));
    }

    // Send the surface dataset to the spatial locator
    tree_->setInputCloud (input_, indices_);
  }

  // Set up the output dataset
  //polygons.clear ();
  //polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
  // Perform the actual surface reconstruction
  performReconstruction (polygons);

  deinitCompute ();
}

template <typename PointT> void
pcl::SegmentDifferences<PointT>::segment (PointCloud &output)
{
    output.header = input_->header;

    if (!initCompute ())
    {
        output.width = output.height = 0;
        output.points.clear ();
        return;
    }

    // If target is empty, input - target = input
    if (target_->points.empty ())
    {
        output = *input_;
        return;
    }

    // Initialize the spatial locator
    if (!tree_)
    {
        if (target_->isOrganized ())
            tree_.reset (new pcl::OrganizedDataIndex<PointT> ());
        else
            tree_.reset (new pcl::KdTreeFLANN<PointT> (false));
    }
    // Send the input dataset to the spatial locator
    tree_->setInputCloud (target_);

    getPointCloudDifference (*input_, *target_, distance_threshold_, tree_, output);

    deinitCompute ();
}

template <typename PointT> void
pcl::registration::ELCH<PointT>::compute ()
{
  if (!initCompute ())
  {
    return;
  }

  LOAGraph grb[4];

  typename boost::graph_traits<LoopGraph>::edge_iterator edge_it, edge_it_end;
  for (boost::tuples::tie (edge_it, edge_it_end) = edges (*loop_graph_); edge_it != edge_it_end; edge_it++)
  {
    for (int j = 0; j < 4; j++)
      add_edge (source (*edge_it, *loop_graph_), target (*edge_it, *loop_graph_), 1, grb[j]);  //TODO add variance
  }

  double *weights[4];
  for (int i = 0; i < 4; i++)
  {
    weights[i] = new double[num_vertices (*loop_graph_)];
    loopOptimizerAlgorithm (grb[i], weights[i]);
  }

  //TODO use pose
  //Eigen::Vector4f cend;
  //pcl::compute3DCentroid (*((*loop_graph_)[loop_end_].cloud), cend);
  //Eigen::Translation3f tend (cend.head (3));
  //Eigen::Affine3f aend (tend);
  //Eigen::Affine3f aendI = aend.inverse ();

  //TODO iterate ovr loop_graph_
  //typename boost::graph_traits<LoopGraph>::vertex_iterator vertex_it, vertex_it_end;
  //for (boost::tuples::tie (vertex_it, vertex_it_end) = vertices (*loop_graph_); vertex_it != vertex_it_end; vertex_it++)
  for (size_t i = 0; i < num_vertices (*loop_graph_); i++)
  {
    Eigen::Vector3f t2;
    t2[0] = loop_transform_ (0, 3) * static_cast<float> (weights[0][i]);
    t2[1] = loop_transform_ (1, 3) * static_cast<float> (weights[1][i]);
    t2[2] = loop_transform_ (2, 3) * static_cast<float> (weights[2][i]);

    Eigen::Affine3f bl (loop_transform_);
    Eigen::Quaternionf q (bl.rotation ());
    Eigen::Quaternionf q2;
    q2 = Eigen::Quaternionf::Identity ().slerp (static_cast<float> (weights[3][i]), q);

    //TODO use rotation from branch start
    Eigen::Translation3f t3 (t2);
    Eigen::Affine3f a (t3 * q2);
    //a = aend * a * aendI;

    pcl::transformPointCloud (*(*loop_graph_)[i].cloud, *(*loop_graph_)[i].cloud, a);
    (*loop_graph_)[i].transform = a;
  }

  add_edge (loop_start_, loop_end_, *loop_graph_);

  deinitCompute ();
}

template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences (
    pcl::Correspondences &correspondences, double max_distance)
{
  if (!initCompute ())
    return;

  double max_dist_sqr = max_distance * max_distance;

  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
  correspondences.resize (indices_->size ());

  std::vector<int> index (1);
  std::vector<float> distance (1);
  pcl::Correspondence corr;
  unsigned int nr_valid_correspondences = 0;
  
  // Check if the template types are the same. If true, avoid a copy.
  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
  if (isSamePointType<PointSource, PointTarget> ())
  {
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  else
  {
    PointTarget pt;
    
    // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      // Copy the source data to a target PointTarget format so we can search in the tree
      pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
            input_->points[*idx], 
            pt));

      tree_->nearestKSearch (pt, 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;

      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;
    }
  }
  correspondences.resize (nr_valid_correspondences);
  deinitCompute ();
}

template <typename PointT> bool
pcl::registration::ELCH<PointT>::initCompute ()
{
  /*if (!PCLBase<PointT>::initCompute ())
  {
    PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
    return (false);
  }*/ //TODO

  if (loop_end_ == 0)
  {
    PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined!\n");
    deinitCompute ();
    return (false);
  }

  //compute transformation if it's not given
  if (compute_loop_)
  {
    PointCloudPtr meta_start (new PointCloud);
    PointCloudPtr meta_end (new PointCloud);
    *meta_start = *(*loop_graph_)[loop_start_].cloud;
    *meta_end = *(*loop_graph_)[loop_end_].cloud;

    typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
    for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
      *meta_start += *(*loop_graph_)[*si].cloud;

    for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
      *meta_end += *(*loop_graph_)[*si].cloud;

    //TODO use real pose instead of centroid
    //Eigen::Vector4f pose_start;
    //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);

    //Eigen::Vector4f pose_end;
    //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);

    PointCloudPtr tmp (new PointCloud);
    //Eigen::Vector4f diff = pose_start - pose_end;
    //Eigen::Translation3f translation (diff.head (3));
    //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
    //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);

    reg_->setInputTarget (meta_start);

    reg_->setInputCloud (meta_end);

    reg_->align (*tmp);

    loop_transform_ = reg_->getFinalTransformation ();
    //TODO hack
    //loop_transform_ *= trans.matrix ();

  }

  return (true);
}

template <typename PointT, typename NormalT> void
pcl::RegionGrowing<PointT, NormalT>::extract (std::vector <pcl::PointIndices>& clusters)
{
  clusters_.clear ();
  clusters.clear ();
  point_neighbours_.clear ();
  point_labels_.clear ();
  num_pts_in_segment_.clear ();
  number_of_segments_ = 0;

  bool segmentation_is_possible = initCompute ();
  if ( !segmentation_is_possible )
  {
    deinitCompute ();
    return;
  }

  segmentation_is_possible = prepareForSegmentation ();
  if ( !segmentation_is_possible )
  {
    deinitCompute ();
    return;
  }

  findPointNeighbours ();
  applySmoothRegionGrowingAlgorithm ();
  assembleRegions ();

  clusters.resize (clusters_.size ());
  std::vector<pcl::PointIndices>::iterator cluster_iter_input = clusters.begin ();
  for (std::vector<pcl::PointIndices>::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++)
  {
    if ((static_cast<int> (cluster_iter->indices.size ()) >= min_pts_per_cluster_) &&
        (static_cast<int> (cluster_iter->indices.size ()) <= max_pts_per_cluster_))
    {
      *cluster_iter_input = *cluster_iter;
      cluster_iter_input++;
    }
  }

  clusters_ = std::vector<pcl::PointIndices> (clusters.begin (), cluster_iter_input);
  clusters.resize(clusters_.size());

  deinitCompute ();
}

template <typename PointInT, typename StateT> void
pcl::tracking::Tracker<PointInT, StateT>::compute ()
{
  if (!initCompute ())
    return;
  
  computeTracking ();
  deinitCompute ();
}

template <typename PointSource, typename PointTarget> inline void
pcl::Registration<PointSource, PointTarget>::align (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
  if (!initCompute ()) return;

  if (!target_)
  {
    PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
    return;
  }

  // Resize the output dataset
  if (output.points.size () != indices_->size ())
    output.points.resize (indices_->size ());
  // Copy the header
  output.header   = input_->header;
  // Check if the output will be computed for all points or only a subset
  if (indices_->size () != input_->points.size ())
  {
    output.width    = (int) indices_->size ();
    output.height   = 1;
  }
  else
  {
    output.width    = input_->width;
    output.height   = input_->height;
  }
  output.is_dense = input_->is_dense;

  // Copy the point data to output
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i] = input_->points[(*indices_)[i]];

  // Set the internal point representation of choice
  if (point_representation_)
    tree_->setPointRepresentation (point_representation_);

  // Perform the actual transformation computation
  converged_ = false;
  final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity ();

  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 
  // transformation
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i].data[3] = 1.0;

  computeTransformation (output, guess);

  deinitCompute ();
}

template <typename PointModelT, typename PointSceneT> void
pcl::CorrespondenceGrouping<PointModelT, PointSceneT>::cluster (std::vector<Correspondences> &clustered_corrs)
{
    clustered_corrs.clear ();
    if (!initCompute ())
    {
        return;
    }

    //Perform the actual clustering
    clusterCorrespondences (clustered_corrs);

    deinitCompute ();
}

/** \brief Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using
 * the surface in setSearchSurface () and the spatial locator in setSearchMethod ()
 * \param output the resultant filtered point cloud dataset
 */
void
pcl::Filter<sensor_msgs::PointCloud2>::filter (PointCloud2 &output)
{
  if (!initCompute ())
    return;

  // Copy fields and header at a minimum
  output.header = input_->header;
  output.fields = input_->fields;

  // Apply the actual filter
  applyFilter (output);

  deinitCompute ();
}

template <typename PointSource, typename PointTarget> void
pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineCorrespondences (
    pcl::Correspondences &correspondences, float max_distance)
{
  typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;

  if (!initCompute ())
    return;

  if (!target_)
  {
    PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
    return;
  }

  float max_dist_sqr = max_distance * max_distance;

  correspondences.resize (indices_->size ());
  std::vector<int> index (1);
  std::vector<float> distance (1);
  pcl::Correspondence corr;
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Copy the source data to a target PointTarget format so we can search in the tree
    PointTarget pt;
    pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
          input_->points[(*indices_)[i]], 
          pt));

    //if (tree_->nearestKSearch (input_->points[(*indices_)[i]], 1, index, distance))
    if (tree_->nearestKSearch (pt, 1, index, distance))
    {
      if (distance[0] <= max_dist_sqr)
      {
        corr.index_query = static_cast<int> (i);
        corr.index_match = index[0];
        corr.distance = distance[0];
        correspondences[i] = corr;
        continue;
      }
    }
//    correspondences[i] = pcl::Correspondence(i, -1, std::numeric_limits<float>::max());
  }
  deinitCompute ();
}

template <typename PointInT, typename PointOutT> inline void
pcl::Keypoint<PointInT, PointOutT>::compute (PointCloudOut &output)
{
  if (!initCompute ())
  {
    PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
    return;
  }

  // Perform the actual computation
  detectKeypoints (output);

  deinitCompute ();

  // Reset the surface
  if (input_ == surface_)
    surface_.reset ();
}

template <typename PointInT, typename PointOutT> void
pcl::CloudSurfaceProcessing<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output)
{
  // Copy the header
  output.header = input_->header;

  if (!initCompute ())
  {
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  // Perform the actual surface reconstruction
  performProcessing (output);

  deinitCompute ();
}

template <typename PointInT> void
pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons)
{
  points.header = input_->header;
  if (!initCompute () || input_->points.empty () || indices_->empty ())
  {
    points.points.clear ();
    return;
  }

  // Perform the actual surface reconstruction
  performReconstruction (points, polygons, true);

  points.width = static_cast<uint32_t> (points.points.size ());
  points.height = 1;
  points.is_dense = true;

  deinitCompute ();
}

template <typename PointT, typename Graph> void
pcl::graph::OctreeAdjacencyGraphBuilder<PointT, Graph>::compute (Graph& graph)
{
  if (!initCompute ())
  {
    graph = Graph ();
    deinitCompute ();
    return;
  }

  if (!octree_adjacency_)
  {
    octree_adjacency_.reset (new OctreeAdjacency (voxel_resolution_));
    if (with_transform_function_)
      octree_adjacency_->setTransformFunction (boost::bind (&OctreeAdjacencyGraphBuilder<PointT, Graph>::transformFunction, _1));
  }

  octree_adjacency_->setInputCloud (input_, indices_);
  octree_adjacency_->addPointsFromInputCloud ();

  graph = Graph (octree_adjacency_->getLeafCount ());

  leaf_vertex_map_.clear ();
  typename OctreeAdjacency::iterator leaf_itr = octree_adjacency_->begin ();
  for (VertexId v = 0; leaf_itr != octree_adjacency_->end (); ++leaf_itr, ++v)
  {
    leaf_vertex_map_[*leaf_itr] = v;
    copyPoint<PointT, PointOutT> ((*leaf_itr)->getData (), graph[v]);
  }

  for (leaf_itr = octree_adjacency_->begin (); leaf_itr != octree_adjacency_->end (); ++leaf_itr)
  {
    const VertexId idx1 = leaf_vertex_map_[*leaf_itr];
    typename OctreeAdjacencyContainer::iterator neighb_itr;
    for (neighb_itr = (*leaf_itr)->begin (); neighb_itr != (*leaf_itr)->end (); ++neighb_itr)
    {
      const VertexId idx2 = leaf_vertex_map_[*neighb_itr];
      if (idx1 > idx2) // this prevents from adding self-edges and duplicates
        boost::add_edge (idx1, idx2, graph);
    }
  }
}