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