Transform matrix of 3D positions with correspondin

问题:

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.

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.

This can be done with a for loop as follows:

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);

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 ();
  

  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 ();
  

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

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

回答1:

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

Then, you need to interpret your 3xN matrices as a vector of Vector3d using Map:

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

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

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

Putting everything together:

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

回答2:

This answers extends ggaels accepted answer to be compatible with versions of Eigen older than 3.3.

Pre Eigen 3.3 compatibility

ScalarBinaryOpTraits is introduced in Eigen 3.3 as a replacement for internal::scalar_product_traits. Therefore, one should use internal::scalar_product_traits before Eigen 3.3:

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