Transform a ROS Pose msg with another pose

transform_pose.cpp

geometry_msgs::Pose transformPose(geometry_msgs::Pose& _origin_to_frame, geometry_msgs::Pose& _frame_to_target)
{
    geometry_msgs::Pose result;
    tf::Transform origin_to_frame_transform;
    origin_to_frame_transform.setOrigin(tf::Vector3(_origin_to_frame.position.x, _origin_to_frame.position.y, _origin_to_frame.position.z));
    origin_to_frame_transform.setRotation(tf::Quaternion(_origin_to_frame.orientation.x, _origin_to_frame.orientation.y,
            _origin_to_frame.orientation.z, _origin_to_frame.orientation.w));

    tf::Transform frame_to_target_transform;
    frame_to_target_transform.setOrigin(tf::Vector3(_frame_to_target.position.x, _frame_to_target.position.y, _frame_to_target.position.z));
    frame_to_target_transform.setRotation(tf::Quaternion(_frame_to_target.orientation.x, _frame_to_target.orientation.y,
                                                       _frame_to_target.orientation.z, _frame_to_target.orientation.w));

    tf::Transform origin_to_target;
    origin_to_target = origin_to_frame_transform * frame_to_target_transform;
    tf::poseTFToMsg(origin_to_target, result);
    return result;
}