Eigen的基本用法

最新推荐文章于 2024-07-25 16:08:18 发布

JobDocLS

最新推荐文章于 2024-07-25 16:08:18 发布

阅读量77

点赞数

文章标签：深度学习

本文链接：https://blog.csdn.net/weixin_43940118/article/details/132124682

版权

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>


int main(int argc, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());

    if(pcl::io::loadPCDFile(argv[1], *source_cloud)<0)
    {
        std::cout<<"Error loading point cloud"<<argv[1]<<std::endl<<std::endl;
        return -1;
    }

    //定义一个单位矩阵
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    float theta = M_PI /4;
    transform_1(0,0) = cos(theta);
    transform_1(0,1) = -sin(theta);
    transform_1(1,0) = sin(theta);
    transform_1(1,1) = cos(theta);

    transform_1(0,3) = 2.5;
    printf("Method # 1:using a Matrix4f\n");
    std::cout<<transform_1<<std::endl;

    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    transform_2.translation()<<0.8,0.0,0.0;
    // 绕Z轴旋转
    transform_2.rotate(Eigen::AngleAxisf(theta,Eigen::Vector3f::UnitZ()));
    printf("\nMethod #2:using an Affine3f\n");
    std::cout<<transform_2.matrix()<<std::endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    //执行仿射变换
    pcl::transformPointCloud(*source_cloud,*transformed_cloud,transform_2);

    printf("\nPoint cloud colors:while = original point cloud\n"  "red = transformed point cloud\n");
    pcl::visualization::PCLVisualizer viewer("Matrix transformation example");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud,255,255,255);
    viewer.addPointCloud(source_cloud,source_cloud_color_handler,"original_cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(transformed_cloud,230,20,20);
    viewer.addPointCloud(transformed_cloud,transformed_cloud_color_handler,"transformed_cloud");

    viewer.addCoordinateSystem(0.5,"cloud",0);
    viewer.setBackgroundColor(0.05,0.05,0.05,0);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_FONT_SIZE,2,"original_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_FONT_SIZE,2,"transformed_cloud");

    while(!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
    return 0;
}