301 字
1 分钟
点云的机器坐标转世界坐标
点云的机器坐标转世界坐标：原理与实现（C++/Python）
1. 问题背景
在机器人感知系统中，激光雷达采集的点云数据通常以传感器自身坐标系（机器坐标系）为参考。为了将这些数据用于建图、定位等任务，我们需要将其转换到全局的世界坐标系中。本文将详细介绍这一转换的数学原理，并提供C++和Python两种实现方式。
2. 数学原理
2.1 坐标系定义
- 机器坐标系：点云原始坐标(x₁,y₁,z₁)，原点在雷达中心
- 世界坐标系：全局固定坐标系
2.2 转换公式
转换过程分为旋转和平移两个步骤：
- 旋转：应用由姿态角(yaw,pitch,roll)构成的旋转矩阵R
- 平移：加上机器在世界坐标系中的位置(x,y)
数学表达式：P_world = R * P_machine + t
2.3 旋转矩阵计算
完整旋转矩阵（Z-Y-X顺序）： R = R_z(yaw) R_y(pitch) R_x(roll)
展开后： R = [ [cosθcosφ, cosθsinφsinψ-sinθcosψ, cosθsinφcosψ+sinθsinψ], [sinθcosφ, sinθsinφsinψ+cosθcosψ, sinθsinφcosψ-cosθsinψ], [-sinφ, cosφsinψ, cosφcosψ ] ]
3. C++实现
#include <Eigen/Dense>#include <vector>
using namespace Eigen;
struct Pose { double x, y, z; double yaw, pitch, roll;};
std::vector<Vector3d> transformPointCloud( const std::vector<Vector3d>& points, const Pose& pose){ // 计算旋转矩阵 double cy = cos(pose.yaw), sy = sin(pose.yaw); double cp = cos(pose.pitch), sp = sin(pose.pitch); double cr = cos(pose.roll), sr = sin(pose.roll);
Matrix3d R; R << cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr, sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr, -sp, cp*sr, cp*cr;
// 应用变换 std::vector<Vector3d> transformed; for (const auto& p : points) { Vector3d p_world = R * p; p_world += Vector3d(pose.x, pose.y, pose.z); transformed.push_back(p_world); }
return transformed;}4. python实现
import numpy as np
def transform_point_cloud(points, pose): """ :param points: Nx3 numpy数组 :param pose: 包含x,y,z,yaw,pitch,roll的字典或对象 :return: 转换后的Nx3点云 """ # 解包位姿 x, y, z = pose['x'], pose['y'], pose['z'] yaw, pitch, roll = pose['yaw'], pose['pitch'], pose['roll']
# 计算旋转矩阵 cy, sy = np.cos(yaw), np.sin(yaw) cp, sp = np.cos(pitch), np.sin(pitch) cr, sr = np.cos(roll), np.sin(roll)
R = np.array([ [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr], [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr], [-sp, cp*sr, cp*cr] ])
# 应用变换 points_rotated = np.dot(points, R.T) points_transformed = points_rotated + np.array([x, y, z])
return points_transformed 分享
如果这篇文章对你有帮助，欢迎分享给更多人！
部分信息可能已经过时
相关文章 智能推荐
随机文章 随机推荐