3D 位置的变换矩阵与相应的变换矩阵答案

【问题标题】：Transform matrix of 3D positions with corresponding transformation matrix3D 位置的变换矩阵与相应的变换矩阵
【发布时间】：2017-06-26 11:32:29
【问题描述】：

我有一个 3D 点矩阵 (positions)，其中每一列代表一个 3D 点，在特定时间实例的本地帧中表示。

transforms（行）向量包含移动局部坐标系在每个时刻的变换矩阵，即第i个变换矩阵对应positions的第i列。

我想通过将变换矩阵应用于它们的对应点来计算全局框架 (transformed) 中的位置。

这可以通过如下的 for 循环来完成：

Eigen::Matrix<Eigen::Isometry3d, 1, Eigen::Dynamic> transforms;
Eigen::Matrix<double, 3, Eigen::Dynamic> positions, transformed;

for (int i = 0; i < positions.cols(); ++i)
    transformed.col(i) = transforms(i) * positions.col(i);

我想知道是否可以执行相同的操作来避免 for 循环。我尝试了以下两种方法，但它们给了我编译错误：

按列应用转换：
```
transformed = transforms.colwise() * positions.colwise ();
```
错误：二进制表达式的操作数无效（ColwiseReturnType（又名VectorwiseOp<Eigen::Matrix<Eigen::Transform<double, 3, 1, 0>, 1, -1, 1, 1, -1>, Vertical>）和ColwiseReturnType（又名VectorwiseOp<Eigen::Matrix<double, 3, -1, 0, 3, -1>, Vertical>））
使用数组应用转换：
```
transformed = transforms.array() * positions.array().colwise ();
```
错误：二进制表达式的操作数无效（ArrayWrapper<Eigen::Matrix<Eigen::Transform<double, 3, 1, 0>, 1, -1, 1, 1, -1> > 和 ColwiseReturnType（又名VectorwiseOp<Eigen::ArrayWrapper<Eigen::Matrix<double, 3, -1, 0, 3, -1> >, Vertical>））

问题：如何重写 for 循环以消除（显式）for 循环？

【问题讨论】：

标签： transform eigen

【解决方案1】：

这并不容易，但可行。首先，您必须告诉 Eigen，您允许 Isometry3D 和 Vector3d 之间的标量积，结果是 Vector3d：

namespace Eigen {
  template<>
  struct ScalarBinaryOpTraits<Isometry3d,Vector3d,internal::scalar_product_op<Isometry3d,Vector3d> > {
    typedef Vector3d ReturnType;
  };
}

然后，您需要使用Map 将您的 3xN 矩阵解释为 Vector3d 的向量：

auto as_vec_of_vec3 = [] (Matrix3Xd& v) { return Matrix<Vector3d,1,Dynamic>::Map(reinterpret_cast<Vector3d*>(v.data()), v.cols()); };

最后，您可以使用cwiseProduct一次执行所有产品：

as_vec_of_vec3(transformed2) = transforms.cwiseProduct(as_vec_of_vec3(positions));

把所有东西放在一起：

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;

namespace Eigen {
  template<>
  struct ScalarBinaryOpTraits<Isometry3d,Vector3d,internal::scalar_product_op<Isometry3d,Vector3d> > {
    typedef Vector3d ReturnType;
  };
}

int main()
{
  int n = 10;
  Matrix<Isometry3d, 1, Dynamic> transforms(n);
  Matrix<double, 3, Dynamic> positions(3,n), transformed(3,n);

  positions.setRandom();
  for (int i = 0; i < n; ++i)
    transforms(i).matrix().setRandom();

  auto as_vec_of_vec3 = [] (Matrix3Xd& v) { return Matrix<Vector3d,1,Dynamic>::Map(reinterpret_cast<Vector3d*>(v.data()), v.cols()); };

  as_vec_of_vec3(transformed) = transforms.cwiseProduct(as_vec_of_vec3(positions));

  cout << transformed << "\n\n";
}

【讨论】：

不错的解决方案，但不是很通用而且相当棘手。如果将点存储为Eigen::Matrix<double, Eigen::Dynamic, 3>，即为每个点使用一行，则它将不起作用。
使用internal::scalar_product_traits的Eigen 3.2.9是否也可以使用这种方法？
我不是 100% 确定 3.2.9，但您可以尝试...关于该方法的通用性，我们可以通过 1) 使用 c++11 decltype 使用户更容易避免需要 ScalarBinaryOpTraits 和 2) 通过添加方便的方法将矩阵视为嵌套向量，反之亦然。
好的，谢谢！我使用internal::scalar_product_traits 添加了一个答案。

【解决方案2】：

此答案扩展了 ggaels 接受的答案，以与早于 3.3 的 Eigen 版本兼容。

Pre Eigen 3.3 兼容性

ScalarBinaryOpTraits 在 Eigen 3.3 中被引入以替代 internal::scalar_product_traits。因此，应该在 Eigen 3.3 之前使用internal::scalar_product_traits：

template<> 
struct internal::scalar_product_traits<Isometry3d,Vector3d> {
  enum {Defined = 1};
  typedef Vector3d ReturnType;
};

【讨论】：