///'#include //n#include //n//nclass Matrix {//nprivate://n std::vector<std::vector> data;//n int rows;//n int cols;//n//npublic://n Matrix(int rows, int cols) : rows(rows), cols(cols) {//n data.resize(rows, std::vector(cols, 0.0));//n }//n//n Matrix(std::vector<std::vector> data) : data(data), rows(data.size()), cols(data[0].size()) {}//n//n int getRows() const {//n return rows;//n }//n//n int getCols() const {//n return cols;//n }//n//n double& operator()(int i, int j) {//n return data[i][j];//n }//n//n const double& operator()(int i, int j) const {//n return data[i][j];//n }//n//n Matrix operator+(const Matrix& other) const {//n if (rows != other.rows || cols != other.cols) {//n throw std::runtime_error(/'Matrix dimensions must match for addition./');//n }//n//n Matrix result(rows, cols);//n for (int i = 0; i < rows; ++i) {//n for (int j = 0; j < cols; ++j) {//n result(i, j) = data[i][j] + other(i, j);//n }//n }//n return result;//n }//n//n Matrix operator-(const Matrix& other) const {//n if (rows != other.rows || cols != other.cols) {//n throw std::runtime_error(/'Matrix dimensions must match for subtraction./');//n }//n//n Matrix result(rows, cols);//n for (int i = 0; i < rows; ++i) {//n for (int j = 0; j < cols; ++j) {//n result(i, j) = data[i][j] - other(i, j);//n }//n }//n return result;//n }//n//n Matrix operator*(const Matrix& other) const {//n if (cols != other.rows) {//n throw std::runtime_error(/'Matrix dimensions must match for multiplication./');//n }//n//n Matrix result(rows, other.cols);//n for (int i = 0; i < rows; ++i) {//n for (int j = 0; j < other.cols; ++j) {//n for (int k = 0; k < cols; ++k) {//n result(i, j) += data[i][k] * other(k, j);//n }//n }//n }//n return result;//n }//n//n Matrix transpose() const {//n Matrix result(cols, rows);//n for (int i = 0; i < rows; ++i) {//n for (int j = 0; j < cols; ++j) {//n result(j, i) = data[i][j];//n }//n }//n return result;//n }//n//n void print() const {//n for (int i = 0; i < rows; ++i) {//n for (int j = 0; j < cols; ++j) {//n std::cout << data[i][j] << /' /';//n }//n std::cout << std::endl;//n }//n }//n};//n//n#include //n#include //n#include //n//nclass SO3 {//nprivate://n Matrix rotation;//n//npublic://n SO3() : rotation(3, 3) {//n rotation(0, 0) = 1.0;//n rotation(1, 1) = 1.0;//n rotation(2, 2) = 1.0;//n }//n//n SO3(const Matrix& rotation) : rotation(rotation) {//n if (rotation.getRows() != 3 || rotation.getCols() != 3) {//n throw std::runtime_error(/'Rotation matrix must be 3x3./');//n }//n }//n//n SO3(double roll, double pitch, double yaw) : rotation(3, 3) {//n double cr = cos(roll);//n double sr = sin(roll);//n double cp = cos(pitch);//n double sp = sin(pitch);//n double cy = cos(yaw);//n double sy = sin(yaw);//n//n rotation(0, 0) = cp * cy;//n rotation(0, 1) = cp * sy;//n rotation(0, 2) = -sp;//n rotation(1, 0) = sr * sp * cy - cr * sy;//n rotation(1, 1) = sr * sp * sy + cr * cy;//n rotation(1, 2) = sr * cp;//n rotation(2, 0) = cr * sp * cy + sr * sy;//n rotation(2, 1) = cr * sp * sy - sr * cy;//n rotation(2, 2) = cr * cp;//n }//n//n Matrix getRotation() const {//n return rotation;//n }//n//n void print() const {//n rotation.print();//n }//n};//n//nclass SE3 {//nprivate://n Matrix rotation;//n std::array<double, 3> translation;//n//npublic://n SE3() : rotation(3, 3), translation{0.0, 0.0, 0.0} {//n rotation(0, 0) = 1.0;//n rotation(1, 1) = 1.0;//n rotation(2, 2) = 1.0;//n }//n//n SE3(const Matrix& rotation, const std::array<double, 3>& translation) : rotation(rotation), translation(translation) {//n if (rotation.getRows() != 3 || rotation.getCols() != 3) {//n throw std::runtime_error(/'Rotation matrix must be 3x3./');//n }//n }//n//n SE3(const SO3& so3, const std::array<double, 3>& translation) : rotation(so3.getRotation()), translation(translation) {}//n//n Matrix getRotation() const {//n return rotation;//n }//n//n std::array<double, 3> getTranslation() const {//n return translation;//n }//n//n void print() const {//n rotation.print();//n std::cout << translation[0] << /' /' << translation[1] << /' /' << translation[2] << std::endl;//n }//n};//n/
