Transform some coordinates from source to target frame

transformCoordinates.cpp

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_eigen/tf2_eigen.h>  

bool transformCoordinates(const std_msgs::Float32MultiArray& points,
                            const std_msgs::Header& header,
                            std::vector<geometry_msgs::Vector3Stamped>& transformed_points);




Eigen::Matrix4d GridMappingNodelet::to_homogeneous_matrix(geometry_msgs::TransformStamped transform)
{
  // get the quaternion
  geometry_msgs::Quaternion orientation = transform.transform.rotation;
  Eigen::Quaterniond q;
  tf2::convert(orientation, q);

  // tranform the quaternion to a rotation matrix
  Eigen::Matrix3d R = q.normalized().toRotationMatrix();

  // get the translation vector
  geometry_msgs::Vector3 translation = transform.transform.translation;
  geometry_msgs::Point point;
  point.x = translation.x;
  point.y = translation.y;
  point.z = translation.z;
  Eigen::Vector3d T;
  tf2::convert(point, T);

  // use the rotation matrix and translation vector
  Eigen::Matrix4d h_mat;
  h_mat.setIdentity();  // Set to Identity to make bottom row of Matrix 0,0,0,1
  h_mat.block<3, 3>(0, 0) = R;
  h_mat.block<3, 1>(0, 3) = T;

  return h_mat;
}

bool GridMappingNodelet::transformCoordinates(const std_msgs::Float32MultiArray& points,
                                          const std_msgs::Header& header,
                                          std::vector<geometry_msgs::Vector3Stamped>& transformed_points)
{
  // sanity check
  if (points.data.size() % 3 != 0)
  {
    ROS_WARN_STREAM(ros::this_node::getName() + ": Error: Invalid cluster info points received; " +
                    "updates must be flattened tuples of {x, y, z}");
    return false;
  }

  // constants
  const std::string target_frame = "map";
  const ros::Duration timeout = ros::Duration(0.01);

  // Get the transformation to the target frame from the data frame at the time of the data
  geometry_msgs::TransformStamped transform;
  try
  {
    transform = tf_buffer_.lookupTransform(target_frame, header.frame_id, header.stamp, timeout);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_WARN_STREAM(ros::this_node::getName() + ": Error: " << ex.what() << "; skipping update");
    return false;
  }

  // Turn the transform into an Eigen matrix
  Eigen::Matrix4d transform_matrix = to_homogeneous_matrix(transform);

  // Transform the points
  size_t num_points = points.data.size() / 3;
  transformed_points.resize(num_points);

  // Index into transformed_points
  size_t i = 0;
  for (auto it = points.data.begin(); it < points.data.end(); std::advance(it, 3))
  {
    Eigen::Vector4d v_in_matrix(*it, *std::next(it), *std::next(it + 1), 1.0);
    Eigen::Vector4d v_out_matrix = transform_matrix * v_in_matrix;

    transformed_points[i].header = header;
    transformed_points[i].vector.x = v_out_matrix(0);
    transformed_points[i].vector.y = v_out_matrix(1);
    transformed_points[i].vector.z = v_out_matrix(2);

    // Increment the index
    ++i;
  }

  return true;
}

In case you need it as well, get the yaw from the quaternion:

  geometry_msgs::Quaternion camera_orientation = camera_pose.transform.rotation;
  Eigen::Quaterniond q;
  tf2::convert(camera_orientation, q);
  Eigen::Matrix3d R = q.normalized().toRotationMatrix();
  Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(0, 1, 2);
  const float camera_yaw_rad = euler_angles[0];