请仿照eigen库的矩阵实现设计一个矩阵类实现基本运算及功能函数继续实现李群SO3和SE3的类

下面是一个简单的矩阵类的实现，包括基本的运算和功能函数的实现：

#include <vector>
#include <iostream>

class Matrix {
private:
    std::vector<std::vector<double>> data;
    int rows;
    int cols;

public:
    Matrix(int rows, int cols) : rows(rows), cols(cols) {
        data.resize(rows, std::vector<double>(cols, 0.0));
    }

    Matrix(std::vector<std::vector<double>> data) : data(data), rows(data.size()), cols(data[0].size()) {}

    int getRows() const {
        return rows;
    }

    int getCols() const {
        return cols;
    }

    double& operator()(int i, int j) {
        return data[i][j];
    }

    const double& operator()(int i, int j) const {
        return data[i][j];
    }

    Matrix operator+(const Matrix& other) const {
        if (rows != other.rows || cols != other.cols) {
            throw std::runtime_error("Matrix dimensions must match for addition.");
        }

        Matrix result(rows, cols);
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                result(i, j) = data[i][j] + other(i, j);
            }
        }
        return result;
    }

    Matrix operator-(const Matrix& other) const {
        if (rows != other.rows || cols != other.cols) {
            throw std::runtime_error("Matrix dimensions must match for subtraction.");
        }

        Matrix result(rows, cols);
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                result(i, j) = data[i][j] - other(i, j);
            }
        }
        return result;
    }

    Matrix operator*(const Matrix& other) const {
        if (cols != other.rows) {
            throw std::runtime_error("Matrix dimensions must match for multiplication.");
        }

        Matrix result(rows, other.cols);
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < other.cols; ++j) {
                for (int k = 0; k < cols; ++k) {
                    result(i, j) += data[i][k] * other(k, j);
                }
            }
        }
        return result;
    }

    Matrix transpose() const {
        Matrix result(cols, rows);
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                result(j, i) = data[i][j];
            }
        }
        return result;
    }

    void print() const {
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                std::cout << data[i][j] << " ";
            }
            std::cout << std::endl;
        }
    }
};

接下来，我们可以实现李群SO(3)和SE(3)的类。其中，SO(3)是旋转矩阵的特殊正交群，SE(3)是旋转矩阵和平移向量的组合。下面是一个简单的实现：

#include <cmath>
#include <array>
#include <iostream>

class SO3 {
private:
    Matrix rotation;

public:
    SO3() : rotation(3, 3) {
        rotation(0, 0) = 1.0;
        rotation(1, 1) = 1.0;
        rotation(2, 2) = 1.0;
    }

    SO3(const Matrix& rotation) : rotation(rotation) {
        if (rotation.getRows() != 3 || rotation.getCols() != 3) {
            throw std::runtime_error("Rotation matrix must be 3x3.");
        }
    }

    SO3(double roll, double pitch, double yaw) : rotation(3, 3) {
        double cr = cos(roll);
        double sr = sin(roll);
        double cp = cos(pitch);
        double sp = sin(pitch);
        double cy = cos(yaw);
        double sy = sin(yaw);

        rotation(0, 0) = cp * cy;
        rotation(0, 1) = cp * sy;
        rotation(0, 2) = -sp;
        rotation(1, 0) = sr * sp * cy - cr * sy;
        rotation(1, 1) = sr * sp * sy + cr * cy;
        rotation(1, 2) = sr * cp;
        rotation(2, 0) = cr * sp * cy + sr * sy;
        rotation(2, 1) = cr * sp * sy - sr * cy;
        rotation(2, 2) = cr * cp;
    }

    Matrix getRotation() const {
        return rotation;
    }

    void print() const {
        rotation.print();
    }
};

class SE3 {
private:
    Matrix rotation;
    std::array<double, 3> translation;

public:
    SE3() : rotation(3, 3), translation{0.0, 0.0, 0.0} {
        rotation(0, 0) = 1.0;
        rotation(1, 1) = 1.0;
        rotation(2, 2) = 1.0;
    }

    SE3(const Matrix& rotation, const std::array<double, 3>& translation) : rotation(rotation), translation(translation) {
        if (rotation.getRows() != 3 || rotation.getCols() != 3) {
            throw std::runtime_error("Rotation matrix must be 3x3.");
        }
    }

    SE3(const SO3& so3, const std::array<double, 3>& translation) : rotation(so3.getRotation()), translation(translation) {}

    Matrix getRotation() const {
        return rotation;
    }

    std::array<double, 3> getTranslation() const {
        return translation;
    }

    void print() const {
        rotation.print();
        std::cout << translation[0] << " " << translation[1] << " " << translation[2] << std::endl;
    }
};

这是一个简单的实现，可以根据需要进行扩展和优化