test.cpp:86:42: error: conversion from ‘Eigen::Matrix3d’ {aka ‘Eigen::Matrix<double, 3, 3>’} to non-scalar type ‘std::vector<std::vector<double> >’ requested
86 | std::vector<std::vector<double>> A = R;
| ^
test.cpp:88:42: error: conversion from ‘Eigen::Matrix3f’ {aka ‘Eigen::Matrix<float, 3, 3>’} to non-scalar type ‘std::vector<std::vector<double> >’ requested
88 | std::vector<std::vector<double>> B = RR;
| ^~
test.cpp:105:38: error: conflicting declaration ‘std::vector<std::vector<double> > R’
105 | std::vector<std::vector<double>> R(3, std::vector<double>(3, 0.0));
| ^
test.cpp:32:14: note: previous declaration as ‘Eigen::Matrix3d R’
32 | Matrix3d R;
| ^
test.cpp:109:21: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript
109 | R[i][j] += C[i][k] * temp[k][j];
| ^
test.cpp:118:45: error: ‘setprecision’ is not a member of ‘std’
118 | std::cout << std::fixed << std::setprecision(6)
| ^~~~~~~~~~~~
test.cpp:119:31: error: ‘setw’ is not a member of ‘std’
119 | << std::setw(12) << R[i][j];
| ^~~~
test.cpp:119:47: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript
119 | << std::setw(12) << R[i][j];
| ^
test.cpp:125:22: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript
125 | double r11 = R[0][0], r12 = R[0][1], r13 = R[0][2];
| ^
test.cpp:126:22: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript
126 | double r21 = R[1][0], r22 = R[1][1], r23 = R[1][2];
| ^
test.cpp:127:22: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript
127 | double r31 = R[2][0], r32 = R[2][1], r33 = R[2][2];
| ^
test.cpp:130:12: error: redeclaration of ‘double pitch’
130 | double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33));
| ^~~~~
test.cpp:36:17: note: ‘double pitch’ previously declared here
36 | double yaw, pitch, roll;
| ^~~~~
test.cpp:130:47: error: ‘r32’ was not declared in this scope; did you mean ‘r31’?
130 | double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33));
| ^~~
| r31
test.cpp:130:57: error: ‘r33’ was not declared in this scope; did you mean ‘r31’?
130 | double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33));
| ^~~
| r31
test.cpp:133:12: error: redeclaration of ‘double yaw’
133 | double yaw = std::atan2(r21, r11);
| ^~~
test.cpp:36:12: note: ‘double yaw’ previously declared here
36 | double yaw, pitch, roll;
| ^~~
test.cpp:136:12: error: redeclaration of ‘double roll’
136 | double roll = std::atan2(r32, r33);
| ^~~~
test.cpp:36:24: note: ‘double roll’ previously declared here
36 | double yaw, pitch, roll;
| ^~~~
In file included from /usr/include/eigen3/Eigen/Core:164,
from /usr/include/eigen3/Eigen/Dense:1,
from test.cpp:6:
/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h: In instantiation of ‘Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator[](Eigen::Index) [with Derived = Eigen::Matrix<double, 3, 3>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]’:
test.cpp:109:20: required from here
/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:408:36: error: static assertion failed: THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD
408 | EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
| ^~~~~~~~~~~~~~~~~~~~~
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:54: note: in definition of macro ‘EIGEN_STATIC_ASSERT’
33 | #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
| ^
/usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:408:36: note: ‘Eigen::Matrix<double, 3, 3>::IsVectorAtCompileTime’ evaluates to false
408 | EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime,
| ^~~~~~~~~~~~~~~~~~~~~
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:54: note: in definition of macro ‘EIGEN_STATIC_ASSERT’
33 | #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
// g++ -std=c++11 -I/usr/include/eigen3 test.cpp -o rotation_matrix
#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <vector>
using namespace Eigen;
using namespace std;
int main() {
// 输入向量
Vector3d znew(-0.0741472, 0.321715, -0.943929);
Vector3d xnew(-0.997053, 0.00682574, 0.0764145);
// 1. 归一化向量
Vector3d z_unit = znew.normalized();
Vector3d x_unit = xnew.normalized();
// 2. Gram-Schmidt正交化
// 移除x_unit在z_unit方向的分量
double dot_product = x_unit.dot(z_unit);
Vector3d x_ortho = x_unit - dot_product * z_unit;
x_ortho.normalize();
// 3. 计算正交的y轴
Vector3d y_unit = z_unit.cross(x_ortho);
y_unit.normalize();
// 4. 构建旋转矩阵
Matrix3d R;
R << x_ortho, y_unit, z_unit;
// 5. 计算欧拉角 (Z-Y-X顺序: yaw-pitch-roll)
double yaw, pitch, roll;
// 计算俯仰角 (pitch)
pitch = std::asin(-R(2, 0)); // -r31
// 处理万向锁情况 (cos(pitch) ≈ 0)
if (std::abs(std::cos(pitch)) > 1e-6) {
yaw = std::atan2(R(1, 0), R(0, 0)); // r21, r11
roll = std::atan2(R(2, 1), R(2, 2)); // r32, r33
} else {
// 万向锁时使用备用公式
yaw = std::atan2(-R(0, 1), R(1, 1));
roll = 0.0; // 横滚角自由度丢失,设为0
}
// 弧度转角度
const double rad2deg = 180.0 / M_PI;
yaw *= rad2deg;
pitch *= rad2deg;
roll *= rad2deg;
// 规范化角度到[0, 360)范围
auto normalize_angle = [](double angle) {
angle = std::fmod(angle, 360.0);
if (angle < 0) angle += 360.0;
return angle;
};
yaw = normalize_angle(yaw);
roll = normalize_angle(roll);
// 输出结果
std::cout << "Rotation Matrix R:\n" << R << "\n\n";
std::cout << "Euler Angles (Z-Y-X order, degrees):\n";
std::cout << "Yaw (ψ): " << yaw << "°\n";
std::cout << "Pitch (θ): " << pitch << "°\n";
std::cout << "Roll (φ): " << roll << "°\n";
// 定义欧拉角 (Z-Y-X 顺序)
const float yaw2 = M_PI / 2; // 绕 Z 轴
const float pitch2 = 0.0f; // 绕 Y 轴
const float roll2 = -M_PI / 2; // 绕 X 轴
// 构建旋转矩阵 (Z-Y-X 欧拉角)
Eigen::Matrix3f RR;
RR = Eigen::AngleAxisf(yaw2, Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(pitch2, Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(roll2, Eigen::Vector3f::UnitX());
std::cout << "Rotation Matrix RR:\n" << RR << std::endl;
std::vector<std::vector<double>> A = R;
std::vector<std::vector<double>> B = RR;
std::vector<std::vector<double>> C = {{-0.99595, 0.0633803, 0.0637694},
{-0.0621051, -0.997832, 0.0217865},
{0.065012, 0.0177379, 0.997727}};
// 计算 B * A
std::vector<std::vector<double>> temp(3, std::vector<double>(3, 0.0));
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
for (int k = 0; k < 3; ++k) {
temp[i][j] += B[i][k] * A[k][j];
}
}
}
// 计算 C * temp
std::vector<std::vector<double>> R(3, std::vector<double>(3, 0.0));
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
for (int k = 0; k < 3; ++k) {
R[i][j] += C[i][k] * temp[k][j];
}
}
}
// 输出合矩阵
std::cout << "Combined Matrix:" << std::endl;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
std::cout << std::fixed << std::setprecision(6)
<< std::setw(12) << R[i][j];
}
std::cout << std::endl;
}
// 提取欧拉角 (ZYX顺序)
double r11 = R[0][0], r12 = R[0][1], r13 = R[0][2];
double r21 = R[1][0], r22 = R[1][1], r23 = R[1][2];
double r31 = R[2][0], r32 = R[2][1], r33 = R[2][2];
// 计算pitch (Y轴)
double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33));
// 计算yaw (Z轴)
double yaw = std::atan2(r21, r11);
// 计算roll (X轴)
double roll = std::atan2(r32, r33);
// 转换为角度并取反
double yaw_deg = -yaw * 180.0 / M_PI; // 取反
double pitch_deg = -pitch * 180.0 / M_PI; // 取反
double roll_deg = -roll * 180.0 / M_PI; // 取反
// 角度归一化 (-180°到180°)
yaw_deg = std::fmod(yaw_deg + 180.0, 360.0) - 180.0;
pitch_deg = std::fmod(pitch_deg + 180.0, 360.0) - 180.0;
roll_deg = std::fmod(roll_deg + 180.0, 360.0) - 180.0;
// 输出修正后的欧拉角
std::cout << "\nCorrected Euler Angles (ZYX order):" << std::endl;
std::cout << "Yaw (Z) : " << yaw_deg << "°" << std::endl;
std::cout << "Pitch (Y) : " << pitch_deg << "°" << std::endl;
std::cout << "Roll (X) : " << roll_deg << "°" << std::endl;
return 0;
}请修正语法
最新发布