VM LAB: 座標軸を回転させる

　点群データは、時に移動させたり回転させたりしなければならないことが多々ある。今回は座標軸を回転させるプログラムを作る。尚PCLでは赤の座標軸がX軸、緑がY軸、青がZ軸を表している

コード

#include "stdafx.h"

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <stdlib.h>

void centroid(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    Eigen::Vector4f xyz_centroid;
    pcl::compute3DCentroid(*cloud, xyz_centroid);//重心を計算

    for(size_t i = 0; i < cloud->points.size(); i++){
        cloud->points[i].x = cloud->points[i].x - xyz_centroid[0];//X座標の移動
        cloud->points[i].y = cloud->points[i].y - xyz_centroid[1];//Y座標の移動
        cloud->points[i].z = cloud->points[i].z - xyz_centroid[2];//Z座標の移動
    }
}

pcl::PointCloud<pcl::PointXYZ> rotation_x(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta)///rotate point cloud by Y axiz
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_centroid (new pcl::PointCloud<pcl::PointXYZ>);
    cloud_centroid = cloud;

    centroid(cloud_centroid);//点群の重心位置求めを（0,0,0）に移動する

    Eigen::Matrix4f rot_x;
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    cloud_out = *cloud;
    double theta_x = theta * 3.1415926 / 180.0;//角度の変換
    rot_x(0,0) = 1.0;
    rot_x(1,0) = 0.0;
    rot_x(2,0) = 0.0;
    rot_x(3,0) = 0.0;
    rot_x(0,1) = 0.0;
    rot_x(1,1) = cos(theta_x);
    rot_x(2,1) = -sin(theta_x);
    rot_x(3,1) = 0.0;
    rot_x(0,2) = 0.0;
    rot_x(1,2) = sin(theta_x);
    rot_x(2,2) = cos(theta_x);
    rot_x(3,2) = 0.0;
    rot_x(0,3) = 0.0;
    rot_x(1,3) = 0.0;
    rot_x(2,3) = 0.0;
    rot_x(3,3) = 1.0;

    for(size_t i = 0; i < cloud->points.size(); ++i){
        cloud_out.points[i].x = rot_x(0,0) * cloud->points[i].x + rot_x(0,1) * cloud->points[i].y + rot_x(0,2) * cloud->points[i].z + rot_x(0,3) * 1;
        cloud_out.points[i].y = rot_x(1,0) * cloud->points[i].x + rot_x(1,1) * cloud->points[i].y + rot_x(1,2) * cloud->points[i].z + rot_x(1,3) * 1;
        cloud_out.points[i].z = rot_x(2,0) * cloud->points[i].x + rot_x(2,1) * cloud->points[i].y + rot_x(2,2) * cloud->points[i].z + rot_x(2,3) * 1;
    }

    return cloud_out;
}

pcl::PointCloud<pcl::PointXYZ> rotation_y(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta)///rotate point cloud by Y axiz
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_centroid (new pcl::PointCloud<pcl::PointXYZ>);
    cloud_centroid = cloud;

    centroid(cloud_centroid);//点群の重心位置求めを（0,0,0）に移動する

    Eigen::Matrix4f rot_y;
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    cloud_out = *cloud;
    double theta_y = theta * 3.1415926 / 180.0;//角度の変換
    rot_y(0,0) = cos(theta_y);
    rot_y(1,0) = 0.0;
    rot_y(2,0) = sin(theta_y);
    rot_y(3,0) = 0.0;
    rot_y(0,1) = 0.0;
    rot_y(1,1) = 1.0;
    rot_y(2,1) = 0.0;
    rot_y(3,1) = 0.0;
    rot_y(0,2) = -sin(theta_y);
    rot_y(1,2) = 0.0;
    rot_y(2,2) = cos(theta_y);
    rot_y(3,2) = 0.0;
    rot_y(0,3) = 0.0;
    rot_y(1,3) = 0.0;
    rot_y(2,3) = 0.0;
    rot_y(3,3) = 1.0;

    for(size_t i = 0; i < cloud->points.size(); ++i){
        cloud_out.points[i].x = rot_y(0,0) * cloud->points[i].x + rot_y(0,1) * cloud->points[i].y + rot_y(0,2) * cloud->points[i].z + rot_y(0,3) * 1;
        cloud_out.points[i].y = rot_y(1,0) * cloud->points[i].x + rot_y(1,1) * cloud->points[i].y + rot_y(1,2) * cloud->points[i].z + rot_y(1,3) * 1;
        cloud_out.points[i].z = rot_y(2,0) * cloud->points[i].x + rot_y(2,1) * cloud->points[i].y + rot_y(2,2) * cloud->points[i].z + rot_y(2,3) * 1;
    }

    return cloud_out;
}

pcl::PointCloud<pcl::PointXYZ> rotation_z(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta)///rotate point cloud by Y axiz
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_centroid (new pcl::PointCloud<pcl::PointXYZ>);
    cloud_centroid = cloud;

    centroid(cloud_centroid);//点群の重心位置求めを（0,0,0）に移動する

    Eigen::Matrix4f rot_z;
    pcl::PointCloud<pcl::PointXYZ> cloud_out;
    cloud_out = *cloud;
    double theta_z = theta * 3.1415926 / 180.0;//角度の変換
    rot_z(0,0) = cos(theta_z);
    rot_z(1,0) = -sin(theta_z);
    rot_z(2,0) = 0.0;
    rot_z(3,0) = 0.0;
    rot_z(0,1) = sin(theta_z);
    rot_z(1,1) = cos(theta_z);
    rot_z(2,1) = 0.0;
    rot_z(3,1) = 0.0;
    rot_z(0,2) = 0.0;
    rot_z(1,2) = 0.0;
    rot_z(2,2) = 1.0;
    rot_z(3,2) = 0.0;
    rot_z(0,3) = 0.0;
    rot_z(1,3) = 0.0;
    rot_z(2,3) = 0.0;
    rot_z(3,3) = 1.0;

    for(size_t i = 0; i < cloud->points.size(); ++i){
        cloud_out.points[i].x = rot_z(0,0) * cloud->points[i].x + rot_z(0,1) * cloud->points[i].y + rot_z(0,2) * cloud->points[i].z + rot_z(0,3) * 1;
        cloud_out.points[i].y = rot_z(1,0) * cloud->points[i].x + rot_z(1,1) * cloud->points[i].y + rot_z(1,2) * cloud->points[i].z + rot_z(1,3) * 1;
        cloud_out.points[i].z = rot_z(2,0) * cloud->points[i].x + rot_z(2,1) * cloud->points[i].y + rot_z(2,2) * cloud->points[i].z + rot_z(2,3) * 1;
    }

    return cloud_out;
}

int _tmain(int argc, _TCHAR* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr rotated_cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile(argv[1],*cloud);

    Eigen::Vector4f xyz_centroid;
    pcl::compute3DCentroid(*cloud, xyz_centroid);//元の点群の重心位置を記録

    *rotated_cloud = rotation_x(cloud, -90.0);//回転
    //*rotated_cloud = rotation_y(cloud, -90.0);//回転
    //*rotated_cloud = rotation_z(cloud, -90.0);//回転

    std::stringstream Filename;
    string name = argv[1];
    name.erase(name.length()-4);
    Filename << name << "_rotated.pcd";

    pcl::io::savePCDFileBinary(Filename.str(), *rotated_cloud);
    return 0;
}

結果