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