Function getRigidBodyTransform

grid_map_pcl/src/helpers.cpp:104–115

}

Eigen::Affine3f getRigidBodyTransform(const Eigen::Vector3d& translation, const Eigen::Vector3d& intrinsicRpy) {
  Eigen::Affine3f rigidBodyTransform;
  rigidBodyTransform.setIdentity();
  rigidBodyTransform.translation() << translation.x(), translation.y(), translation.z();
  Eigen::Matrix3f rotation(Eigen::Matrix3f::Identity());
  rotation *= getRotationMatrix(intrinsicRpy.x(), XYZ::X);
  rotation *= getRotationMatrix(intrinsicRpy.y(), XYZ::Y);
  rotation *= getRotationMatrix(intrinsicRpy.z(), XYZ::Z);
  rigidBodyTransform.rotate(rotation);

  return rigidBodyTransform;
}

Eigen::Matrix3f getRotationMatrix(double angle, XYZ axis) {
  Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();