template <typename PointT> void pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) { if (indices_->size () != indices_tgt_->size ()) { PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ()); optimized_coefficients = model_coefficients; return; } // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients) || !target_) { optimized_coefficients = model_coefficients; return; } std::vector<int> indices_src (inliers.size ()); std::vector<int> indices_tgt (inliers.size ()); for (size_t i = 0; i < inliers.size (); ++i) { // NOTE: not tested! indices_src[i] = (*indices_)[inliers[i]]; indices_tgt[i] = (*indices_tgt_)[inliers[i]]; } estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients); }
template <typename PointSource, typename PointTarget> inline void TransformationEstimationJointOptimize<PointSource, PointTarget>::estimateRigidTransformation ( const pcl::PointCloud<PointSource> &cloud_src, const pcl::PointCloud<PointTarget> &cloud_tgt, const pcl::Correspondences &correspondences, const pcl::Correspondences &correspondences_dfp, float alpha_arg, Eigen::Matrix4f &transformation_matrix) { const int nr_correspondences = (int)correspondences.size(); std::vector<int> indices_src(nr_correspondences); std::vector<int> indices_tgt(nr_correspondences); for (int i = 0; i < nr_correspondences; ++i) { indices_src[i] = correspondences[i].index_query; indices_tgt[i] = correspondences[i].index_match; } const int nr_correspondences_dfp = (int)correspondences_dfp.size(); std::vector<int> indices_src_dfp(nr_correspondences_dfp); std::vector<int> indices_tgt_dfp(nr_correspondences_dfp); for (int i = 0; i < nr_correspondences_dfp; ++i) { indices_src_dfp[i] = correspondences_dfp[i].index_query; indices_tgt_dfp[i] = correspondences_dfp[i].index_match; } estimateRigidTransformation(cloud_src, indices_src, indices_src_dfp, cloud_tgt, indices_tgt, indices_tgt_dfp, alpha_arg, transformation_matrix); }
template <typename PointSource, typename PointTarget, typename MatScalar> inline void pcl::registration::TransformationEstimationLM<PointSource, PointTarget, MatScalar>::estimateRigidTransformation ( const pcl::PointCloud<PointSource> &cloud_src, const pcl::PointCloud<PointTarget> &cloud_tgt, const pcl::Correspondences &correspondences, Matrix4 &transformation_matrix) const { const int nr_correspondences = static_cast<const int> (correspondences.size ()); std::vector<int> indices_src (nr_correspondences); std::vector<int> indices_tgt (nr_correspondences); for (int i = 0; i < nr_correspondences; ++i) { indices_src[i] = correspondences[i].index_query; indices_tgt[i] = correspondences[i].index_match; } estimateRigidTransformation (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix); }
template <typename PointT> bool pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) { if (!target_) { PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n"); return (false); } // Need 3 samples if (samples.size () != 3) return (false); std::vector<int> indices_tgt (3); for (int i = 0; i < 3; ++i) indices_tgt[i] = correspondences_[samples[i]]; estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients); return (true); }