当前位置：首页 > news >正文

pcl案例六基于配准的无序抓取

news 2025/9/18 14:46:04

流程步骤：

1，先在source上找到抓取点。

——这里以obb的上表面的中点为抓取点。抓取时z方向垂直与表面

2，计算法向量

3，采样关键点

——这里点云数量少，特征角点少，所以采用了直接uinform的下采样

4，计算fpfh的描述子

5，sample_consensus_prerejective 基于采样一致性的配准方案配准

——详细看了一下参数设置，这个设置参数还是很有考究的。

6，用的笨办法，去掉匹配到的target关键点，和关键点描述子。将剩下的点再进行二次配准。

7，计算每次匹配后的抓取点

——三个抓取点

8，这里没有用icp进一步配准（懒）。

感觉：没有halcon里面的find_surface_shape_model好用，在halcon里可能十行代码就可以搞定。但pcl里面，需要几百行。匹配效果和操作难度都要高很多。

下面附上我的粗糙代码，有玩的好的朋友欢迎指点

#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
#include <thread>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include<pcl/common/common.h>
#include<pcl/filters/project_inliers.h>
#include<pcl/common/transforms.h>
#include<pcl/filters/passthrough.h>
#include<pcl/filters/crop_box.h>
#include<pcl/features/normal_3d.h>
#include<pcl/filters/uniform_sampling.h>
#include<pcl/features/fpfh.h>
#include<pcl/registration/correspondence_estimation.h>
#include<pcl/registration//sample_consensus_prerejective.h>
#include<pcl/correspondence.h>
#include<pcl/registration/transformation_estimation_svd.h>
#include<pcl/recognition/trimmed_icp.h>
#include <pcl/registration/icp.h>
#include<pcl/registration/sample_consensus_prerejective.h>
#include<pcl/keypoints/iss_3d.h>#include "tolls.h"using namespace std;template<class point7>
class pcl::PointCloud<point7>::Ptr uniform_sample_by_radius(class pcl::PointCloud<point7>::Ptr inputcloud, float radius = 0.1)
{pcl::UniformSampling <point7 > usample;class pcl::PointCloud<point7>::Ptr source_keyp(new pcl::PointCloud<point7>);usample.setRadiusSearch(radius);usample.setInputCloud(inputcloud);usample.filter(*source_keyp);return source_keyp;
};int main ()
{pcl::PointCloud<pcl::PointXYZ>::Ptr test(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPLYFile("D:\\Desktop\\pacl_learning\\pick_up\\source0.ply",*source);pcl::io::loadPLYFile("D:\\Desktop\\pacl_learning\\pick_up\\target2.ply", *target);vector<int> a;pcl::removeNaNFromPointCloud(*source, *source, a);pcl::removeNaNFromPointCloud(*target, *target, a);//这里假设以obb包围盒的上表面中心。顶点作为抓取点，抓取时z方向垂直于上表面。pcl::PointXYZ pb_min, pb_max,pb_center;Eigen::Affine3f trans_obb = get_trans_obb<pcl::PointXYZ>(source, pb_min, pb_max, pb_center);test->push_back(pb_min);test->push_back(pb_max);test->push_back(pb_center);pcl::PointXYZ pb_top,tempt1(0, 0, pb_min.z);pb_top=pcl::transformPoint<pcl::PointXYZ> (tempt1,  trans_obb.inverse());test->push_back(pb_top);Eigen::Affine3f pick_0 = trans_obb.inverse();//pick_0就是我的抓取点。定义在外包围盒的上表面中心。z轴方向垂直于上表面。pick_0.translate(Eigen::Vector3f(pb_top.x-pb_center.x, pb_top.y - pb_center.y, pb_top.z - pb_center.z ));//对source回转以后，可以看到。坐标系与source的显示关系//pcl::transformPointCloud(*source, *source, pick_0.inverse());   //用于显示查看抓取点的准确性。cout << "picl_0" << endl;cout << pick_0.matrix()<< endl;//计算法线pcl::PointCloud<pcl::PointNormal>::Ptr source_normal = estimate_normal<pcl::PointXYZ>(source, 10);pcl::PointCloud<pcl::PointNormal>::Ptr target_normal = estimate_normal<pcl::PointXYZ>(target, 10);//求解点云分辨率，float resolution = get_cloud_resolution<pcl::PointXYZ>(source, 100);cout << "resolution___" << resolution << endl;//检索target_keyp的法线pcl::PointCloud<pcl::PointNormal>::Ptr target_kpn=uniform_sample_by_radius<pcl::PointNormal>(target_normal,2*resolution);pcl::PointCloud<pcl::PointNormal>::Ptr source_kpn = uniform_sample_by_radius<pcl::PointNormal>(source_normal,2 * resolution);cout << source_kpn->points.size() << endl;cout << target_kpn->points.size() << endl;//计算fpfh的值pcl::FPFHEstimation<pcl::PointNormal, pcl::PointNormal, pcl::FPFHSignature33> fpfh_est;pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal>);fpfh_est.setSearchMethod(tree);fpfh_est.setKSearch(30);//计算source特征pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);fpfh_est.setInputCloud(source_kpn);fpfh_est.setInputNormals(source_kpn);fpfh_est.compute(*source_fpfh);//计算target特征,如果特征点事pointnormal格式的话，setinputcloud和setinputnormals是点云指针。pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);fpfh_est.setInputCloud(target_kpn);fpfh_est.setInputNormals(target_kpn);fpfh_est.compute(*target_fpfh);cout << "fpfh值计算完成" << endl;vector<Eigen::Affine3f> picl_points;int tempt = 1;while (tempt>0){//开始进行sample_consensus_prerejective  基于采样一致性的配准方案pcl::PointCloud<pcl::PointNormal>::Ptr source_keyp_trans(new pcl::PointCloud<pcl::PointNormal>);Eigen::Matrix4f trans_pose = Eigen::Matrix4f::Identity();pcl::SampleConsensusPrerejective<pcl::PointNormal, pcl::PointNormal, pcl::FPFHSignature33> align;align.setInputSource(source_kpn);align.setSourceFeatures(source_fpfh);align.setInputTarget(target_kpn);align.setTargetFeatures(target_fpfh);align.setMaximumIterations(5000);      //最大迭代次数align.setNumberOfSamples(3);         //每次选几个点来进行配准 3-6最佳align.setCorrespondenceRandomness(10);   //配准时近邻的几个点作为备选项align.setSimilarityThreshold(0.7);   //相似性阈值，排除不相似的点align.setTransformationEpsilon(1e-6);  //两次迭代值小于值则认为收敛 e-3_e-8之间 //align.setEuclideanFitnessEpsilon(1e-4);  //匹配后的点云均方差小于值则提前收敛，这个值感觉不太好设置align.setMaxCorrespondenceDistance(3*resolution);  //对应点的最大距离阈值，超过此值则认为是外点  一般为2-3倍的特征点resolutionalign.setInlierFraction(0.6);    //内点占比大于此值时停止匹配,不可过高，不达标的的时候会匹配失败align.align(*source_keyp_trans);trans_pose = align.getFinalTransformation();cout << align.hasConverged() << endl;if (align.hasConverged()){pcl::PointCloud<pcl::PointXYZ>::Ptr source_find(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*source, *source_find, trans_pose);pcl::PointCloud<pcl::PointXYZ>::Ptr target_pick(new pcl::PointCloud<pcl::PointXYZ>);Eigen::Affine3f trans_pose_affine(trans_pose);Eigen::Affine3f pick_up = trans_pose_affine * pick_0;picl_points.push_back(pick_up);cout << "抓取点" << endl;cout << pick_up.matrix() << endl;pcl::transformPointCloud(*target, *target_pick, pick_up.inverse());show_pointcloud<pcl::PointXYZ>(target_pick, 2, 100);pcl::PointCloud<pcl::PointNormal>::Ptr target_k_tempt(new pcl::PointCloud<pcl::PointNormal>);pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_f_tempt(new pcl::PointCloud<pcl::FPFHSignature33>);vector<float> distance;vector<int> index;set<int> in_lier;//剔除匹配用到的关键点和关键特征pcl::search::KdTree<pcl::PointNormal>::Ptr tree3(new pcl::search::KdTree<pcl::PointNormal>);tree3->setInputCloud(target_kpn);for (auto point :source_keyp_trans->points){tree3->nearestKSearch(point, 20, index, distance);for (int i:index){in_lier.insert(i);}}for (int i =0;i< target_kpn->points.size();i++){if (in_lier.find(i)==in_lier.end()){target_k_tempt->push_back(target_kpn->points[i]);target_f_tempt->push_back(target_fpfh->points[i]);}}target_kpn = target_k_tempt;target_fpfh = target_f_tempt;//show_pointcloud<pcl::PointNormal>(target_kpn);}else { tempt = 0; }}return 0;
}