矩阵转方向向量，用C++程序

可以使用以下代码将矩阵转换为方向向量：

#include <stdio.h>
#include <math.h>

typedef struct {
    float x, y, z;
} Vector3;

typedef struct {
    float m[3][3];
} Matrix3x3;

void matrixToDirectionVector(Matrix3x3 m, Vector3* v) {
    v->x = m.m[0][2];
    v->y = m.m[1][2];
    v->z = m.m[2][2];
}

int main() {
    Matrix3x3 m = {
        {1, 0, 0},
        {0, 0, -1},
        {0, 1, 0}
    };
    Vector3 v;
    matrixToDirectionVector(m, &v);
    printf("Direction vector: (%f, %f, %f)\n", v.x, v.y, v.z);
    return 0;
}

这里的矩阵是一个 3x3 的旋转矩阵，其中第三列就是方向向量。在这个例子中，我们将矩阵设置为绕 Y 轴旋转 90 度，所以方向向量为 (0, 0, -1)。

相关问题

C++ 矩阵转方向向量

可以使用以下代码将矩阵转换为方向向量：

import numpy as np

# 定义矩阵
matrix = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])

# 提取方向向量
direction_vector = matrix[:, 2]

print(direction_vector)

这将输出矩阵的第三列，即方向向量。

ros中rpy转姿态矩阵的C++程序

可以使用Eigen库来实现。具体代码如下：

#include <Eigen/Core>
#include <Eigen/Geometry>

Eigen::Matrix3d rpyToRotationMatrix(const Eigen::Vector3d& rpy)
{
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ())
                    * Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX());
    return rotation_matrix;
}

其中，rpy是一个3维向量，分别表示roll、pitch和yaw角度。函数返回一个3x3的旋转矩阵，表示对应的姿态。