PCL交互迭代最近点方法

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/u014801811/article/details/79979764

代码出处:http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

交互迭代最近法允许用户每次按下“空格”键,一次ICP迭代算法执行,并更新视图。
此程序主要步骤:

1、导入点云数据

PCL提供IO模块直接读入点云文件。

pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in)

注:控制台参数输入时,cd到exe路径,执行 “XX arg1 arg2 …”

2、旋转平移获取待配准点云数据

pcl中矩阵的定义及赋值:

//定义旋转矩阵和平移矩阵
    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

    double theta = M_PI / 8;//旋转角度
    transformation_matrix(0,0) = cos(theta);
    transformation_matrix(0,1) = -sin(theta);
    transformation_matrix(1,0) = sin(theta);
    transformation_matrix(1,1) = cos(theta);

    transformation_matrix(0, 3) = 0;//沿x轴移动0.m
    transformation_matrix(1, 3) = 0;//沿y轴移动0.m
    transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m

    pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换

3、icp配准

    pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;//配准对象
    icp.setInputSource(cloud_icp);
    icp.setInputTarget(cloud_in);
    /*icp.setEuclideanFitnessEpsilon(0.1);
    icp.setRANSACIterations(10);
    icp.setTransformationEpsilon(1e-6);*/
    icp.align(*cloud_icp);

    icp.setMaximumIterations(1);//迭代一次

4、点云可视化

//可视化
    pcl::visualization::PCLVisualizer viewer("ICP demo");
    //创建两个视口
    int v1(0);
    int v2(1);
    viewer.createViewPort(0, 0, 0.5, 1, v1);
    viewer.createViewPort(0.5, 0, 1, 1, v2);
    //颜色
    float bckgr_gray_level = 0.0;//黑色
    float txt_gray_lvl = 1.0 - bckgr_gray_level;

    //原始点云 白色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
        (cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);

    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

    //目标变换点云 绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h
        (cloud_tr, 20,180,20);
    viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

    //ICP 配准点云 红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h
        (cloud_tr, 180, 20, 20);
    viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

完整代码

/** Filename:  interaction_icp.cpp

** Date: 2018-4-17

**Description:

**/

#include "stdafx.h"
#include <iostream>
#include <string>
#include <vector>

#include <pcl\visualization\pcl_plotter.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\point_types.h>
#include <pcl\point_cloud.h>
#include <pcl\range_image\range_image.h>
#include <pcl\visualization\range_image_visualizer.h>
#include <pcl\io\ply_io.h>
#include <pcl\io\pcd_io.h>
#include <pcl\registration\icp.h>
#include <pcl\console\time.h>

bool next_iteration = false;

void print4x4Matrix(const Eigen::Matrix4d& matrix)
{
    printf("旋转矩阵R:\n");
    printf("      |%6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
    printf(" R =  |%6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
    printf("      |%6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
    printf("平移矩阵T: \n");
    printf("T = < %6.3f %6.3f %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}


void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing)
{
    if (event.getKeySym() == "space" && event.keyDown())
        next_iteration = true;
}

int main(int argc, char* argv[])
{
    //定义点云类型
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);//目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr(new pcl::PointCloud<pcl::PointXYZ>);//变换点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);//icp配准后点云

    //检查程序参数
    if (argc < 2)
    {
        printf("Usage:\n");
        printf("\t\t%s file.ply number_of_ICP_iterations\n",argv[0]);
        PCL_ERROR("Provide one ply file. \n");
        return -1;
    }
    int iterations = 1;//默认迭代次数
    if (argc>2)
    {
        iterations = atoi(argv[1]);
        if (iterations < 1)
        {
            PCL_ERROR("Number of initial iterations must be >= 1\n");
            return -1;
        }
    }
    pcl::console::TicToc time;
    time.tic();
    //加载点云数据
    if (pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in) < 0)
    {
        PCL_ERROR("Error loading cloud %s.\n", argv[1]);
        return -1;
    }

    std::cout << "\n Loaded file " << argv[1] << "(" << cloud_in->points.size() << "points) in " << time.toc() << "ms\n\n";

    //定义旋转矩阵和平移矩阵
    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
    print4x4Matrix(transformation_matrix);
    std::cout << transformation_matrix << std::endl;

    double theta = M_PI / 8;//旋转角度
    transformation_matrix(0,0) = cos(theta);
    transformation_matrix(0,1) = -sin(theta);
    transformation_matrix(1,0) = sin(theta);
    transformation_matrix(1,1) = cos(theta);

    transformation_matrix(0, 3) = 0;//沿x轴移动0.m
    transformation_matrix(1, 3) = 0;//沿y轴移动0.m
    transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m

    //在控制台显示变换矩阵
    printf("刚性变换矩阵:\n");
    std::cout << transformation_matrix << std::endl;
    pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换
    *cloud_tr = *cloud_icp;//备份目标点云

    //ICP算法
    std::cout << "ICP 配准开始" << std::endl;
    time.tic();
    pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
    icp.setInputSource(cloud_icp);
    icp.setInputTarget(cloud_in);
    /*icp.setEuclideanFitnessEpsilon(0.1);
    icp.setRANSACIterations(10);
    icp.setTransformationEpsilon(1e-6);*/
    icp.align(*cloud_icp);

    icp.setMaximumIterations(1);
    std::cout << "Applied" << iterations << "ICP iteration(s) in " << time.toc() << "ms"<<std::endl;
    if (icp.hasConverged())
    {
        std::cout << "\n ICP has converged, score is " << icp.getFitnessScore() << std::endl;
        std::cout << "\n ICP transformation " << iterations << ": cloud_icp -> cloud_in" << std::endl;
        transformation_matrix = icp.getFinalTransformation().cast<double>();;
        print4x4Matrix(transformation_matrix);
    }
    else
    {
        PCL_ERROR(" \n ICP has not converged.\n");
        return -1;
    }

    //可视化
    pcl::visualization::PCLVisualizer viewer("ICP demo");
    //创建两个视口
    int v1(0);
    int v2(1);
    viewer.createViewPort(0, 0, 0.5, 1, v1);
    viewer.createViewPort(0.5, 0, 1, 1, v2);
    //颜色
    float bckgr_gray_level = 0.0;//黑色
    float txt_gray_lvl = 1.0 - bckgr_gray_level;

    //原始点云 白色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
        (cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);

    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
    viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

    //目标变换点云 绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h
        (cloud_tr, 20,180,20);
    viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

    //ICP 配准点云 红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h
        (cloud_tr, 180, 20, 20);
    viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
    //添加文本描述
    viewer.addText("White: Original point cloud\n Green :Matix transformed point cloud", 10, 15, 16,
        txt_gray_lvl, txt_gray_lvl, txt_gray_lvl,"icp_info_1",v1);
    viewer.addText("White: Original point cloud\n Red: ICP aligned point cloud", 10, 15, 16, 
        txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

    std::stringstream ss;
    ss << iterations;
    std::string iterations_cnt = "ICP iterations = " + ss.str();
    viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

    //设置背景颜色
    viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
    viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

    //设置相机位置和方向
    viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
    //窗口大小
    viewer.setSize(1280, 1024);
    viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
        //按下空格键
        if (next_iteration)
        {
            //ICP 算法
            time.tic();
            icp.align(*cloud_icp);
            std::cout << "Applied 1 ICP iteration in " << time.toc() << "ms" << std::endl;
            if (icp.hasConverged())
            {
                printf("\033[11A");
                printf("\n ICP has converged ,score is %+.0e\n", icp.getFitnessScore());
                std::cout << "\n ICP transformation" << ++iterations << " :cloud_icp -> cloud_in" << std::endl;
                transformation_matrix *= icp.getFinalTransformation().cast<double>();  
                print4x4Matrix(transformation_matrix);  

                ss.str("");
                ss << iterations;
                std::string iterations_cnt = "ICP iterations = " + ss.str();
                viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
                viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
            }
            else
            {
                PCL_ERROR("\nICP has not converged.\n");
                return (-1);
            }
        }
        next_iteration = false;
    }
    system("pause");
    return 0;
}

可视化结果

这里写图片描述
这里写图片描述

阅读更多
想对作者说点什么?

博主推荐

换一批

没有更多推荐了,返回首页