本文整理汇总了C++中eigen::Affine3f::makeAffine方法的典型用法代码示例。如果您正苦于以下问题：C++ Affine3f::makeAffine方法的具体用法？C++ Affine3f::makeAffine怎么用？C++ Affine3f::makeAffine使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Affine3f的用法示例。
在下文中一共展示了Affine3f::makeAffine方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: rand
bool reduce_measurement_g2o::estimate_transform_ransac(
const pcl::PointCloud<pcl::PointXYZ> & src,
const pcl::PointCloud<pcl::PointXYZ> & dst,
const std::vector<cv::DMatch> matches, int num_iter,
float distance2_threshold, int min_num_inliers, Eigen::Affine3f & trans,
std::vector<bool> & inliers) {
int max_inliers = 0;
if (matches.size() < min_num_inliers)
return false;
for (int iter = 0; iter < num_iter; iter++) {
int rand_idx[3];
// Select 3 random points
for (int i = 0; i < 3; i++) {
rand_idx[i] = rand() % matches.size();
}
while (rand_idx[0] == rand_idx[1] || rand_idx[0] == rand_idx[2]
|| rand_idx[1] == rand_idx[2]) {
for (int i = 0; i < 3; i++) {
rand_idx[i] = rand() % matches.size();
}
}
//std::cerr << "Random idx " << rand_idx[0] << " " << rand_idx[1] << " "
// << rand_idx[2] << " " << matches.size() << std::endl;
Eigen::Matrix3f src_rand, dst_rand;
for (int i = 0; i < 3; i++) {
src_rand.col(i) =
src[matches[rand_idx[i]].queryIdx].getVector3fMap();
dst_rand.col(i) =
dst[matches[rand_idx[i]].trainIdx].getVector3fMap();
}
Eigen::Affine3f transformation;
transformation = Eigen::umeyama(src_rand, dst_rand, false);
//std::cerr << "src_rand " << std::endl << src_rand << std::endl;
//std::cerr << "dst_rand " << std::endl << dst_rand << std::endl;
//std::cerr << "src_rand_trans " << std::endl << transformation * src_rand
// << std::endl;
//std::cerr << "trans " << std::endl << transformation.matrix()
// << std::endl;
int current_num_inliers = 0;
std::vector<bool> current_inliers;
current_inliers.resize(matches.size());
for (size_t i = 0; i < matches.size(); i++) {
Eigen::Vector4f distance_vector = transformation
* src[matches[i].queryIdx].getVector4fMap()
- dst[matches[i].trainIdx].getVector4fMap();
current_inliers[i] = distance_vector.squaredNorm()
< distance2_threshold;
if (current_inliers[i])
current_num_inliers++;
}
if (current_num_inliers > max_inliers) {
max_inliers = current_num_inliers;
inliers = current_inliers;
}
}
if (max_inliers < min_num_inliers) {
return false;
}
Eigen::Matrix3Xf src_rand(3, max_inliers), dst_rand(3, max_inliers);
int col_idx = 0;
for (size_t i = 0; i < inliers.size(); i++) {
if (inliers[i]) {
src_rand.col(col_idx) = src[matches[i].queryIdx].getVector3fMap();
dst_rand.col(col_idx) = dst[matches[i].trainIdx].getVector3fMap();
col_idx++;
}
}
trans = Eigen::umeyama(src_rand, dst_rand, false);
trans.makeAffine();
std::cerr << max_inliers << std::endl;
return true;
}