三角函数实现
下面是代码c++和python实现:
cpp
#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;
}
python
import math
class 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_rotated
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})")
矩阵实现:
下面是代码c++和python实现:
cpp
#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 方法使用上述的数学公式和矩阵进行坐标变换。
python
import numpy as np
import math
class 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: