Function getRotationMatrix

grid_map_pcl/src/helpers.cpp:117–136

}

Eigen::Matrix3f getRotationMatrix(double angle, XYZ axis) {
  Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
  switch (axis) {
    case XYZ::X: {
      rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitX());
      break;
    }
    case XYZ::Y: {
      rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY());
      break;
    }
    case XYZ::Z: {
      rotationMatrix = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ());
      break;
    }
    default:
      ROS_ERROR("Unknown axis while trying to rotate the pointcloud");
  }
  return rotationMatrix;
}

Eigen::Vector3d calculateMeanOfPointPositions(Pointcloud::ConstPtr inputCloud) {
  Eigen::Vector3d mean = Eigen::Vector3d::Zero();