3D位置的变换矩阵及相应的变换矩阵 [英] Transform matrix of 3D positions with corresponding transformation matrix

问题描述

我有一个3D点矩阵（位置），其中每一列代表在特定时间实例中以本地帧表示的3D点。

I have a matrix of 3D points (positions), in which every column represents a 3D point expressed in a local frame at a specific time instance.

变换（行）向量包含每个时间点处移动局部帧的变换矩阵，即第i个变换矩阵对应与 positions 的第i列。

The transforms (row)vector contains the transformation matrix of the moving local frame at each time instance, i.e. the ith transformation matrix corresponds with the ith column of positions.

我想计算全局框架中的位置（已转换），方法是将转换矩阵应用于相应点。

I want to calculate the position in the global frame (transformed) by applying the transformation matrixes to their corresponding point.

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循环。我尝试了以下两种方法，但是它们给了我编译错误：

I was wondering if it is possible to perform the same operation avoiding the for loop. I tried the following two approaches, but they are giving me compilation errors:

• 按列应用转换：

• Apply the transformation columnwise:

transformed = transforms.colwise() * positions.colwise ();

错误：对二进制表达式无效的操作数（ ColwiseReturnType （又名 VectorwiseOp< Eigen :: Matrix< Eigen :: Transform< double，3，1，0> ;, 1，-1，1，1，-1> ;,垂直> ）和 ColwiseReturnType （aka VectorwiseOp< Eigen :: Matrix< double，3，-1，0，3 ，-1> ;，垂直> ））

error: invalid operands to binary expression (ColwiseReturnType (aka VectorwiseOp<Eigen::Matrix<Eigen::Transform<double, 3, 1, 0>, 1, -1, 1, 1, -1>, Vertical>) and ColwiseReturnType (aka VectorwiseOp<Eigen::Matrix<double, 3, -1, 0, 3, -1>, Vertical>))

使用数组应用转换：

Apply the transformation using arrays:

transformed = transforms.array() * positions.array().colwise ();

错误：对二进制表达式无效的操作数（ ArrayWrapper< Eigen :: Matrix< Eigen :: Transform< double，3、1、0、1、1，-1、1、1，-1>> 和 ColwiseReturnType （又名 VectorwiseOp< Eigen :: ArrayWrapper< Eigen :: Matrix< double，3，-1，0，3，-1> ;, Vertical> ））

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

Question: How can I rewrite the for loop to eliminate the (explicit) for loop?

推荐答案

简单但可行。首先，您必须告诉Eigen，您允许在 Isometry3D 和 Vector3d 之间进行标量积运算，结果是 Vector3d ：

That's not easy but doable. First you have to tell Eigen that you allow scalar products between an Isometry3D and a Vector3d and that the result is a Vector3d:

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

然后，您需要使用<将3xN矩阵解释为Vector3d的向量code>地图：

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

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

Finally, you can use cwiseProduct to carry out all products at once:

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

这篇关于3D位置的变换矩阵及相应的变换矩阵的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！