1. 需求描述
实际项目中,有Lidar和 IMU两个传感器,上下背靠背安装。设Lidar所在坐标系为1系,IMU所在坐标系为2系,且均为右手系 。
其中2系的x轴与1系的y轴方向相同,2系的y轴与1系z轴方向相反,2系的z轴与1系的x轴相反,两个坐标系原点重.
求1系中 (0.2, 0.5, 1) 在2系中的坐标。请自己编写一个c++程序实现它,并用Cmake编译,得到能输出答案的可执行文件
2. 需求分析
整个变化经历了两次旋转:
经分析,整个变化经历了两次旋转(动轴):
(1)先绕Z轴旋转90度(逆时针);
(2)再绕第一次旋转后的X轴旋转90度(顺时针);
也可以看做(动轴):
(1)先绕Y轴旋转90度(顺时针);
(2)再绕第一次旋转后的Z轴旋转90度(顺时针);
我们以第一种旋转为例进行介绍。建议读者按第二种方案自行联系验证。
通过手工分析草图不难发现:
(1)在1系中的原始坐标为: P(x, y, z),即:P(0.2, 0.5, 1) ;
(2)经过第一次旋转后: 在m系中的坐标应为:P’(x’, y’, z’),即: P’(0.5, -0.2, 1);
(3)经过第二次旋转后: 在2系中的坐标应为:P"(x", y", z"),即: P"(0.5, -1, -0.2);
注:m 为middle的意思。
也就是: P(0.2, 0.5, 1) ----> P"(0.5, -1, -0.2)
3. 理论基础(来源于本人:实践中测试)
(1)在使用旋转矩阵、旋转向量等进行坐标旋转时,使用公式: P_2 = T_2_1 * P_1, 可将1系下的P点,旋转至2系下;
(2)对于 T_2_1 的计算,需要注意:若为顺时针旋转,则计算过程中旋转角度值为正数; 若为逆时针旋转,则计算过程中旋转角度值为负数;
(3)若T_2_1计算中,逆时针时也将旋转角度设置为了正数,则实际计算的 旋转矩阵为: T_1_2, 需要将计算结果再求逆: T_1_2.inverse();
注意:这里的顺时针、逆时针的判定标准为:以旋转无关轴 面向相关轴所构成平面的方向视角。
比如两次旋转是按照:
(1)先绕Z轴旋转90度:从Z轴面向X、Y轴所构成的平面的视角来看,为逆时针。
(2)再绕第一次旋转后的X轴旋转90度:从X轴面向Y、Z轴所构成的平面的视角来看,为顺时针;
若两次旋转是按照:
(1)先绕Y轴旋转90度(顺时针):从Y轴面向X、Z轴所构成的平面的视角来看,为顺时针;
(2)再绕第一次旋转后的Z轴旋转90度: 从Z轴面向X、Y轴所构成的平面的视角来看,为逆时针;
4. C++ Eigen代码 - 旋转方式1
两次旋转是按照:
(1)先绕Z轴旋转90度:从Z轴面向X、Y轴所构成的平面的视角来看,为逆时针。
(2)再绕第一次旋转后的X轴旋转90度:从X轴面向Y、Z轴所构成的平面的视角来看,为顺时针;
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
//应用旋转矩阵: 旋转一个三维点
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& axis, double angle, const Eigen::Vector3d& translation)
{
Eigen::Vector3d normalized_axis = axis.normalized(); // 归一化轴向量
//转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符)
Eigen::AngleAxisd rotation_vector(angle, normalized_axis);
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
return rotation_vector.toRotationMatrix();
}
int main(int argc, char **argv)
{
//构造第一次旋转的旋转矩阵
Eigen::Matrix3d R_w1 = Eigen::Matrix3d::Identity();
//方案1: 实现与验证:
{
double angle = -M_PI / 2; //旋转角度(弧度),例如90度(PI / 2)
Eigen::Vector3d axis(0, 0, 1); //定义旋转轴: 绕z轴旋转
Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
R_w1 = GetRotationMatrix(axis, angle, translation);
//for test: 分析8个单位顶点的坐标变化:
{
std::cout << "方案1(首次旋转): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (R_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
}
}
//方案2 实现与验证:
Eigen::Matrix3d R_w1_temp = Eigen::Matrix3d::Identity();
{
double angle = M_PI / 2; //旋转角度(弧度),例如90度(PI / 2)
Eigen::Vector3d axis(0, 0, 1); //定义旋转轴: 绕z轴旋转
Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
R_w1_temp = GetRotationMatrix(axis, angle, translation);
//for test: 顶点的坐标变化:
{
std::cout << "方案2(首次旋转): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (R_w1_temp.inverse() * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
std::cout << std::endl;
}
}
}
//构造第二次旋转的旋转矩阵
Eigen::Isometry3d R_w2 = Eigen::Isometry3d::Identity();
{
double angle = M_PI / 2; // 旋转角度(弧度),例如90度(PI / 2)
Eigen::Vector3d axis(1, 0, 0); //定义旋转轴: 绕X轴旋转
Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
R_w2 = GetRotationMatrix(axis, angle, translation);
}
std::cout << "方案1(两次叠加): Above Z plane: Eigen::Vector3d( 0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (R_w2 * (R_w1 * Eigen::Vector3d(0.2, 0.5, 1))).transpose() << std::endl;
std::cout << "方案1(两次叠加, 矩阵满足结合律): Above Z plane: Eigen::Vector3d( 0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (R_w2 * R_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
std::cout << std::endl;
return 0;
}
5. C++ Eigen代码 - 旋转方式2
两次旋转是按照:
(1)先绕Z轴旋转90度:从Z轴面向X、Y轴所构成的平面的视角来看,为逆时针。
(2)再绕第一次旋转后的X轴旋转90度:从X轴面向Y、Z轴所构成的平面的视角来看,为顺时针;
两次旋转是按照:
(1)先绕Y轴旋转90度(顺时针):从Y轴面向X、Z轴所构成的平面的视角来看,为顺时针;
(2)再绕第一次旋转后的Z轴旋转90度: 从Z轴面向X、Y轴所构成的平面的视角来看,为逆时针;
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
//应用旋转矩阵: 旋转一个三维点
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& axis, double angle, const Eigen::Vector3d& translation)
{
Eigen::Vector3d normalized_axis = axis.normalized(); // 归一化轴向量
//转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符)
Eigen::AngleAxisd rotation_vector(angle, normalized_axis);
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
return rotation_vector.toRotationMatrix();
}
int main(int argc, char **argv)
{
//构造第一次旋转的旋转矩阵
Eigen::Matrix3d R_w1 = Eigen::Matrix3d::Identity();
{
//方案1: 实现与验证:
{
double angle = M_PI / 2; //旋转角度(弧度),例如90度(PI / 2)
Eigen::Vector3d axis(0, 1, 0); //定义旋转轴: 绕Y轴旋转
Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
R_w1 = GetRotationMatrix(axis, angle, translation);
//for test: 分析8个单位顶点的坐标变化:
{
//在Z平面(与Z轴垂直)以上,x,y 构成的二维平面中,逆时针寻找: (0.2, 0.5, 1), (-0.2, 0.5, 1), (-0.2,-0.5, 1), (0.2,-0.5, 1) 四个顶点
std::cout << "方案1(首次旋转): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (R_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
}
}
//方案2 实现与验证:
Eigen::Matrix3d R_w1_temp = Eigen::Matrix3d::Identity();
{
double angle = -M_PI / 2; //旋转角度(弧度),例如90度(PI / 2)
Eigen::Vector3d axis(0, 1, 0); //定义旋转轴: 绕Y轴旋转
Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
R_w1_temp = GetRotationMatrix(axis, angle, translation);
//for test: 分析8个单位顶点的坐标变化:
{
//在Z平面(与Z轴垂直)以上,x,y 构成的二维平面中,逆时针寻找: (1, 1), (-1, 1), (-1, -1), (1, -1) 四个顶点
std::cout << "方案2(首次旋转): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -0.2, 1), now is : " << (R_w1_temp.reverse() * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
}
}
}
//构造第二次旋转的旋转矩阵
Eigen::Isometry3d R_w2 = Eigen::Isometry3d::Identity();
{
double angle = -M_PI / 2; // 旋转角度(弧度),例如90度(PI / 2)
Eigen::Vector3d axis(0, 0, 1); //定义旋转轴: 绕Z轴旋转
Eigen::Vector3d translation = Eigen::Vector3d(0, 0, 0); //定义平移量
R_w2 = GetRotationMatrix(axis, angle, translation);
}
std::cout << "方案1(两次叠加): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (R_w2 * (R_w1 * Eigen::Vector3d(0.2, 0.5, 1))).transpose() << std::endl;
std::cout << "方案1(两次叠加, 矩阵满足结合律): Above Z plane: Eigen::Vector3d(0.2, 0.5, 1) --> Eigen::Vector3d(0.5, -1, -0.2), now is : " << (R_w2 * R_w1 * Eigen::Vector3d(0.2, 0.5, 1)).transpose() << std::endl;
std::cout << std::endl;
return 0;
}