标签：配准 TRICP source include pcd pcl 点云 PointCloud cloud

tricp点云配准

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/recognition/trimmed_icp.h>//tricp头文件
#include <time.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>


using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;


void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{
    pcl::visualization::PCLVisualizer viewer("registration Viewer");
    //原始点云绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
    //目标点云红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
    //匹配好的点云蓝色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);

    viewer.setBackgroundColor(255, 255, 255);
    viewer.addPointCloud(pcd_src, src_h, "source cloud");
    viewer.addPointCloud(pcd_tgt, tgt_h, "target cloud");
    viewer.addPointCloud(pcd_final, final_h, "result cloud");
    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

}

int main(int argc, char** argv)
{
    //加载点云文件
    PointCloud::Ptr cloud_source(new PointCloud);
    PointCloud::Ptr cloud_target(new PointCloud);

    //std::string filename = "point_source/hippo1-1224_s100.pcd";//
    std::string filename = "target.pcd";//

    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud_target) == -1)//*打开点云文件

    {
        PCL_ERROR("Couldn't read file test_pcd.pcd\n");
        return(-1);
    }

    //filename = "point_source/hippo2-1224_s100.pcd";//
    filename = "source.pcd";//

    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud_source) == -1)//*打开点云文件

    {
        PCL_ERROR("Couldn't read file test_pcd.pcd\n");
        return(-1);
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);//定义降采样后点云
    *filtered_cloud = *cloud_source;

    //体素降采样
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setLeafSize(0.5, 0.5, 0.5);//bunny
    //voxel_grid.setLeafSize(2.0, 2.0, 2.0);//hippo
    voxel_grid.setInputCloud(filtered_cloud);
    voxel_grid.filter(*filtered_cloud);
    std::cout << "down size input_cloud to" << filtered_cloud->size() << endl;//显示降采样后的点云数量

    pcl::recognition::TrimmedICP<PointT, double> tricp;
    tricp.init(cloud_target);//target

    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
    clock_t start = clock();
    /*align(source, num_source_points_to_use, guess_and_result)
    * source是输入待配准点云（源点云）
    * num_source_points_to_use就是点云乘以重叠度，不同点云模型重叠度
    * guess_and_result是输出算法得到的变换矩阵
    */
    tricp.align(*filtered_cloud, (int)filtered_cloud->size() * 0.8f, transformation_matrix);
    pcl::io::savePCDFileBinary("tricp.pcd", *filtered_cloud);
    clock_t end = clock();

    cout << "time:" << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
    cout << "matrix:" << endl << transformation_matrix << endl << endl << endl;

    PointCloud::Ptr cloud_end(new PointCloud);
    pcl::transformPointCloud(*cloud_source, *cloud_end, transformation_matrix);

    visualize_pcd(cloud_source, cloud_target, cloud_end);
    return (0);


}

标签：配准,TRICP,source,include,pcd,pcl,点云,PointCloud,cloud
来源： https://blog.csdn.net/weixin_56004792/article/details/122778013

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。