点云数据旋转

点云数据旋转

#include <iostream>  
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h>  
#include <pcl/filters/passthrough.h>  
#include <pcl/visualization/pcl_visualizer.h>  
  
int main(int argc, char** argv)  
{  
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);  

    double theta = 37/180*3.1415926;
  
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) {  
        PCL_ERROR("Couldn't read file: %s\n", argv[1]);  
        return -1;  
    }  
  
    // rotate the cloud around its z-axis by angle theta  
    for (size_t i = 0; i < cloud->size(); ++i) {  
        cloud->at(i).y = cloud->at(i).y + sin(i * M_PI / 180) * cos(theta);  
        cloud->at(i).x = cloud->at(i).x + cos(i * M_PI / 180) * sin(theta);  
    }  
  
    // save the rotated cloud to disk  
    pcl::io::savePCDFileASCII(argv[1], *cloud, "cloud_rotated.pcd");  
  
    return 0;  
}

##################