PCL点云旋转（平面参数）


PCL点云旋转（平面参数）

实现根据输入平面参数（标准式），基于旋转矩阵 旋转点云到XY面的功能
旋转矩阵

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#include "stdafx.h"
#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>
/*输入输入点云，输出点云地址和分割平面方程，返回旋转完的点云，可直接xy面z轴0值分割*/

pcl::PointCloud<pcl::PointXYZRGBA> Rotate(pcl::PointCloud<pcl::PointXYZRGBA> input,double A,double B,double C, pcl::PointCloud<pcl::PointXYZRGBA> &transformed_cloud)
{
    //点云旋转，基于平面标准式法向量原理
    //先绕x旋转,再绕y旋转

    /* Reminder: how transformation matrices work :
    |-------> This column is the translation
    | 1 0 0 x |  \
    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
    | 0 0 1 z |  /
    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)

    METHOD #1: Using a Matrix4f

    This is the "manual" method, perfect to understand but error prone !

    */

    Eigen::Matrix4f transform_x = Eigen::Matrix4f::Identity();
    /* x转移矩阵
      |1   0   0    |
      |0  cos -sin  |
      |0  sin  cos  |
    */
    double cosx, sinx;
    double dis = sqrt(pow(A, 2) + pow(B, 2) + pow(C, 2));
    cosx = sqrt(pow(A, 2) + pow(C, 2))/dis;
    sinx = B / dis;
    transform_x(1, 1) = cosx;
    transform_x(1, 2) = -sinx;
    transform_x(2,1) = sinx;
    transform_x(2,2) = cosx;

    Eigen::Matrix4f transform_y = Eigen::Matrix4f::Identity();
    /* y转移矩阵
      |cos  0  sin  |
      |0    1   0   |
      |-sin 0  cos  |
    */
    double cosy, siny;
    cosy = C/dis;
    siny = sqrt(pow(A,2)+pow(B,2))/dis;
    // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
    transform_y(0, 0) = cosy;
    transform_y(0, 2) = siny;
    transform_y(2, 0) = -siny;
    transform_y(2, 2) = cosy;
    pcl::PointCloud<pcl::PointXYZRGBA> transformedx_cloud;
    pcl::transformPointCloud(input,transformedx_cloud, transform_x);
    pcl::transformPointCloud(transformedx_cloud, transformed_cloud, transform_y);
    return transformed_cloud;

}

在这里插入图片描述
在这里插入图片描述