Last active
September 22, 2021 23:38
-
-
Save sgarciav/1227d4011a72c730b1972ce327948f59 to your computer and use it in GitHub Desktop.
Transform some coordinates from source to target frame
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
In case you need it as well, get the yaw from the quaternion: