12.3 运动对象分割与配准算法实现
boost::filesystem::is_directory (dir_) //判断dir_是否为目录 boost::filesystem::directory_iterator itr(dir_) //迭代器,结合for循环遍历dir_内的所有内容 无参数的directory_iterator() 为迭代器终点
boost::shared_ptr
#include <string> #include <iostream> #include <boost/shared_ptr.hpp> class implementation { public: ~implementation() { std::cout <<"destroying implementation\n"; } void do_something() { std::cout << "did something\n"; } }; void test() { boost::shared_ptr<implementation> sp1(new implementation()); std::cout<<"The Sample now has "<<sp1.use_count()<<" references\n"; boost::shared_ptr<implementation> sp2 = sp1; std::cout<<"The Sample now has "<<sp2.use_count()<<" references\n"; sp1.reset(); std::cout<<"After Reset sp1. The Sample now has "<<sp2.use_count()<<" references\n"; sp2.reset(); std::cout<<"After Reset sp2.\n"; } void main() { test(); }
该程序的输出结果如下:
The Sample now has 1 references
The Sample now has 2 references
After Reset sp1. The Sample now has 1 references
destroying implementation
After Reset sp2.
可以看到,boost::shared_ptr指针sp1和sp2同时拥有了implementation对象的访问权限,且当sp1和sp2都释放对该对象的所有权时,其所管理的的对象的内存才被自动释放。在共享对象的访问权限同时,也实现了其内存的自动管理。
setAxis()、 setEpsAngle()
setAxis():用于设置所搜索平面垂直的轴
setEpsAngle():用于设置待检测的平面模型和上述设置轴的最大角度
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
double degree = 25, distance = 0.03, max_iteration = 10000; pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::PointIndices::Ptr index(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coe(new pcl::ModelCoefficients); seg.setModelType(pcl::SACMODEL_PLANE); seg.setAxis(Eigen::Vector3f(0, 1, 0)); seg.setEpsAngle(pcl::deg2rad(degree)); seg.setOptimizeCoefficients(true); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(max_iteration); seg.setDistanceThreshold(distance); seg.setInputCloud(cloud_filtered); seg.segment(*index, *coe); pcl::ExtractIndices<pcl::PointXYZRGB> ext; ext.setInputCloud(cloud_filtered); ext.setIndices(index); ext.setNegative(true); pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ncloud_ground_plane(new pcl::PointCloud < pcl::PointXYZRGB>()); ext.filter(*Ncloud_ground_plane); pcl::copyPointCloud(*Ncloud_ground_plane, *cloud_filtered); printf("show the data after delete ground plane"); showCloudstmp(cloud_filtered);
pcl::compute3DCentroid<pcl::PointXYZRGB, float>
计算质心#include <pcl/common/transforms.h>
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
//1 //其中clusters为聚类后的indices vector vector<Eigen::Vector4f> mass_centers; // 齐次坐标 mass_centers.resize(clusters_size); for (int i = 0; i < clusters_size; i++) { pcl::compute3DCentroid<pcl::PointXYZRGB, float>(*cloud_filtered, clusters[i], mass_centers[i]); } //2 Eigen::Vector4f centroid; // 质心 pcl::compute3DCentroid(*cloud, centroid); // 齐次坐标,(c0,c1,c2,1)
pcl::distances::l2(mass_centors[i],ROI_backmass)
求两个质心之间的距离
#include <iostream> #include <string.h> #include <boost/filesystem.hpp> #include <vector> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/common/common_headers.h> #include <pcl/filters/voxel_grid.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/filters/extract_indices.h> #include <pcl/PointIndices.h> #include <pcl/sample_consensus/model_types.h> #include <pcl/sample_consensus/method_types.h> #include <pcl/ModelCoefficients.h> #include <pcl/sample_consensus/sac_model_perpendicular_plane.h> #include <pcl/sample_consensus/ransac.h> #include <pcl/search/kdtree.h> #include <pcl/segmentation/extract_clusters.h> #include <pcl/common/transforms.h> #include <pcl/registration/icp.h> #include <pcl/registration/icp_nl.h> #include <pcl/registration/transforms.h> #include <pcl/common/angles.h> #include <pcl/visualization/cloud_viewer.h> #include <pcl/console/print.h> #include <pcl/registration/distances.h> using namespace std; boost::shared_ptr<pcl::visualization::PCLVisualizer> p; int vp_1, vp_2, vp_3; int cidx = -100; void pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie) { static int k; string str; cidx = event.getPointIndex(); if (cidx == -1) return; pcl::PointXYZRGB picked_pt; event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z); str = k; p->addSphere(picked_pt, 0.03, 1, 0, 0, str); k++; } void showCloudsLeft(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source) { p->removePointCloud("vp1_source"); p->setBackgroundColor(255, 255, 255); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> src_h(cloud_source, 255, 0, 0); p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1); pcl::console::print_info("visualization of source in first viewport,click q to continue\n"); p->spin(); } void showCloudstmp(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud) { p->addPointCloud(cloud, "cloud", vp_3); p->setBackgroundColor(255, 255, 255); p->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "cloud"); p->spin(); p->removePointCloud("cloud"); } //void showCloudstmp(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source) //{ // p->setBackgroundColor(255, 255, 255); // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> src_h(cloud_source, 0, 0, 255); // p->addPointCloud(cloud_source, src_h, "tmp", vp_3); // printf("visualization of tempresult in viewport 3,click q to continue\n"); // p->spin(); // p->removePointCloud("tmp"); //} void showCloudstmp(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source, Eigen::Matrix<float, 4, 1>& centroid_back, Eigen::Matrix<float, 4, 1>& centroid_forth) { p->setBackgroundColor(255, 255, 255); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> src_h(cloud_source, 0, 0, 255); p->addPointCloud(cloud_source, src_h, "tmpforthROI", vp_3); printf("visualization of tempresult in viewport 3,click q to continue\n"); pcl::PointXYZ picked_ptf, picked_ptb; picked_ptf.x = centroid_forth[0]; picked_ptf.y = centroid_forth[1]; picked_ptf.z = centroid_forth[2]; picked_ptb.x = centroid_back[0]; picked_ptb.y = centroid_back[1]; picked_ptb.z = centroid_back[2]; p->addSphere(picked_ptb, 0.03, 0, 1, 0, "tmpforthsphere"); p->addSphere(picked_ptf, 0.03, 0, 1, 0, "tmpbacksphere"); p->spin(); p->removePointCloud("tmpforthROI"); p->removeShape("tmpforthsphere"); p->removeShape("tmpbacksphere"); } void showCloudsRight(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target) { p->setBackgroundColor(255, 255, 255); p->removePointCloud("target"); pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> src_h(cloud_target, 0, 255, 0); p->addPointCloud(cloud_target, src_h, "target", vp_2); printf("visualization of tempresult in viewport 2,click q to continue\n"); p->spin(); } int main(int argc, char** argv) { bool voxel_filter = true, del_plane = true; Eigen::Matrix4f toFristT = Eigen::Matrix4f::Identity(); string dir_; cin >> dir_; vector<string> pcd_files_; vector<boost::filesystem::path> pcd_paths_; boost::filesystem::directory_iterator end_itr; printf("Begin to have pcd file list\n"); if (boost::filesystem::is_directory(dir_)) { for (boost::filesystem::directory_iterator itr(dir_); itr != end_itr; itr++) { string ext = itr->path().extension().string(); if (ext == ".pcd") { pcd_files_.push_back(itr->path().string()); pcd_paths_.push_back(itr->path()); } else { //Found nonpcd file PCL_DEBUG("[PCDVideoPlayer::selectFolderButtonPressed]:found a different file\n"); } } } else { PCL_ERROR("Path is not a directory\n"); exit(-1); } printf("Have pcd file list Successfuly\n"); //p.reset(new pcl::visualization::PCLVisualizer(argc, argv, "PCD viewer")); p.reset(new pcl::visualization::PCLVisualizer("PCD viewer")); p->createViewPort(0, 0, 0.5, 0.5, vp_1); p->createViewPort(0.5, 0, 1.0, 0.5, vp_2); p->createViewPort(0.0, 0.5, 1.0, 1.0, vp_3); pcl::PointCloud<pcl::PointXYZRGB>::Ptr back_cloud(new pcl::PointCloud<pcl::PointXYZRGB>()), forth_cloud(new pcl::PointCloud<pcl::PointXYZRGB>()), cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>()), All_raws(new pcl::PointCloud<pcl::PointXYZRGB>()), All_Traws(new pcl::PointCloud<pcl::PointXYZRGB>()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr ROI_back(new pcl::PointCloud<pcl::PointXYZRGB>()), ROI_forth(new pcl::PointCloud < pcl::PointXYZRGB>()); vector<pcl::PointCloud<pcl::PointXYZRGB> > ROI_list, ROIT_list; vector<Eigen::Matrix4f> R_Lforth2back; Eigen::Vector4f ROI_backmass, ROI_forthmass; int size_squences = pcd_files_.size(); cout << "Total file of squences is " << size_squences << endl; for (int i = 0; i < size_squences; i++) { pcl::io::loadPCDFile(pcd_files_[i], *back_cloud); Eigen::Quaternionf ori(1, 0, 0, 0); back_cloud->sensor_orientation_ = ori; if (voxel_filter == true) { pcl::VoxelGrid<pcl::PointXYZRGB> vg; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1(new pcl::PointCloud<pcl::PointXYZRGB>()); vg.setInputCloud(back_cloud); vg.setLeafSize(0.01, 0.01, 0.01); vg.filter(*cloud_filtered1); pcl::copyPointCloud(*cloud_filtered1, *cloud_filtered); printf("visualization of tmpresult in viewport 3, click q to continue\n"); showCloudstmp(cloud_filtered1); double degree = 25, distance = 0.03, max_iteration = 10000; pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::PointIndices::Ptr index(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coe(new pcl::ModelCoefficients); seg.setModelType(pcl::SACMODEL_PLANE); seg.setAxis(Eigen::Vector3f(0, 1, 0)); seg.setEpsAngle(pcl::deg2rad(degree)); seg.setOptimizeCoefficients(true); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(max_iteration); seg.setDistanceThreshold(distance); seg.setInputCloud(cloud_filtered); seg.segment(*index, *coe); pcl::ExtractIndices<pcl::PointXYZRGB> ext; ext.setInputCloud(cloud_filtered); ext.setIndices(index); ext.setNegative(true); pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ncloud_ground_plane(new pcl::PointCloud < pcl::PointXYZRGB>()); ext.filter(*Ncloud_ground_plane); pcl::copyPointCloud(*Ncloud_ground_plane, *cloud_filtered); printf("show the data after delete ground plane\n"); showCloudstmp(cloud_filtered); } if (del_plane == true) { double distance = 0.02, max_iteration = 10000; pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::PointIndices::Ptr index(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coe(new pcl::ModelCoefficients); seg.setModelType(pcl::SACMODEL_PLANE); seg.setOptimizeCoefficients(true); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(max_iteration); seg.setDistanceThreshold(distance); for (int i = 0; i < 1; i++) { seg.setInputCloud(cloud_filtered); seg.segment(*index, *coe); if (index->indices.size() == 0) { cout << "Cloud not estimate a planar model for the given dataset" << endl; break; } pcl::ExtractIndices<pcl::PointXYZRGB> ext; ext.setInputCloud(cloud_filtered); ext.setIndices(index); ext.setNegative(true); pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ncloud_ground_plane(new pcl::PointCloud < pcl::PointXYZRGB>()); ext.filter(*Ncloud_ground_plane); pcl::copyPointCloud(*Ncloud_ground_plane, *cloud_filtered); printf("show the data after delete ground plane\n"); showCloudstmp(cloud_filtered); } } double minn = 100, maxx = 100000, nab = 0.2; pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree < pcl::PointXYZRGB>); pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ext_cluster; vector<pcl::PointIndices> clusters; ext_cluster.setInputCloud(cloud_filtered); ext_cluster.setSearchMethod(tree); ext_cluster.setMaxClusterSize(maxx); ext_cluster.setMinClusterSize(minn); ext_cluster.setClusterTolerance(nab); ext_cluster.extract(clusters); int clusters_size = clusters.size(); vector<Eigen::Vector4f> mass_centers; // 齐次坐标 mass_centers.resize(clusters_size); for (int i = 0; i < clusters_size; i++) { pcl::compute3DCentroid<pcl::PointXYZRGB, float>(*cloud_filtered, clusters[i], mass_centers[i]); } double minimum_d = 100000; int minimum_g; if (i == 0) { p->registerPointPickingCallback(&pp_callback, static_cast<void*> (&cloud_filtered)); p->addPointCloud(cloud_filtered, "cloud_filtered", vp_3); cout << "Please press shift+left click chose one seed get the first ROI group" << endl; p->spin(); bool found = false; for (int i = 0; i < clusters_size; i++) { if (find(clusters[i].indices.begin(), clusters[i].indices.end(), cidx) != clusters[i].indices.end()) { pcl::copyPointCloud(*cloud_filtered, clusters[i], *ROI_back); p->removePointCloud("cloud_filtered", vp_3); p->addPointCloud(ROI_back, "ROI_back"); ROI_backmass = mass_centers[i]; cout << "visualization of ROI in viewer_port 3, press q to continue" << endl; p->spin(); found = true; break; } } p->removePointCloud("ROI_back", vp_3); continue; } else { for (int i = 0; i < clusters_size; i++) { double temp = pcl::distances::l2(mass_centers[i], ROI_backmass); if (temp < minimum_d) { minimum_d = temp; minimum_g = i; ROI_forthmass = mass_centers[i]; } } pcl::copyPointCloud(*cloud_filtered, clusters[i], *ROI_forth); showCloudstmp(ROI_forth, ROI_backmass, mass_centers[minimum_g]); } pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp; icp.setInputCloud(ROI_forth); icp.setInputCloud(ROI_back); icp.setMaximumIterations(5000); icp.setMaxCorrespondenceDistance(0.1); icp.setRANSACOutlierRejectionThreshold(0.01); icp.setEuclideanFitnessEpsilon(0.1); icp.setTransformationEpsilon(1e-8); pcl::PointCloud<pcl::PointXYZRGB>::Ptr Final(new pcl::PointCloud<pcl::PointXYZRGB>()), temp_forthT(new pcl::PointCloud<pcl::PointXYZRGB>()); icp.align(*Final); Eigen::Matrix4f T_forth2back = icp.getFinalTransformation(); toFristT *= T_forth2back; ROI_list.push_back(*ROI_back); pcl::transformPointCloud(*ROI_forth, *temp_forthT, toFristT); if (i == 1) { *All_Traws += *ROI_back; *All_Traws += *temp_forthT; *All_raws += *ROI_back; *All_raws += *ROI_forth; } else if (i == size_squences - 1) { ROI_list.push_back(*ROI_forth); *All_raws += *ROI_forth; *All_Traws += *temp_forthT; } else { *All_Traws += *temp_forthT; *All_raws += *ROI_forth; } showCloudsRight(All_Traws); showCloudsLeft(All_raws); *ROI_back = *ROI_forth; } return 0; }
自己选择的路,跪着也要走完。朋友们,虽然这个世界日益浮躁起来,只要能够为了当时纯粹的梦想和感动坚持努力下去,不管其它人怎么样,我们也能够保持自己的本色走下去。