将旋转和平移合并为一个矩阵的Eigen操作

Question

我有一个旋转矩阵rot（Eigen::Matrix3d）和一个平移向量transl（Eigen::Vector3d），我想将它们一起放在一个4x4的变换矩阵中。但是我真的无法弄清楚如何在Eigen中实现这一点。我认为可以以某种方式使用Affine，但我不理解它的工作原理。

本质上，我想要的是如何在Eigen中平移矩阵（4x4）？和在Eigen中乘以Transform和Matrix类型的组合。

我的代码（由于我不理解Affine的工作原理而无法编译）看起来像这样：

Eigen::Affine3d r(rot);
Eigen::Affine3d t(transl);
Eigen::Matrix4d m = t.matrix();
m *= r.matrix();

- DaedalusAlpha

---

由于您链接的所有答案都使用了 m = m * t.matrix()，也许问题在于缺少 *= 运算符。这种方式可行吗？ - pqnet

我不知道Eigen，但旋转矩阵通常是一个3x3的矩阵，你可以直接放入你的4x4矩阵中（假设你没有任何缩放）。在一个4x4的变换矩阵中，元素_00、_01、_02、_10、_11、_12、_20、_21和_22构成了旋转部分（如果有缩放则包括时间尺度）。元素_30、_31和_32是平移向量元素。 - rashmatash

@pqnet 不，那也不行，我认为问题在于我需要以某种方式指示我想要将不同部分“插入”到主矩阵中的位置。但是我根本不知道该怎么做。 - DaedalusAlpha

@rashmatash，有两种4x4矩阵类型：列序矩阵，例如OpenGL中使用的那些，和行序矩阵，通常在数学或物理中使用。因此，平移向量的系数可以是(3,0)(3,1)(3,2)，或者(0,3)(1,3)(2,3)，具体取决于定义。例如，请参见：https://bitbucket.org/Coin3D/coin/src/5b520d2362af5e658486de904d987b15296a5c5b/src/base/SbMatrix.cpp#lines-42 - rmbianchi

Answer 1

另一种方法是执行以下操作：

Eigen::Matrix3d R;
// Find your Rotation Matrix
Eigen::Vector3d T;
// Find your translation Vector
Eigen::Matrix4d Trans; // Your Transformation Matrix
Trans.setIdentity();   // Set to Identity to make bottom row of Matrix 0,0,0,1
Trans.block<3,3>(0,0) = R;
Trans.block<3,1>(0,3) = T;

这个方法会将旋转矩阵直接复制到第一行和第一列，将平移向量复制到第四列。然后将右下角的矩阵元素设置为1。 最终的矩阵如下：

R R R T
R R R T
R R R T
0 0 0 1

R是旋转矩阵对应的值，T是平移向量的值。

- Timothy Murphy

---

好的简单解决方案。我认为甚至可以使用 Trans.setIdentity(); 代替 Trans.setZero(4,4);，并且能够跳过最后一行 Trans(3,3) = 1;。 - DaedalusAlpha

正是我所需要的，不过我使用了Trans.block<3,1>(0,3) = T;而不是Trans.rightCols<1>() = T;以避免出现YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES错误。 - Morris Franken

谢谢，我猜他们自从我最初发布这个答案以来已经增加了更严格的类型检查。 - Timothy Murphy

你也可以写成更直观的形式，如 Trans.linear() = R; Trans.translation() = T;。此外，如果你使用 Eigen::Transform<..., Eigen::AffineCompact> 作为你的变换类型，你不需要用 setIdentity() 进行初始化，因为没有最低行。尽管如此，你可能想保留它，因为 a) 编译器很聪明（即没有运行时成本），b) 你可能想在某个时候更改类型，长时间忘记了这个“优化”。 - tobi_s

Answer 2

您没有发布编译错误，也没有说明rot和transl是什么。下面是一个可行的示例，展示了如何创建一个4x4的变换矩阵。

#include <Eigen/Geometry>

Eigen::Affine3d create_rotation_matrix(double ax, double ay, double az) {
  Eigen::Affine3d rx =
      Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
  Eigen::Affine3d ry =
      Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
  Eigen::Affine3d rz =
      Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
  return rz * ry * rx;
}

int main() {
  Eigen::Affine3d r = create_rotation_matrix(1.0, 1.0, 1.0);
  Eigen::Affine3d t(Eigen::Translation3d(Eigen::Vector3d(1,1,2)));

  Eigen::Matrix4d m = (t * r).matrix(); // Option 1

  Eigen::Matrix4d m = t.matrix(); // Option 2
  m *= r.matrix();
  return 0;
}

- Blaz Bratanic

---

谢谢，这正是我想要的！唯一的问题是，如果我直接将我的翻译向量输入到 Eigen::Translation3d() 中，它不起作用（t.matrix() 无法编译），所以我必须编写更加复杂的 Eigen::Translation3d(Eigen::Vector3d(transl(0), transl(1), transl(2)))。我猜这是由于 Eigen 模板的黑暗魔法造成的。 - DaedalusAlpha

@BlazBratanic 那么 Eigen::Transform 类是用来做什么的呢？ - David Doria

Answer 3

Another way is to use the Eigen::Transform.

Let's take an example, such as implementing this affine transform ,

#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;

Matrix4f create_affine_matrix(float a, float b, float c, Vector3f trans)
{
    Transform<float, 3, Eigen::Affine> t;
    t = Translation<float, 3>(trans);
    t.rotate(AngleAxis<float>(a, Vector3f::UnitX()));
    t.rotate(AngleAxis<float>(b, Vector3f::UnitY()));
    t.rotate(AngleAxis<float>(c, Vector3f::UnitZ()));
    return t.matrix();
}

您也可以按以下方式实现。

Matrix4f create_affine_matrix(float a, float b, float c, Vector3f trans)
{
    Transform<float, 3, Eigen::Affine> t;
    t = AngleAxis<float>(c, Vector3f::UnitZ());
    t.prerotate(AngleAxis<float>(b, Vector3f::UnitY()));
    t.prerotate(AngleAxis<float>(a, Vector3f::UnitX()));
    t.pretranslate(trans);
    return t.matrix();
}

第一种实现和第二种实现的区别就像“固定角度”和“欧拉角”的区别，您可以参考this video。

- Yantao Xie