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()：用于设置待检测的平面模型和上述设置轴的最大角度

            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>

//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;
}