特征将旋转和平移组合成一个矩阵

Question

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

基本上我想要How translation a matrix(4x4) in Eigen?和Multiplying Transform and Matrix types in Eigen

的组合

我的代码（由于我不理解Affine如何工作而无法编译）如下所示：

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

Answer 1

您没有发布编译错误，也没有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;
}

Answer 2

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

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.rightCols<1>() = T;

此方法从字面上将旋转矩阵复制到前3行和列，将平移向量复制到第4列。然后将右下方的矩阵条目设置为1。 你的最终矩阵看起来像：

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

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

Answer 3

另一种方法是使用Eigen::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();
}

第一个实现和第二个实现之间的差异就像 Fix Angle 和 Euler Angle 之间的差异，您可以参考this video。

Answer 4

对我来说，最后一个anwser没有编译： 错误C2659：＆＃39; =＆＃39; ：作为左操作数的函数

    Eigen::Vector3f x       
    Eigen::Matrix3f rot;
    Eigen::Matrix4f Trans;
    Trans.setIdentity();
    Trans.block<3, 3> = rot;
    Trans.rightCols<1> = x;