【三维点云数据处理】ISS3d+CSHOT+RANSAC+ICP

系列文章目录

【三维点云数据处理】ISS特征点提取算法

【三维点云数据处理】SHOT三维特征描述子

【三维点云数据处理】RANSAC实现点云粗配准



前言

        利用ISS3d+CSHOT+RANSAC+ICP来实现点云的配准。前面已经将ISS3D、SHOT三位特征描述子、RANSAC粗配准都进行了讲解,接下来将这些结合起来实现点云的粗配准,最后使用ICP进行优化,最终实现点云的配准。


一、算法原理

        首先利用ISS3D算法找到点云P和点云Q的关键点,ISS3D具有旋转不变性的特点,所以理论上两幅点云的相同特征具有相同的关键点。检测出两幅点云的关键点之后,利用SHOT三维特征描述子,相同的关键点具有相同的三维特征,具有相同特征的关键点,我们就把它看作一对对应点,理论上我们只要找到三对不共面的对应点,就可以实现点云的匹配。但是,我们在计算特征描述子时,肯定不可避免的会有误匹配,所以我们可以利用RANSAN来去除误匹配的同时,计算出最优的变换矩阵。最后在利用ICP进行优化。

二、代码实现

1.头文件

/*
* Parameter Setting
* Author: Team SLAMer, University of Trento
* Supervisor: MARIOLINO DE CECCO, ALESSANDRO LUCHETTI
* Ref:
*   [1]
*   [2]
*/

#include <iostream>
#include <fstream>
#include <cmath>
#include <time.h>
#include <vector>


namespace datastream
{
    // transXYZ(+6,0,0) rotXYZ(0,+30,0)
    //std::string filename_sparse_src = "pointcloud_sparse_src_InTheTorii.pcd";
    //std::string filename_sparse_tar = "pointcloud_sparse_tar_InTheTorii.pcd";

    // transXYZ(0,0,0) rotXYZ(0,+30,0)
    std::string filename_dense_src = "pointcloud_dense_src_WarriorInTheHouse.pcd";
    std::string filename_dense_tar = "pointcloud_dense_tar_WarriorInTheHouse.pcd";

    std::string filename_noNaN_src = "pointcloud_src_noNaN.pcd";
    std::string filename_noNaN_tar = "pointcloud_tar_noNaN.pcd";

    std::string filename_sift_src = "pointcloud_src_sift_keypoints.pcd";
    std::string filename_sift_tar = "pointcloud_tar_sift_keypoints.pcd";

    std::string filename_iss3d_src = "pointcloud_src_iss3d_keypoints.pcd";
    std::string filename_iss3d_tar = "pointcloud_tar_iss3d_keypoints.pcd";
    
    std::string filename_normal_src = "iCloud_src_normals.pcd";
    std::string filename_normal_tar = "iCloud_tar_normals.pcd";

    std::string filename_SAC_trans_src = "pointcloud_src_RANSAC_transformed.pcd";
    std::string filename_SAC_ICP_trans_src = "pointcloud_src_ICP_transformed_src.pcd";

    std::string filename_desc_cshot_src = "rCSHOTDesc_src.pcd";
    std::string filename_desc_shot_src = "rSHOTDesc_src.pcd";
    std::string filename_desc_si_src = "rSpinImageDesc_src.pcd";
    std::string filename_desc_fpfh_src = "rFPFHDesc_src.pcd";
    std::string filename_desc_sift_src = "rSIFTDesc_src.pcd";

    std::string filename_desc_cshot_tar = "rCSHOTDesc_tar.pcd";
    std::string filename_desc_shot_tar = "rSHOTDesc_tar.pcd";
    std::string filename_desc_si_tar = "rSpinImageDesc_tar.pcd";
    std::string filename_desc_fpfh_tar = "rFPFHDesc_tar.pcd";
    std::string filename_desc_sift_tar = "rSIFTDesc_tar.pcd";

    std::string filename_pip_cshot = "pip_evaluation_ISS3D+CSHOT+RANSAC+ICP.txt";
    std::string filename_pip_shot = "pip_evaluation_SIFT+SHOT+RANSAC+ICP.txt";
    std::string filename_pip_si = "pip_evaluation_SIFT+SpinImage+RANSAC+ICP.txt";
    std::string filename_pip_fpfh = "pip_evaluation_SIFT+FPFH+RANSAC+ICP.txt";
    std::string filename_pip_sift = "pip_evaluation_SIFT+RANSAC+ICP.txt";
}

// Normal Estimation
namespace normalparm
{
    // Warrior and Cat scene: r = 1.0 (no nan)
    //                        r = 0.5 no nan
    // 鸟居                   r = 
    double radius_ic_sparse = 1.0;

    // Small flat furniture scene:
    //      r = 0.3 no nan at all(ok, better)
    //      r = 0.2 (a little nan)(ok)
    double radius_ic_dense = 0.3;

    // r = 2.0~2.5(ok for si,2.5 no nan)
    double radius_kp_dense = 2.5;
    //int knn = 20;
}

// Feature Detectors
namespace iss3dparm
{
    int minKnn = 5;
    double threshold21 = 0.975;
    double threshold32 = 0.975;
    double salientRadius = 2.4;
    double nonMaxRadius = 1.6;
    double nonNormalRadius = 2.4;
    double nonBorderRadius = 1.6;
    //float angleThreshold = static_cast<float> (M_PI) / 2.0;
}
namespace siftparm
{
    const float min_scale = 0.1f;
    const int n_octaves = 6;
    const int n_scales_per_octave = 4;
    const float min_contrast = 0.1f;
}

// Feature Descriptors
namespace fpfhparm
{
    double radius_sparse = 3.0;

    // r = 0.2, no nan(ok)
    double radius_dense = 0.2;
}

namespace siparm
{
    unsigned int image_width = 8U;
    double support_ang_cos = 0.5;
    unsigned int min_pts_neighb = 16U;

    double radius_sparse = 3.0;

    // r = 1.0(ok for si)
    double radius_dense = 1.0;
}

namespace shotparm
{
    // Warrior and Cat scene: r = 1.0 (no nan)
    //                        r = 
    // 鸟居                   r = 2?
    double radius_sparse = 3.0;
    // Dense: 0.1(x), 0.2(x), 0.3(ok)
    double radius_dense = 0.3;
}
namespace cshotparm
{
    double radiusLRF = 0.2;

    // Warrior and Cat scene: r = 3.0 (no nan in desc)
    // Torii and Cat scene: r = 1.0 (no nan)
    double radius_sparse = 3.0;

    // Warrior in the house scene
    //  r = 0.1, no nan(ok)
    //  r = 0.15 no nan(ok)
    double radius_dense = 0.15;
}

namespace vfhparm
{
    double radius_sparse = 3.0;

    double radius_dense = 0.15;
}

// Registeration
namespace sacparm
{
    int max_iterations = 2000;
    int num_sampels = 3;
    int corresp_randomness = 20;
    float max_corresp_dist = 1.25f;
    float inlier_fraction = 0.25f;
    float similarity_threshold = 0.9f;
}

namespace icpparm
{
    float max_corresp_dist = 20.0f; // most important
    int max_iterations = 10000;
    double trans_epsilon = 1e-10;
    double euclidean_fitness_epsilon = 0.2;
}

// Point Cloud Visualization
namespace visualparm
{
    float srcRGBparm[] = { 255.0f, 255.0f, 255.0f };
    float tarRGBparm[] = { 255.0f, 0.0f, 0.0f };
    float finalRGBparm[] = { 0.0f, 0.0f, 255.0f };
}
#ifndef ISS3D_SHOT_RANSAC_ICP_H
#define ISS3D_SHOT_RANSAC_ICP_H

#include"f_pipeline_setup.h"
#include <string>
// Basics
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <Eigen/Dense>
#include <pcl/console/time.h>

// Feature Extraction
#include <pcl/filters/filter.h>
#include <pcl/keypoints/iss_3d.h>


// Feature Descriptors
#include <pcl/features/normal_3d.h>
#include <pcl/features/shot.h>
#include <pcl/features/shot_lrf.h>
#include <pcl/search/kdtree.h>

//downsample
#include <pcl/filters/voxel_grid.h>


// Registration
#include <pcl/registration/correspondence_estimation.h>
#include <boost/thread/thread.hpp>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp.h>

// Visualization
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化

using pcl::NormalEstimation;
using pcl::search::KdTree;
using namespace datastream;
using namespace iss3dparm;

// typedef shortcut
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::SHOT1344 SHOTSignT;
typedef pcl::PointCloud<SHOTSignT> PointCloudSHOTDesc;
typedef pcl::ReferenceFrame PointRef;

class iss3d_Shot_Ransac_Icp
{
public:
    iss3d_Shot_Ransac_Icp();
    // Registration Visualization
    void visualize_registration(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tar, PointCloud::Ptr pcd_final);
    void visualize_keypoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_src_og, pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_keypoints);
    double computeCloudResolution(const pcl::PointCloud<PointT>::ConstPtr& cloud);

    int fpip_ISS3D_CSHOT_RANSAC_ICP();
};

#endif // ISS3D_SHOT_RANSAC_ICP_H

2.源文件

#pragma once


#include "iss3d_shot_ransac_icp.h"




iss3d_Shot_Ransac_Icp::iss3d_Shot_Ransac_Icp()
{

}

// Registration Visualization
void iss3d_Shot_Ransac_Icp::visualize_registration(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tar, PointCloud::Ptr pcd_final)
{
    int vp_1, vp_2;

    // Create a PCLVisualizer object
    pcl::visualization::PCLVisualizer viewer("Registration Viewer");

    viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
    viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);

    pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h(pcd_src, visualparm::srcRGBparm[0],
        visualparm::srcRGBparm[1],
        visualparm::srcRGBparm[2]);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> tar_h(pcd_tar, visualparm::tarRGBparm[0],
        visualparm::tarRGBparm[1],
        visualparm::tarRGBparm[2]);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> final_h(pcd_final, visualparm::finalRGBparm[0],
        visualparm::finalRGBparm[1],
        visualparm::finalRGBparm[2]);

    viewer.setBackgroundColor(1.0, 1.0, 1.0);

    viewer.addPointCloud(pcd_src, src_h, "source cloud");
    viewer.addPointCloud(pcd_tar, tar_h, "tgt cloud");
    viewer.addPointCloud(pcd_final, final_h, "final cloud");
    //viewer.addCoordinateSystem(1.0);
    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}


void iss3d_Shot_Ransac_Icp::visualize_keypoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_src_og,
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_keypoints)
{
    pcl::visualization::PCLVisualizer viewer("Sift keypoint");
    viewer.setBackgroundColor(0, 0, 0);

    viewer.addPointCloud(pcd_src_og, "OG point cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "OG point cloud");

    viewer.addPointCloud(pcd_keypoints, "Keypoints");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "Keypoints");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 255, 0, "Keypoints");

    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
        boost::this_thread::sleep(boost::posix_time::microseconds(300));
    }
}


double iss3d_Shot_Ransac_Icp::computeCloudResolution(const pcl::PointCloud<PointT>::ConstPtr& cloud)
{
    double res = 0.0;
    int n_points = 0;
    int nres;
    std::vector<int> indices(2);
    std::vector<float> sqr_distances(2);
    pcl::search::KdTree<PointT> tree;
    tree.setInputCloud(cloud);

    for (size_t i = 0; i < cloud->size(); ++i)
    {
        if (!pcl_isfinite((*cloud)[i].x))
        {
            continue;
        }

        nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
        if (nres == 2)
        {
            res += sqrt(sqr_distances[1]);
            ++n_points;
        }
    }

    if (n_points != 0)
    {
        res /= n_points;
    }
    return res;
}

int iss3d_Shot_Ransac_Icp::fpip_ISS3D_CSHOT_RANSAC_ICP()
{

    std::cout << "------> Start ... Run Miumiu Run ..." << std::endl;
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Load point cloud data
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // create xyzrgb.pcd containers and load point cloud data to them
    PointCloud::Ptr rCloud_src(new PointCloud);
    PointCloud::Ptr rCloud_tar(new PointCloud);

    if (pcl::io::loadPCDFile<PointT>(datastream::filename_dense_src, *rCloud_src) == -1)
    {
        PCL_ERROR("Cannot load source.pcd file\n");
        return -1;
    }
    if (pcl::io::loadPCDFile<PointT>(datastream::filename_dense_tar, *rCloud_tar) == -1)
    {
        PCL_ERROR("Cannot load target.pcd file\n");
        return -1;
    }
    std::cout << "------> PCD Loaded ... " << std::endl;


    std::cout << "------> Downsanmple......" << std::endl;
    //体素下采样
    pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid_src;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_down_src(new pcl::PointCloud<pcl::PointXYZRGB>);
    voxel_grid_src.setInputCloud(rCloud_src);
    voxel_grid_src.setLeafSize(downsampleparm::LeafSize, downsampleparm::LeafSize, downsampleparm::LeafSize);
    voxel_grid_src.filter(*pcd_down_src);

    std::cout << "------> src下采样后点个数:" << pcd_down_src->points.size() <<  std::endl;

    pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid_tar;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_down_tar(new pcl::PointCloud<pcl::PointXYZRGB>);
    voxel_grid_tar.setInputCloud(rCloud_src);
    voxel_grid_tar.setLeafSize(downsampleparm::LeafSize, downsampleparm::LeafSize, downsampleparm::LeafSize);
    voxel_grid_tar.filter(*pcd_down_tar);

    std::cout << "------> tar下采样后点个数:" << pcd_down_tar->points.size() <<  std::endl;


    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Point Cloud cleaning and Smooth surface normal estimation
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Remove NaN cloud points
    PointCloud::Ptr iCloud_src(new PointCloud);
    PointCloud::Ptr iCloud_tar(new PointCloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr wCloud_src(new  pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr wCloud_tar(new  pcl::PointCloud<pcl::PointXYZ>);

    std::vector<int> indices_src;
    pcl::removeNaNFromPointCloud(*pcd_down_src, *iCloud_src, indices_src);
    pcl::io::savePCDFileASCII(datastream::filename_noNaN_src, *iCloud_src);
    std::cout << "------> Remove NaN points from *rCloud_src to *iCloud_src ...[saved]" << std::endl;
    pcl::copyPointCloud(*iCloud_src, *wCloud_src);

    std::vector<int> indices_tar;
    pcl::removeNaNFromPointCloud(*pcd_down_src, *iCloud_tar, indices_tar);
    pcl::io::savePCDFileASCII(datastream::filename_noNaN_tar, *iCloud_tar);
    std::cout << "------> Remove NaN points from *rCloud_tar to *iCloud_tar ...[saved]" << std::endl;
    pcl::copyPointCloud(*iCloud_tar, *wCloud_tar);


    pcl::search::KdTree<PointT>::Ptr modelKdTree_src(new pcl::search::KdTree<PointT>);
    modelKdTree_src->setInputCloud(iCloud_src);
    pcl::search::KdTree<PointT>::Ptr modelKdTree_tar(new pcl::search::KdTree<PointT>);
    modelKdTree_tar->setInputCloud(iCloud_tar);

    clock_t tik_normals_a = clock();

    // A: Calc normals for iCloud_src and iCloud_tar
    pcl::NormalEstimation<PointT, pcl::Normal> ne_iCloud_src;
    ne_iCloud_src.setSearchMethod(modelKdTree_src);
    ne_iCloud_src.setRadiusSearch(normalparm::radius_ic_dense);
    ne_iCloud_src.setInputCloud(iCloud_src);
    pcl::PointCloud<pcl::Normal>::Ptr iCloud_src_normals(new pcl::PointCloud<pcl::Normal>);
    ne_iCloud_src.compute(*iCloud_src_normals);
    std::cout << "------> Got *iCloud_src_normals ..." << std::endl;

    pcl::NormalEstimation<PointT, pcl::Normal> ne_iCloud_tar;
    ne_iCloud_tar.setSearchMethod(modelKdTree_tar);
    ne_iCloud_tar.setRadiusSearch(normalparm::radius_ic_dense);
    ne_iCloud_tar.setInputCloud(iCloud_tar);
    pcl::PointCloud<pcl::Normal>::Ptr iCloud_tar_normals(new pcl::PointCloud<pcl::Normal>);
    ne_iCloud_tar.compute(*iCloud_tar_normals);
    std::cout << "------> Got *iCloud_tar_normals ..." << std::endl;

    pcl::io::savePCDFileASCII(datastream::filename_normal_src, *iCloud_src_normals);
    pcl::io::savePCDFileASCII(datastream::filename_normal_tar, *iCloud_tar_normals);

    /*
    // Read computed normals to boost the test
    pcl::PointCloud<pcl::Normal>::Ptr iCloud_src_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Normal>::Ptr iCloud_tar_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::io::loadPCDFile("boost_src_normals.pcd", *iCloud_src_normals);
    pcl::io::loadPCDFile("boost_tar_normals.pcd", *iCloud_tar_normals);
    pcl::io::savePCDFileASCII(filename_normal_src, *iCloud_src_normals);
    pcl::io::savePCDFileASCII(filename_normal_tar, *iCloud_tar_normals);
    std::cout << "------> Got *iCloud_src_normals ..." << std::endl;
    std::cout << "------> Got *iCloud_tar_normals ..." << std::endl;
    */
    clock_t tik_normals_b = clock();

    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Feature Extraction: run ISS3D
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    clock_t tik_detect_a = clock();
    // create ISS3D object
    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_det_src;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_iss_src(new pcl::search::KdTree<pcl::PointXYZ>());
    iss_det_src.setInputCloud(wCloud_src);
    iss_det_src.setSearchMethod(tree_iss_src);
    double iResolution_src = computeCloudResolution(iCloud_src);
    iss_det_src.setSalientRadius(6 * iResolution_src);
    iss_det_src.setNonMaxRadius(4 * iResolution_src);
    iss_det_src.setNormalRadius(6 * iResolution_src);
    iss_det_src.setBorderRadius(4 * iResolution_src);
    iss_det_src.setAngleThreshold(static_cast<float> (M_PI) / 2.0);
    iss_det_src.setMinNeighbors(iss3dparm::minKnn);
    iss_det_src.setThreshold21(iss3dparm::threshold21);
    iss_det_src.setThreshold32(iss3dparm::threshold32);
    pcl::PointCloud<pcl::PointXYZ>::Ptr rKeypoints_src(new  pcl::PointCloud<pcl::PointXYZ>);
    iss_det_src.setNumberOfThreads(6);
    iss_det_src.compute(*rKeypoints_src);
    PointCloud::Ptr iKeypoints_src(new PointCloud);
    pcl::PointIndices idx_kp_src = *iss_det_src.getKeypointsIndices();
    pcl::copyPointCloud(*iCloud_src, idx_kp_src, *iKeypoints_src);

    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_det_tar;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_iss_tar(new pcl::search::KdTree<pcl::PointXYZ>());
    iss_det_tar.setInputCloud(wCloud_tar);
    iss_det_tar.setSearchMethod(tree_iss_tar);
    double iResolution_tar = computeCloudResolution(iCloud_tar);
    iss_det_tar.setSalientRadius(6 * iResolution_tar);
    iss_det_tar.setNonMaxRadius(4 * iResolution_tar);
    iss_det_tar.setNormalRadius(6 * iResolution_tar);
    iss_det_tar.setBorderRadius(4 * iResolution_tar);
    iss_det_tar.setAngleThreshold(static_cast<float> (M_PI) / 2.0);
    iss_det_tar.setMinNeighbors(iss3dparm::minKnn);
    iss_det_tar.setThreshold21(iss3dparm::threshold21);
    iss_det_tar.setThreshold32(iss3dparm::threshold32);
    pcl::PointCloud<pcl::PointXYZ>::Ptr rKeypoints_tar(new  pcl::PointCloud<pcl::PointXYZ>);
    iss_det_tar.setNumberOfThreads(6);
    iss_det_tar.compute(*rKeypoints_tar);
    PointCloud::Ptr iKeypoints_tar(new PointCloud);
    pcl::PointIndices idx_kp_tar = *iss_det_tar.getKeypointsIndices();
    pcl::copyPointCloud(*iCloud_tar, idx_kp_tar, *iKeypoints_tar);

    clock_t tik_detect_b = clock();

    pcl::io::savePCDFileASCII(datastream::filename_iss3d_src, *iKeypoints_src);
    std::cout << "------> ISS3Ding: size *iCloud_src to *iKeypoints_src from [ " << iCloud_src->size() << " -> " << iKeypoints_src->size() << " ]"
        << " ... [saved]" << std::endl;

    pcl::io::savePCDFileASCII(datastream::filename_iss3d_tar, *iKeypoints_tar);
    std::cout << "------> ISS3Ding: size *iCloud_tar to *iKeypoints_tar from [ " << iCloud_tar->size() << " -> " << iKeypoints_tar->size() << " ]"
        << " ... [saved]" << std::endl;

    visualize_keypoints(iCloud_src, rKeypoints_src);

    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Feature Descriptor: compute SHOTColor feature descriptors
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    clock_t tik_descriptors_a = clock();

    // Calc SHOTColor feature descriptors
    pcl::SHOTColorEstimation<PointT, pcl::Normal, SHOTSignT, PointRef> cshot_src;
    //pcl::search::KdTree<PointT>::Ptr tree_cshot_src(new pcl::search::KdTree<PointT>);
    cshot_src.setSearchMethod(modelKdTree_src);
    cshot_src.setInputCloud(iKeypoints_src);
    cshot_src.setInputNormals(iCloud_src_normals);
    cshot_src.setRadiusSearch(cshotparm::radius_dense);
    cshot_src.setSearchSurface(iCloud_src);
    PointCloudSHOTDesc::Ptr rCSHOTDesc_src(new PointCloudSHOTDesc());
    cshot_src.compute(*rCSHOTDesc_src);
    std::cout << "------> Computed *rCSHOTDesc_src SHOTColor descriptor values ..." << std::endl;
    std::cout << rCSHOTDesc_src->is_dense << std::endl;
    std::cout << rCSHOTDesc_src->size() << std::endl;

    // ---------------直方图可视化-----------------

//    //定义绘图器
//    pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter("My Plotter");
//    //设置特性
//    plotter->setTitle("SHOT");
//    plotter->setShowLegend(true);
//    cout << pcl::getFieldsList<SHOTSignT>(*rCSHOTDesc_src)<< endl;;
//    plotter->addFeatureHistogram<SHOTSignT>(*rCSHOTDesc_src, "rf", 5, "one_SHOT");/*第2个参数为点云类型的field name,5表示可视化第五个点的FPFH特征该参数可通过getFieldsList()返回,并且只限定于注册过的点云类型*/
//    plotter->setWindowSize(800, 600);
//    plotter->spinOnce(30000000);
//    plotter->clearPlots();

    pcl::SHOTColorEstimation<PointT, pcl::Normal, SHOTSignT, PointRef> cshot_tar;
    //pcl::search::KdTree<PointT>::Ptr tree_cshot_tar(new pcl::search::KdTree<PointT>);
    cshot_tar.setSearchMethod(modelKdTree_tar);
    cshot_tar.setInputCloud(iKeypoints_tar);
    cshot_tar.setInputNormals(iCloud_tar_normals);
    cshot_tar.setRadiusSearch(cshotparm::radius_dense);
    cshot_tar.setSearchSurface(iCloud_tar);
    PointCloudSHOTDesc::Ptr rCSHOTDesc_tar(new PointCloudSHOTDesc());
    cshot_tar.compute(*rCSHOTDesc_tar);
    std::cout << "------> Computed *rCSHOTDesc_tar SHOTColor descriptor values ..." << std::endl;
    std::cout << rCSHOTDesc_tar->is_dense << std::endl;
    std::cout << rCSHOTDesc_tar->size() << std::endl;

    // ---------------直方图可视化-----------------

    //定义绘图器
    //pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter("My Plotter");
    //设置特性
//    plotter->setTitle("SHOT");
//    plotter->setShowLegend(true);
//    cout << pcl::getFieldsList<SHOTSignT>(*rCSHOTDesc_tar)<< endl;;
//    plotter->addFeatureHistogram<SHOTSignT>(*rCSHOTDesc_tar, "shot", 1, "one_SHOT");/*第2个参数为点云类型的field name,5表示可视化第五个点的FPFH特征该参数可通过getFieldsList()返回,并且只限定于注册过的点云类型*/
//    plotter->setWindowSize(800, 600);
//    plotter->spinOnce(30000000);
//    plotter->clearPlots();

    clock_t tik_descriptors_b = clock();

    pcl::io::savePCDFileASCII(datastream::filename_desc_cshot_src, *rCSHOTDesc_src);
    pcl::io::savePCDFileASCII(datastream::filename_desc_cshot_tar, *rCSHOTDesc_tar);

    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Coarse Registration: RANSAC
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // RANSAC  Coarse Regisration
    clock_t tik_ransac_a = clock();

    pcl::SampleConsensusInitialAlignment<PointT, PointT, SHOTSignT> scia;
    PointCloud::Ptr sac_result(new PointCloud);
    // RANSAC setup
    scia.setInputSource(iKeypoints_src);
    scia.setInputTarget(iKeypoints_tar);
    scia.setSourceFeatures(rCSHOTDesc_src);
    scia.setTargetFeatures(rCSHOTDesc_tar);
    scia.setMaximumIterations(sacparm::max_iterations);
    scia.setNumberOfSamples(sacparm::num_sampels);
    scia.setCorrespondenceRandomness(sacparm::corresp_randomness);
    //scia.setMaxCorrespondenceDistance(sacparm::max_corresp_dist);
    //scia.setInlierFraction(sacparm::inlier_fraction);
    //scia.setSimilarityThreshold(sacparm::similarity_threshold)

    scia.align(*sac_result);
    std::cout << "------> RANSAC has converged:" << scia.hasConverged() << "  Score: " << scia.getFitnessScore() << std::endl;

    Eigen::Matrix4f sac_rotTrans;
    sac_rotTrans = scia.getFinalTransformation();
    std::cout << "------> RANSAC Rot-Translation Matrix:" << std::endl;
    std::cout << sac_rotTrans << std::endl;

    clock_t tik_ransac_b = clock();

    pcl::io::savePCDFileASCII(datastream::filename_SAC_trans_src, *sac_result);

    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Fine Registration: ICP
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    clock_t tik_icp_a = clock();

    // ICP regisrater
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    PointCloud::Ptr icp_result(new PointCloud);

    // ICP setup
    icp.setInputSource(iKeypoints_src);
    icp.setInputTarget(iCloud_tar);

    icp.setMaxCorrespondenceDistance(icpparm::max_corresp_dist);
    icp.setMaximumIterations(icpparm::max_iterations);
    icp.setTransformationEpsilon(icpparm::trans_epsilon);
    icp.setEuclideanFitnessEpsilon(icpparm::euclidean_fitness_epsilon);
    icp.align(*icp_result, sac_rotTrans);

    std::cout << "------> ICP has converged:" << icp.hasConverged() << "  Score: " << icp.getFitnessScore() << std::endl;

    clock_t tik_icp_b = clock();

    Eigen::Matrix4f icp_rotTrans;
    icp_rotTrans = icp.getFinalTransformation();
    std::cout << "------> ICP Rot-Translation Matrix:" << std::endl;
    std::cout << icp_rotTrans << std::endl;

    pcl::transformPointCloud(*iCloud_src, *icp_result, icp_rotTrans);
    pcl::io::savePCDFileASCII(datastream::filename_SAC_ICP_trans_src, *icp_result);

    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Visualization
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    visualize_registration(iCloud_src, iCloud_tar, icp_result);

    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Pipeline Evaluation
    // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Evaluate Time Cost
    std::cout << "------> Total time     : " << (double)(tik_icp_b - tik_normals_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    std::cout << "------> ISS3D time     : " << (double)(tik_detect_b - tik_detect_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    std::cout << "------> Normals time   : " << (double)(tik_normals_b - tik_normals_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    std::cout << "------> CSHOT time     : " << (double)(tik_descriptors_b - tik_descriptors_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    std::cout << "------> RANSAC time    : " << (double)(tik_ransac_b - tik_ransac_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    std::cout << "------> ICP time       : " << (double)(tik_icp_b - tik_icp_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;

    // Evaluate RMSE
    pcl::registration::CorrespondenceEstimation<PointT, PointT> corre;
    corre.setInputSource(icp_result);
    corre.setInputTarget(iCloud_tar);

    pcl::Correspondences all_corre;

    corre.determineReciprocalCorrespondences(all_corre);

    float sum = 0.0, sum_x = 0.0, sum_y = 0.0, sum_z = 0.0, rmse, rmse_x, rmse_y, rmse_z;

    std::vector<float> co;

    for (size_t j = 0; j < all_corre.size(); j++) {
        sum += all_corre[j].distance;
        co.push_back(all_corre[j].distance);
        sum_x += pow((iCloud_tar->points[all_corre[j].index_match].x - icp_result->points[all_corre[j].index_query].x), 2);
        sum_y += pow((iCloud_tar->points[all_corre[j].index_match].y - icp_result->points[all_corre[j].index_query].y), 2);
        sum_z += pow((iCloud_tar->points[all_corre[j].index_match].z - icp_result->points[all_corre[j].index_query].z), 2);
    }

    rmse = sqrt(sum / all_corre.size());        // Total RMSE on XYZ
    rmse_x = sqrt(sum_x / all_corre.size());    // RMSE on X-axis
    rmse_y = sqrt(sum_y / all_corre.size());    // RMSE on X-axis
    rmse_z = sqrt(sum_z / all_corre.size());    // RMSE on X-axis

    std::vector<float>::iterator max = max_element(co.begin(), co.end());       // Get the Farthest cloud point pair
    std::vector<float>::iterator min = min_element(co.begin(), co.end());       // Get the Nearest cloud point pair

    std::cout << "------> Number of correspondent points: " << all_corre.size() << std::endl;
    std::cout << "------> Maximum distance: " << sqrt(*max) * 100 << "cm" << std::endl;
    std::cout << "------> Minimum distance: " << sqrt(*min) * 100 << "cm" << std::endl;

    std::cout << "------> Total RMSE     :" << rmse << " m" << std::endl;
    std::cout << "------> RMSE on x-axis :" << rmse_x << " m" << std::endl;
    std::cout << "------> RMSE on y-axis :" << rmse_y << " m" << std::endl;
    std::cout << "------> RMSE on z-axis :" << rmse_z << " m" << std::endl;

    // Record the pipeline evaluation
    ofstream fout(datastream::filename_pip_cshot);
    fout << "Pipeline Evaluation Result\n";
    fout << "Purpose             : 3D XYZRGB Point Cloud Registration\n";
    fout << "Keypoints Detector  : ISS3D\n";
    fout << "Featrue Descriptor  : CSHOT\n";
    fout << "Coarse Registration : RANSAC\n";
    fout << "Fina Registration   : ICP\n";
    fout << "Author              : @Team SLAMer\n";
    fout << "Supervisor          : MARIOLINO DE CECCO, ALESSANDRO LUCHETTI\n";

    fout << "\nTime Evaluation\n";
    fout << "------> Total time     : " << (double)(tik_icp_b - tik_normals_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    fout << "------> ISS3D time     : " << (double)(tik_detect_b - tik_detect_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    fout << "------> Normals time   : " << (double)(tik_normals_b - tik_normals_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    fout << "------> CSHOT time     : " << (double)(tik_descriptors_b - tik_descriptors_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    fout << "------> RANSAC time    : " << (double)(tik_ransac_b - tik_ransac_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;
    fout << "------> ICP time       : " << (double)(tik_icp_b - tik_icp_a) / (double)CLOCKS_PER_SEC << " s" << std::endl;

    fout << "\nRMSE Error Evaluation\n";
    fout << "------> Number of correspondent points: " << all_corre.size() << std::endl;
    fout << "------> Maximum distance: " << sqrt(*max) * 100 << "cm" << std::endl;
    fout << "------> Minimum distance: " << sqrt(*min) * 100 << "cm" << std::endl;
    fout << "------> Total RMSE     :" << rmse << " m" << std::endl;
    fout << "------> RMSE on x-axis :" << rmse_x << " m" << std::endl;
    fout << "------> RMSE on y-axis :" << rmse_y << " m" << std::endl;
    fout << "------> RMSE on z-axis :" << rmse_z << " m" << std::endl;

    fout.close();

    std::cout << "------> Saved evaluation result '" << datastream::filename_pip_cshot << "', run run Miumiu run...\n";

    return (0);
}

三、实现结果

配准矩阵:

 

 配准结果:

------> Start ... Run Miumiu Run ...
------> PCD Loaded ...
------> Downsanmple......
------> src下采样后点个数:3566
------> tar下采样后点个数:3566
------> Remove NaN points from *rCloud_src to *iCloud_src ...[saved]
------> Remove NaN points from *rCloud_tar to *iCloud_tar ...[saved]
------> Got *iCloud_src_normals ...
------> Got *iCloud_tar_normals ...
------> ISS3Ding: size *iCloud_src to *iKeypoints_src from [ 3566 -> 35 ] ... [saved]
------> ISS3Ding: size *iCloud_tar to *iKeypoints_tar from [ 3566 -> 35 ] ... [saved]
[setPointCloudRenderingProperties] Colors go from 0.0 to 1.0!
------> Computed *rCSHOTDesc_src SHOTColor descriptor values ...
1
35
------> Computed *rCSHOTDesc_tar SHOTColor descriptor values ...
1
35
------> RANSAC has converged:1  Score: 8.75654e-05
------> RANSAC Rot-Translation Matrix:
   0.99969 -0.0168972  0.0182904 0.00439214
 0.0131553    0.98204   0.188215  0.0062426
-0.0211422  -0.187916   0.981958  0.0186846
         0          0          0          1
------> ICP has converged:1  Score: 3.0793e-06
------> ICP Rot-Translation Matrix:
   0.999859  0.00626789   -0.015622  0.00168091
-0.00588297    0.999681   0.0245647  0.00247214
   0.015771  -0.0244691    0.999577  0.00120919
          0           0           0           1

  • 5
    点赞
  • 46
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 4
    评论
以下是点云ISS3DSC、SAC-IA、ICP的代码示例: 1. ISS 特征提取 ```c++ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector; iss_detector.setSearchMethod(tree); iss_detector.setSalientRadius(6 * resolution); iss_detector.setNonMaxRadius(4 * resolution); iss_detector.setThreshold21(0.975); iss_detector.setThreshold32(0.975); iss_detector.setMinNeighbors(5); iss_detector.setInputCloud(cloud); iss_detector.compute(*keypoints); ``` 2. 3DSC 特征描述 ```c++ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::ShapeContext1980>::Ptr descriptors(new pcl::PointCloud<pcl::ShapeContext1980>); pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::ShapeContext1980> desc_est; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); desc_est.setInputCloud(keypoints); desc_est.setInputNormals(normals); desc_est.setSearchMethod(tree); desc_est.setRadiusSearch(4 * resolution); desc_est.compute(*descriptors); ``` 3. SAC-IA 平面拟合 ```c++ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::SACSegmentation<pcl::PointXYZ> seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.01); seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); ``` 4. ICP 点云配准 ```c++ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>); pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; icp.setInputSource(cloud1); icp.setInputTarget(cloud2); icp.setMaxCorrespondenceDistance(0.05); icp.setMaximumIterations(50); icp.setTransformationEpsilon(1e-8); icp.setEuclideanFitnessEpsilon(1); icp.align(*aligned); ```
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

点云兔子

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值