1、坐标转化接口

include/pcl/trans_coor.hpp

@@ -0,0 +1,90 @@
+#pragma once
+#include <math.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+//template <typename T>
+class CoordinateTransformation {
+public:
+    struct Point2D {
+        float x;
+        float y;
+    };
+
+    struct transInfo2D {
+        Point2D move;
+        float angle;
+    };
+
+    struct transInfo3D {
+        pcl::PointXYZ move;
+        float roll;
+        float pitch;
+        float yaw;
+
+        transInfo3D() = default;
+        transInfo3D(pcl::PointXYZ move_, float roll_, float pitch_, float yaw_): move(move_), roll(roll_), pitch(pitch_) {}
+    };
+
+    CoordinateTransformation() = default;
+    ~CoordinateTransformation() = default;
+
+    void setTranInfo(const transInfo2D &trans, bool radian = false) {
+
+    }
+
+    void setTranInfo(const transInfo3D &trans, bool radian = false) {
+        trans3D = true;
+        float cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
+        if (!radian) {
+            cosYaw = cos(trans.yaw * float(M_PI) / 180.0f);
+            sinYaw = sin(trans.yaw * float(M_PI) / 180.0f);
+            cosPitch = cos(trans.pitch * float(M_PI) / 180.0f);
+            sinPitch = sin(trans.pitch * float(M_PI) / 180.0f);
+            cosRoll = cos(trans.roll * float(M_PI) / 180.0f);
+            sinRoll = sin(trans.roll * float(M_PI) / 180.0f);
+        } else {
+            cosYaw = cos(trans.yaw);
+            sinYaw = sin(trans.yaw);
+            cosPitch = cos(trans.pitch);
+            sinPitch = sin(trans.pitch);
+            cosRoll = cos(trans.roll);
+            sinRoll = sin(trans.roll);
+        }
+        trans3D_move = trans.move;
+        rotationMatrix3D[0][0] = cosYaw * cosPitch;
+        rotationMatrix3D[0][1] = cosYaw * sinPitch * sinRoll - sinYaw * cosRoll;
+        rotationMatrix3D[0][2] = cosYaw * sinPitch * cosRoll + sinYaw * sinRoll;
+        rotationMatrix3D[1][0] = sinYaw * cosPitch;
+        rotationMatrix3D[1][1] = sinYaw * sinPitch * sinRoll + cosYaw * cosRoll;
+        rotationMatrix3D[1][2] = sinYaw * sinPitch * cosRoll - cosYaw * sinRoll;
+        rotationMatrix3D[2][0] = -sinPitch;
+        rotationMatrix3D[2][1] = cosPitch * sinRoll;
+        rotationMatrix3D[2][2] = cosPitch * cosRoll;
+    }
+
+    void transPoint(const Point2D &in, Point2D &out) {
+
+    }
+
+    void transPoint(const pcl::PointXYZ &in, pcl::PointXYZ &out) {
+        trans3DPoint(in, out);
+    }
+private:
+    bool trans3DPoint(const pcl::PointXYZ &in, pcl::PointXYZ &out) {
+        if (trans3D) {
+            out.x = rotationMatrix3D[0][0] * in.x + rotationMatrix3D[0][1] * in.y + rotationMatrix3D[0][2] * in.z + trans3D_move.x;
+            out.y = rotationMatrix3D[1][0] * in.x + rotationMatrix3D[1][1] * in.y + rotationMatrix3D[1][2] * in.z + trans3D_move.y;
+            out.z = rotationMatrix3D[2][0] * in.x + rotationMatrix3D[2][1] * in.y + rotationMatrix3D[2][2] * in.z + trans3D_move.z;
+        }
+        return trans3D;
+    }
+
+private:
+    bool trans2D = false;
+    bool trans3D = false;
+    Point2D trans2D_move{};
+    pcl::PointXYZ trans3D_move{0, 0, 0};
+    float rotationMatrix2D[1] = {0};
+    float rotationMatrix3D[3][3] = {0};
+};