monage2018
commented
3 months ago Hi, It might be that I didn’t set the parameters correctly or there was some mistake in the process, and the results I got are not very good. I would like you to help me see where the problem lies, thank you.
`#include
include
include
include
include
include
include
include
include <pcl/io/pcd_io.h>
include <pcl/point_types.h>
include <pcl/features/fpfh.h>
include <pcl/features/normal_3d.h>
include <pcl/filters/voxel_grid.h>
include <pcl/registration/correspondence_estimation.h>
include <pcl/visualization/pcl_visualizer.h>
include <pcl/filters/radius_outlier_removal.h>
include <pcl/common/transforms.h>
include <pcl/filters/extract_indices.h>
include <glog/logging.h>
include <Eigen/Core>
include <Eigen/SVD>
include "tic_toc.h"
include "clipperplus/clipperplus_clique.h"
typedef pcl::PointCloud
double ds_leaf_size = 0.1; double or_radius = 0.3; int or_knn = 20; int fpfh_knn = 20; double fpfh_radius = 0.3; double dc_radius = 3.0; double dis_thres = 0.05;
// 删除字符串中的空格、制表符tab等无效字符 std::string Trim(std::string& str) { // 在字符串str中从索引0开始，返回首次不匹配"\t\r\n"的位置 str.erase(0, str.find_first_not_of("\t\r\n")); str.erase(str.find_last_not_of("\t\r\n") + 1); return str; }
void Csv2PointCloud(const std::string& filename, PointCloud::Ptr& cloud)
{
cloud->points.clear();
std::ifstream fin(filename); // 打开文件流操作
std::string line;
while (getline(fin, line)) // 整行读取，换行符"\n"区分，遇到文件尾标志eof终止读取
{
std::istringstream sin(line); // 将整行字符串line读入到字符串流istringstream中
std::vector
void OutlierRemoval(const PointCloud::Ptr &input, const PointCloud::Ptr &output) {
pcl::RadiusOutlierRemoval
void OutlierRemoval(const PointCloud::Ptr &input, const PointCloud::Ptr &output, const FPFHCloud::Ptr & fpfhs) {
pcl::RadiusOutlierRemoval
void Downsample(const PointCloud::Ptr &input, const PointCloud::Ptr &output) {
pcl::VoxelGrid
// 计算FPFH描述子
void ComputeFPFH(const PointCloud::Ptr &input, FPFHCloud::Ptr &fpfhs) {
TicToc tt;
// 法线估计
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::PointCloud
tt.Tic(); // FPFH估计 pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh; fpfh.setInputCloud(input); fpfh.setInputNormals(normals); // fpfh.setKSearch(fpfh_knn); fpfh.setRadiusSearch(fpfh_radius); fpfh.compute(*fpfhs); std::cout << "FPFH estimation time: " << tt.Toc() << std::endl; return; }
// 描述子匹配
void MatchFPFH(const FPFHCloud::Ptr &source_fpfhs, const FPFHCloud::Ptr &target_fpfhs, std::vector<std::vector
void DetermineCorrespondences(const FPFHCloud::Ptr &source_fpfhs, const FPFHCloud::Ptr &target_fpfhs, pcl::Correspondences &correspondences) { pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33, pcl::FPFHSignature33> est; est.setInputSource(source_fpfhs); est.setInputTarget(target_fpfhs); est.determineCorrespondences(correspondences, dc_radius); return; }
Eigen::Matrix4f ComputeTransformation(const PointCloud::Ptr &source_points, const PointCloud::Ptr &target_points) { // 计算质心 Eigen::Vector4f source_centroid; pcl::compute3DCentroid(source_points, source_centroid); Eigen::Vector4f target_centroid; pcl::compute3DCentroid(target_points, target_centroid);
// 去中心化
Eigen::MatrixXf source_demean = source_points->getMatrixXfMap() - source_centroid.replicate(1, source_points->size());
Eigen::MatrixXf target_demean = target_points->getMatrixXfMap() - target_centroid.replicate(1, target_points->size());
// 构建对应关系矩阵
Eigen::MatrixXf H = source_demean * target_demean.transpose();
// SVD分解
Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXf R = svd.matrixV() * svd.matrixU().transpose();
// 确保一个合适的旋转（处理特殊情况）
if (R.determinant() < 0) {
Eigen::MatrixXf V = svd.matrixV();
V.col(2) *= -1;
R = V * svd.matrixU().transpose();
}
// 计算变换矩阵
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
transformation.block<3,3>(0,0) = R.block<3,3>(0,0);
transformation.block<3,1>(0,3) = target_centroid.head<3>() - R * source_centroid.head<3>();
return transformation;}
int main(int argc, char* argv) { google::InstallFailureSignalHandler(); google::InstallFailureWriter([](const char data, int size) { std::string str = std::string(data, size); LOG(ERROR) << str; }); FLAGS_colorlogtostderr = true;
TicToc tt; // Object for storing the point cloud. std::string source_file = argv[1]; std::string target_file = argv[2]; PointCloud::Ptr source_pc(new PointCloud); PointCloud::Ptr target_pc(new PointCloud); Csv2PointCloud(source_file, source_pc); Csv2PointCloud(target_file, target_pc); std::cout << "Raw size: " << source_pc->size() << ", " << target_pc->size() << std::endl; std::cout << "Read file time: " << tt.Toc() << std::endl;
tt.Tic(); PointCloud::Ptr source_pc_filter(new PointCloud); PointCloud::Ptr target_pc_filter(new PointCloud); Downsample(source_pc, source_pc_filter); Downsample(target_pc, target_pc_filter); std::cout << "Down sample size: " << source_pc_filter->size() << ", " << target_pc_filter->size() << std::endl; std::cout << "Down sample time: " << tt.Toc() << std::endl;
tt.Tic(); OutlierRemoval(source_pc_filter, source_pc_filter); OutlierRemoval(target_pc_filter, target_pc_filter); std::cout << "Outlier removal size: " << source_pc_filter->size() << ", " << target_pc_filter->size() << std::endl; std::cout << "Outlier removal time: " << tt.Toc() << std::endl;
tt.Tic(); // FPFH estimation object. FPFHCloud::Ptr source_pc_fpfhs(new FPFHCloud()); FPFHCloud::Ptr target_pc_fpfhs(new FPFHCloud()); ComputeFPFH(source_pc_filter, source_pc_fpfhs); ComputeFPFH(target_pc_filter, target_pc_fpfhs); std::cout << "FPFH size: " << source_pc_fpfhs->size() << ", " << target_pc_fpfhs->size() << std::endl; std::cout << "Compute FPFH time: " << tt.Toc() << std::endl;
// tt.Tic(); // OutlierRemoval(source_pc_filter, source_pc_filter, source_pc_fpfhs); // OutlierRemoval(target_pc_filter, target_pc_filter, target_pc_fpfhs); // std::cout << "Outlier removal size: " << source_pc_filter->size() << ", " << target_pc_filter->size() << ", " // << source_pc_fpfhs->size() << ", " << target_pc_fpfhs->size() << std::endl; // std::cout << "Outlier removal time: " << tt.Toc() << std::endl;
tt.Tic();
// 估计对应关系
// std::vector<std::vector
tt.Tic();
int cn = correspondences.size();
Eigen::MatrixXd adj(cn, cn);
int cnt = 0;
std::vector
// std::sort(dis.begin(), dis.end()); // std::cout << "Dis : ["; // for(auto d : dis){ // std::cout << d << ", "; // } // std::cout << "]" << std::endl;
tt.Tic();
long clique_size = 0;
std::vector
tt.Tic();
PointCloud::Ptr source_points(new PointCloud);
PointCloud::Ptr target_points(new PointCloud);
for (int i = 0; i < clique_size; ++i){
source_points->push_back(source_pc_filter->points[correspondences[clique[i]].index_query]);
target_points->push_back(source_pc_filter->points[correspondences[clique[i]].index_match]);
}
Eigen::Matrix4f T_prec = ComputeTransformation(source_points, target_points);
Eigen::Matrix4f T_true;
T_true << -0.022699, -0.999496, -0.022195, 1.533389,
0.999446, -0.023228, 0.023819, 0.791053,
-0.024322, -0.021642, 0.99947, 0.028004,
0.0, 0.0, 0.0, 1.0;
PointCloud::Ptr source_pc_prec(new PointCloud);
PointCloud::Ptr source_pc_true(new PointCloud);
pcl::transformPointCloud(source_pc, source_pc_prec, T_prec);
pcl::transformPointCloud(source_pc, source_pc_true, T_true);
std::cout << "T_prec: " << std::setfill(' ') << T_prec << std::endl;
std::cout << "T_true: " << std::setfill(' ') << T_true << std::endl;
pcl::visualization::PCLVisualizer viewer("FPFH Feature Matching");
// pcl::visualization::PointCloudColorHandlerCustom
// 启动可视化 viewer.spin ();
return 0; }`
I found the api: unsigned long clipperplus_clique(const Eigen::MatrixXd& adj, long& clique_size, std::vector& clique,
int& certificate)
How to construct adjacency matrix for two point clouds？Thanks