持之以恒（二）坐标系的位姿变换： 旋转 / 偏移

1. 坐标系

1.1 简介

• 坐标系的表示矩阵
• 坐标系的右乘
• 坐标系的左乘

2. 坐标系的计算接口

绕X/Y/Z旋转一定角度

法1：使用Eigen库

Eigen::Matrix4f RotaryAngle(const Eigen::Matrix4f& pose, float angle, CoordinateAxisSequence sequence){
    //旋转姿态是相对于初始位姿的变化量,先初始化为单位矩阵
    Eigen::Matrix4f relation = Eigen::Matrix4f::Identity();
    
    switch (sequence) {
    case X:
        //绕 x 轴旋转 ±angle
        relation.block<3,3>(0,0) = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitX()).toRotationMatrix();
        break;
    case Y:
        //绕 y 轴旋转 ±angle
        relation.block<3,3>(0,0) = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY()).toRotationMatrix();
        break;
    case Z:
        //绕 z 轴旋转 ±angle
        relation.block<3,3>(0,0) = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
        break;
    default:
        break;
    }
    return pose * relation;
}

基于X/Y/Z偏移一定距离

法1：使用Eigen库

Eigen::Matrix4f OffsetDistance(const Eigen::Matrix4f& pose, float distance, CoordinateAxisSequence sequence){
    //旋转姿态是相对于初始位姿的变化量,先初始化为单位矩阵
    Eigen::Matrix4f relation = Eigen::Matrix4f::Identity();
    
    switch (sequence) {
    case X:
        relation(0, 3) = distance;
        break;
    case Y:
        relation(1, 3) = distance;
        break;
    case Z:
        relation(2, 3) = distance;
        break;
    default:
        break;
    }
    return pose * relation;
}