Question

我有一个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循环？

Answer 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";
}

Answer 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;
};

使用相应的变换矩阵

2 个答案: