源码解读
以 examples/teaser_cpp_fpfh/teaser_cpp_fpfh.cc 为例。
-
代码主要流程为：
- 加载 .ply 源点云模型
- 将源点云数据写入 Eigen 矩阵
- 执行旋转和平移变换得到目标点云
- 对目标点云加噪声和外点
- 将目标点云从 Eigen 矩阵转换为
teaser::PointCloud格式 - 对源点云和目标点云计算 FPFH 特征描述子，接着计算对应点对 correspondences
- 设置 Teaser++ 配准参数，执行配准，输出配准结果
-
Teaser++ 中并没有直接调用 PCL ，而是重写了一些 PCL 的内容，可能是考虑到 PCL 更新频率高，避免版本不兼容：
.ply点云加载teaer::PointCloud点云数据结构- Eigen 执行点云欧式变换
- 增加噪声和外点
- 计算 FPFH 特征描述子
- 利用特征描述子计算 correspondences
-
Teaser++ 超参数的含义：
// Run TEASER++ registration // Prepare solver parameters teaser::RobustRegistrationSolver::Params params; params.noise_bound = NOISE_BOUND; // 一个浮点数，表示噪声的界 params.cbar2 = 1; // 可接受噪声与噪声界之比的平方。通常设置为1。 params.estimate_scaling = false; // 是否进行尺度估计 params.rotation_max_iterations = 100; // 旋转估计最大迭代次数 params.rotation_gnc_factor = 1.4; // 旋转估计的GNC因子 // 对于GNC-TLS方法：乘以GNC控制参数 // 对于FGR方法：根据GNC控制参数进行划分 params.rotation_estimation_algorithm = teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; params.rotation_cost_threshold = 0.005; // 旋转估计终止的损失阈值
源码改写与应用
目的是搭配 PCL 点云处理库进行使用，所以需要在 PCL 的点云数据结构和 Teaser++ 的之间进行转圜，Teaser++ 中一些点云处理的方法可以直接使用 PCL 中提供的方法，如欧式变换、特征描述子和 correspondences 的计算。
点云模型读取
- 点云一般存储为
pcd格式，所以从读取ply改为读取pcd。
#include <pcl/io/pcd_io.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr SCloud(new pcl::PointCloud<pcl::PointXYZ>); // 源点云
pcl::io::loadPCDFile("source_cloud.pcd", *SCloud);
格式转换
- 这里我编写了将 PCL 的点云格式转换为 Teaser++ 的格式的函数和将 Teaser++ 的对应点对转 PCL correspondences 的函数。
#include <pcl/point_types.h>
#include <teaser/geometry.h>
#include <pcl/correspondence.h>
typedef pcl::PointXYZ PointT;
/**
* @brief: PCL 点云转 Teaser 点云格式
* @return {*}
* @note:
* @warning:
*/
void pcd_to_teaser(pcl::PointCloud<PointT>::Ptr &input_cloud, teaser::PointCloud &output_cloud) {
for (size_t i = 0; i < input_cloud->points.size(); ++i) {
output_cloud.push_back(
{input_cloud->points[i].x, input_cloud->points[i].y, input_cloud->points[i].z});
}
}
/**
* @brief: Teaser 对应点对转 PCL correspondence
* @return {*}
* @note:
* @warning:
*/
void teaser_to_correspondence(std::vector<std::pair<int, int>> &input,
pcl::Correspondences &output) {
for (size_t i = 0; i < input.size(); ++i) {
output.push_back(pcl::Correspondence(input[i].first, input[i].second, 0.0));
}
}
体素滤波
- 源点云和目标点云的分辨率尽量保持一致，若不一致可以对点云进行下采样，使其相近。
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
typedef pcl::PointXYZ PointT;
/**
* @brief: 使用体素化网格方法对点云数据集进行下采样（即减少点数）
* @param {Ptr} &cloud
* @param {Ptr} &cloud_filtered
* @param {float} leaf_size
* @return {*}
* @note:
* @warning:
*/
void voxel_filter(pcl::PointCloud<PointT>::Ptr &cloud, pcl::PointCloud<PointT>::Ptr &cloud_filtered,
float leaf_size = 1.0f) {
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::VoxelGrid<PointT> voxel;
voxel.setLeafSize(leaf_size, leaf_size, leaf_size);
voxel.setInputCloud(cloud);
voxel.filter(*cloud_filtered);
voxel.getRemovedIndices(*inliers);
}
欧式变换
- 这里我将欧式旋转变换和欧式平移变换分别封装为函数，可以连续调用以实现多次变换。
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
typedef pcl::PointXYZ PointT;
/**
* @brief: 绕 axis 轴旋转 theta 度
* @param {float} theta 单位为度
* @param {string} axis 可选 x, y, z
* @param {Ptr} &source
* @param {Ptr} &target
* @return {*}
* @note:
* @warning:
*/
void euclidean_rotate(float theta, std::string axis, pcl::PointCloud<PointT>::Ptr &source, pcl::PointCloud<PointT>::Ptr &target) {
Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
theta = M_PI / 180 * theta;
if (axis == "x") {
T(1, 1) = cos(theta);
T(1, 2) = sin(theta);
T(2, 1) = -sin(theta);
T(2, 2) = cos(theta);
} else if (axis == "y") {
T(0, 0) = cos(theta);
T(0, 2) = -sin(theta);
T(2, 0) = sin(theta);
T(2, 2) = cos(theta);
} else if (axis == "z") {
T(0, 0) = cos(theta);
T(0, 1) = -sin(theta);
T(1, 0) = sin(theta);
T(1, 1) = cos(theta);
}
pcl::transformPointCloud(*source, *target, T);
}
/**
* @brief: 平移变换
* @param {float} x
* @param {float} y
* @param {float} z
* @param {Ptr} &source
* @param {Ptr} &target
* @return {*}
* @note:
* @warning:
*/
void euclidean_translate(float x, float y, float z, pcl::PointCloud<PointT>::Ptr &source, pcl::PointCloud<PointT>::Ptr &target) {
Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
T(0, 3) = x;
T(1, 3) = y;
T(2, 3) = z;
pcl::transformPointCloud(*source, *target, T);
}
计算 FPFH 特征描述子
- 计算 FPFH 特征描述子需要输入点云法向量。
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>
typedef pcl::PointXYZ PointT;
/**
* @brief: 计算法向量
* @param {Ptr} &cloud
* @param {Ptr} &normals 返回法向坐标和表面曲率估计的点结构
* @return {*}
* @note:
* @warning:
*/
void compute_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::Normal>::Ptr &normals) {
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
//-------------------------法向量估计-----------------------
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
n.setInputCloud(cloud);
n.setNumberOfThreads(8); //设置openMP的线程数
// n.setViewPoint(0,0,0);//设置视点，默认为（0，0，0）
n.setSearchMethod(tree);
n.setKSearch(10);
// n.setRadiusSearch(0.03);//半径搜素
n.compute(*normals);
}
- 这里使用 KDTree 和 omp 多线程加速 FPFH 的计算，替代了 Teaser++ 中重写的 FPFH 的计算，理论上可以提高计算效率。
#include <pcl/point_types.h>
#include <pcl/features/fpfh_omp.h> // fpfh加速计算的omp(多核并行计算)
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;
/**
* @brief: 计算 FPFH 特征描述子
* @param {Ptr} input_cloud 输入点云
* @param {Ptr} normals 输入点云的法向量
* @return {*}
* @note:
* @warning:
*/
fpfhFeature::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, pcl::PointCloud<pcl::Normal>::Ptr normals) {
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
//------------------FPFH估计-------------------------------
fpfhFeature::Ptr fpfh(new fpfhFeature);
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
f.setNumberOfThreads(8); // 指定8核计算
f.setInputCloud(input_cloud);
f.setInputNormals(normals);
f.setSearchMethod(tree);
f.setKSearch(10);
f.compute(*fpfh);
return fpfh;
}
配准总流程
#include <teaser/registration.h>
#include <teaser/matcher.h>
teaser::RegistrationSolution teaserpp_registration(pcl::PointCloud<PointT>::Ptr &SCloud,
pcl::PointCloud<PointT>::Ptr &TCloud,
pcl::Correspondences &cru_correspondences) {
// Convert to teaser point cloud
teaser::PointCloud src_cloud;
pcd_to_teaser(SCloud, src_cloud);
teaser::PointCloud tgt_cloud;
pcd_to_teaser(TCloud, tgt_cloud);
// Compute FPFH
pcl::PointCloud<pcl::Normal>::Ptr src_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr tgt_normals(new pcl::PointCloud<pcl::Normal>);
compute_normal(SCloud, src_normals);
fpfhFeature::Ptr obj_descriptors = compute_fpfh_feature(SCloud, src_normals);
compute_normal(TCloud, tgt_normals);
fpfhFeature::Ptr scene_descriptors = compute_fpfh_feature(TCloud, tgt_normals);
std::cout << "obj_descriptors size: " << obj_descriptors->points.size() << std::endl;
std::cout << "scene_descriptors size: " << scene_descriptors->points.size() << std::endl;
// Compute correspondences
teaser::Matcher matcher;
auto correspondences = matcher.calculateCorrespondences(
src_cloud, tgt_cloud, *obj_descriptors, *scene_descriptors, false, true, false, 0.95);
std::cout << "correspondences size: " << correspondences.size() << std::endl;
teaser_to_correspondence(correspondences, cru_correspondences);
// Run TEASER++ registration
// Prepare solver parameters
teaser::RobustRegistrationSolver::Params params;
params.noise_bound = 0.05;
params.cbar2 = 1;
params.estimate_scaling = false;
params.rotation_max_iterations = 100;
params.rotation_gnc_factor = 1.4;
params.rotation_estimation_algorithm =
teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
params.rotation_cost_threshold = 0.005;
// Solve with TEASER++
teaser::RobustRegistrationSolver solver(params);
solver.solve(src_cloud, tgt_cloud, correspondences);
teaser::RegistrationSolution solution = solver.getSolution();
return solution;
}
主函数测试
- 可以将上面的函数与下面的主函数写在同一个程序中，熟悉 C++ 多文件的可以写在头文件中供调用。
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
int main(int argc, char **argv) {
// Load the .pcd file
pcl::PointCloud<pcl::PointXYZ>::Ptr SCloud(new pcl::PointCloud<pcl::PointXYZ>); // 源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr TCloud(new pcl::PointCloud<pcl::PointXYZ>); // 目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr TransformCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile(argv[1], *SCloud);
pcl::io::loadPCDFile(argv[2], *TCloud);
// 稀疏滤波
voxel_filter(SCloud, SCloud, 4);
voxel_filter(TCloud, TCloud, 4);
std::cout << "SCloud size: " << SCloud->size() << " 个点" << std::endl;
std::cout << "TCloud size: " << TCloud->size() << " 个点" << std::endl;
// 点云变换
euclidean_rotate(90, "x", TCloud, TCloud);
euclidean_rotate(90, "y", TCloud, TCloud);
euclidean_rotate(90, "z", TCloud, TCloud);
euclidean_translate(5, 10, -5, TCloud, TCloud);
// Teaser++ 配准
pcl::Correspondences cru_correspondences;
teaser::RegistrationSolution solution =
teaserpp_registration(SCloud, TCloud, cru_correspondences);
// Compare results
std::cout << "=====================================" << std::endl;
std::cout << " TEASER++ Results " << std::endl;
std::cout << "=====================================" << std::endl;
std::cout << "Expected scale: " << std::endl;
std::cout << solution.scale << std::endl;
std::cout << "Expected rotation: " << std::endl;
// std::cout << T.topLeftCorner(3, 3) << std::endl;
std::cout << "Estimated rotation: " << std::endl;
std::cout << solution.rotation << std::endl;
std::cout << std::endl;
std::cout << "Expected translation: " << std::endl;
// std::cout << T.topRightCorner(3, 1) << std::endl;
std::cout << "Estimated translation: " << std::endl;
std::cout << solution.translation << std::endl;
std::cout << std::endl;
// 执行变换
Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
transformation.block<3, 3>(0, 0) = solution.rotation;
transformation.block<3, 1>(0, 3) = solution.translation;
pcl::transformPointCloud(*SCloud, *TransformCloud, transformation);
computeRMSE(TCloud, TransformCloud);
pcl::visualization::PCLVisualizer viewer("Alignment - Teaser");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(TCloud, 255, 0, 0);
viewer.addPointCloud(TCloud, target_color, "TCloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> reg_color(SCloud, 0, 255, 0);
viewer.addPointCloud(TransformCloud, reg_color, "RegCloud");
// 对应关系可视化
viewer.setWindowName("基于特征描述子的对应");
viewer.addCorrespondences<pcl::PointXYZ>(TransformCloud, TCloud, cru_correspondences,
"correspondence");
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2,
"correspondence");
viewer.spin();
}
评论（2）
您还未登录，请登录后发表或查看评论