template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const std::vector<int> &indices_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const std::vector<int> &indices_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = indices_src.size ();
  if (indices_tgt.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
    return;
  }

  if (weights_.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLSWeighted::estimateRigidTransformation] Number or weights from the number of correspondences! Use setWeights () to set them.\n");
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
  typename std::vector<Scalar>::const_iterator weights_it = weights_.begin ();
  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
    const pcl::PointCloud<PointSource> &cloud_src,
    const pcl::PointCloud<PointTarget> &cloud_tgt,
    const pcl::Correspondences &correspondences,
    Matrix4 &transformation_matrix) const
{
  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const pcl::Correspondences &correspondences,
                             Matrix4 &transformation_matrix) const
{
  ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
  std::vector<Scalar> weights (correspondences.size ());
  for (size_t i = 0; i < correspondences.size (); ++i)
    weights[i] = correspondences[i].weight;
  typename std::vector<Scalar>::const_iterator weights_it = weights.begin ();
  estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimation2D<PointSource, PointTarget, Scalar>::estimateRigidTransformation (
    const pcl::PointCloud<PointSource> &cloud_src,
    const std::vector<int> &indices_src,
    const pcl::PointCloud<PointTarget> &cloud_tgt,
    Matrix4 &transformation_matrix) const
{
  if (indices_src.size () != cloud_tgt.points.size ())
  {
    PCL_ERROR ("[pcl::Transformation2D::estimateRigidTransformation] Number or points in source (%zu) differs than target (%zu)!\n", indices_src.size (), cloud_tgt.points.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);
}
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             Matrix4 &transformation_matrix) const
{
  size_t nr_points = cloud_src.points.size ();
  if (cloud_tgt.points.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", nr_points, cloud_tgt.points.size ());
    return;
  }

  ConstCloudIterator<PointSource> source_it (cloud_src);
  ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  estimateRigidTransformation (source_it, target_it, transformation_matrix);  
}
bool SimpleDirectedGraph::areConnected(int source, int target) const {
	if (source == target) return true;
	SimpleDirectedGraph::BFIterator source_it(*this, 0x3fffffff, true);
	source_it.addNode(source);
	SimpleDirectedGraph::BFIterator target_it(*this, 0x3fffffff, true);
	target_it.addNode(target);

	int from_source_node;
	while (source_it.read(&from_source_node)) {
		if (from_source_node == target) return true;
		int from_target_node;
		if (target_it.read(&from_target_node) && from_target_node == source) return true;
	}

	int from_target_node;
	while (target_it.read(&from_target_node)) {
		if (from_target_node == source) return true;
	}

	return false;
}