三角函数实现

下面是代码c++和python实现：

#include <iostream>
#include <cmath>struct Point {double x;double y;
};class RobotCoordinateTransform {
private:Point origin; // 局部坐标系的原点在世界坐标系中的坐标public:RobotCoordinateTransform(double originX, double originY) : origin({originX, originY}) {}// 将世界坐标转换到局部坐标Point worldToLocal(double x_w, double y_w, double theta_w) {Point local;// 平移坐标系local.x = x_w - origin.x;local.y = y_w - origin.y;// 旋转坐标系double x_local = local.x * cos(theta_w) - local.y * sin(theta_w);double y_local = local.x * sin(theta_w) + local.y * cos(theta_w);local.x = x_local;local.y = y_local;return local;}
};int main() {RobotCoordinateTransform transform(0, 0); // 假设局部坐标系原点在世界坐标系的(0, 0)点double x_w, y_w, theta_w;std::cout << "Enter world coordinates (x_w, y_w) and orientation theta_w (in radians): ";std::cin >> x_w >> y_w >> theta_w;Point local = transform.worldToLocal(x_w, y_w, theta_w);std::cout << "Local coordinates (x_local, y_local): (" << local.x << ", " << local.y << ")" << std::endl;return 0;
}

import mathclass RobotCoordinateTransform:def __init__(self, origin_x, origin_y):self.origin = (origin_x, origin_y)  # 局部坐标系的原点在世界坐标系中的坐标# 将世界坐标转换到局部坐标def world_to_local(self, x_w, y_w, theta_w):# 平移坐标系x_local = x_w - self.origin[0]y_local = y_w - self.origin[1]# 旋转坐标系x_local_rotated = x_local * math.cos(theta_w) - y_local * math.sin(theta_w)y_local_rotated = x_local * math.sin(theta_w) + y_local * math.cos(theta_w)return x_local_rotated, y_local_rotatedif __name__ == "__main__":origin_x = float(input("Enter the x-coordinate of the origin of the local coordinate system: "))origin_y = float(input("Enter the y-coordinate of the origin of the local coordinate system: "))transform = RobotCoordinateTransform(origin_x, origin_y)x_w = float(input("Enter the x-coordinate in the world coordinate system: "))y_w = float(input("Enter the y-coordinate in the world coordinate system: "))theta_w = float(input("Enter the orientation (in radians) in the world coordinate system: "))x_local, y_local = transform.world_to_local(x_w, y_w, theta_w)print(f"Local coordinates (x_local, y_local): ({x_local}, {y_local})")

矩阵实现：

下面是代码c++和python实现：

#include <iostream>
#include <cmath>
#include <Eigen/Dense>  // Eigen库用于矩阵运算class RobotCoordinateTransform {
private:Eigen::Vector2d origin;  // 局部坐标系的原点在世界坐标系中的坐标public:RobotCoordinateTransform(double originX, double originY) : origin(originX, originY) {}// 将世界坐标转换到局部坐标std::pair<double, double> worldToLocal(double x_w, double y_w, double theta_w) {// 平移坐标系的矩阵Eigen::Matrix3d translationMatrix;translationMatrix << 1, 0, -origin[0],0, 1, -origin[1],0, 0, 1;// 旋转坐标系的矩阵Eigen::Matrix3d rotationMatrix;rotationMatrix << cos(theta_w), -sin(theta_w), 0,sin(theta_w),  cos(theta_w), 0,0,             0,            1;// 世界坐标的齐次坐标Eigen::Vector3d worldCoords(x_w, y_w, 1);// 应用平移和旋转变换Eigen::Vector3d localCoords = rotationMatrix * translationMatrix * worldCoords;return std::make_pair(localCoords[0], localCoords[1]);}
};int main() {double originX, originY;std::cout << "Enter the x-coordinate of the origin of the local coordinate system: ";std::cin >> originX;std::cout << "Enter the y-coordinate of the origin of the local coordinate system: ";std::cin >> originY;RobotCoordinateTransform transform(originX, originY);double x_w, y_w, theta_w;std::cout << "Enter the x-coordinate in the world coordinate system: ";std::cin >> x_w;std::cout << "Enter the y-coordinate in the world coordinate system: ";std::cin >> y_w;std::cout << "Enter the orientation (in radians) in the world coordinate system: ";std::cin >> theta_w;auto [x_local, y_local] = transform.worldToLocal(x_w, y_w, theta_w);std::cout << "Local coordinates (x_local, y_local): (" << x_local << ", " << y_local << ")" << std::endl;return 0;
}

Eigen::Vector2d 用于存储坐标点和原点。
Eigen::Matrix3d 用于表示3x3矩阵，进行平移和旋转操作。
worldToLocal 方法使用上述的数学公式和矩阵进行坐标变换。

import numpy as np
import mathclass RobotCoordinateTransform:def __init__(self, origin_x, origin_y):self.origin = np.array([[origin_x], [origin_y]])  # 局部坐标系的原点在世界坐标系中的坐标def world_to_local(self, x_w, y_w, theta_w):# 平移坐标系的矩阵translation_matrix = np.array([[1, 0, -self.origin[0][0]],[0, 1, -self.origin[1][0]],[0, 0, 1]])# 旋转坐标系的矩阵rotation_matrix = np.array([[math.cos(theta_w), -math.sin(theta_w), 0],[math.sin(theta_w), math.cos(theta_w), 0],[0, 0, 1]])# 世界坐标的齐次坐标world_coords = np.array([[x_w], [y_w], [1]])# 应用平移和旋转变换local_coords = np.dot(rotation_matrix, np.dot(translation_matrix, world_coords))return local_coords[0][0], local_coords[1][0]if __name__ == "__main__":origin_x = float(input("Enter the x-coordinate of the origin of the local coordinate system: "))origin_y = float(input("Enter the y-coordinate of the origin of the local coordinate system: "))transform = RobotCoordinateTransform(origin_x, origin_y)x_w = float(input("Enter the x-coordinate in the world coordinate system: "))y_w = float(input("Enter the y-coordinate in the world coordinate system: "))theta_w = float(input("Enter the orientation (in radians) in the world coordinate system: "))x_local, y_local = transform.world_to_local(x_w, y_w, theta_w)print(f"Local coordinates (x_local, y_local): ({x_local}, {y_local})")

Tips：