{"id":154,"date":"2026-03-06T01:27:06","date_gmt":"2026-03-05T17:27:06","guid":{"rendered":"http:\/\/47.104.26.10\/?p=154"},"modified":"2026-03-06T01:27:18","modified_gmt":"2026-03-05T17:27:18","slug":"%e3%80%90rmua%e3%80%91lidar2world","status":"publish","type":"post","link":"http:\/\/47.104.26.10\/index.php\/2026\/03\/06\/%e3%80%90rmua%e3%80%91lidar2world\/","title":{"rendered":"\u3010RMUA\u3011lidar2world"},"content":{"rendered":"\n<p>SUPER\u89c4\u5212\u5668\u8981\u6c42\u8f93\u5165\u7684\u70b9\u4e91\u662f\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\uff1a<\/p>\n\n\n\n<h2 class=\"wp-block-heading\">2.7 Notable Known Issues<\/h2>\n\n\n\n<ul class=\"wp-block-list\">\n<li>[#10]: When using SUPER with your own simulator (e.g., Gazebo) or a LiDAR odometry system other than FAST-LIO2, ensure that the<strong><mark style=\"background-color:#f78da7\" class=\"has-inline-color\"> input point cloud is provided in the world frame.<\/mark><\/strong> ROG-Map does not utilize <code>frame_id<\/code> or <code>\/tf<\/code> information and assumes by default that all input point clouds are in the world frame rather than the body frame.<\/li>\n<\/ul>\n\n\n\n<p>\u7136\u800c\u7ed9\u51fa\u7684lidar\u8bdd\u9898\u7684frame_id\u662f\u201clidar&#8221;,\u6240\u4ee5\u6211\u4eec\u8981\u628a\u5b83\u8f6c\u5316\u6210\u4e16\u754c\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>\u4ee3\u7801\u5982\u4e0b\uff1a<\/p>\n\n\n\n<div class=\"wp-block-group\"><div class=\"wp-block-group__inner-container is-layout-constrained wp-block-group-is-layout-constrained\">\n<details class=\"wp-block-details is-layout-flow wp-block-details-is-layout-flow\"><summary><\/summary>\n<details class=\"wp-block-details is-layout-flow wp-block-details-is-layout-flow\"><summary><\/summary>\n<p>#!\/usr\/bin\/env python3<\/p>\n\n\n\n<p>import rospy<\/p>\n\n\n\n<p>import numpy as np<\/p>\n\n\n\n<p>from sensor_msgs.msg import PointCloud2, PointField<\/p>\n\n\n\n<p>from sensor_msgs import point_cloud2<\/p>\n\n\n\n<p>from geometry_msgs.msg import PoseStamped<\/p>\n\n\n\n<p>import tf.transformations as tf_trans<\/p>\n\n\n\n<p># \u5168\u5c40\u53d8\u91cf\uff1a\u7f13\u5b58\u6700\u65b0\u7684\u771f\u503c\u4f4d\u59ff\uff08\u4f4d\u7f6e+\u59ff\u6001\uff09<\/p>\n\n\n\n<p>latest_pose_gt = None<\/p>\n\n\n\n<p>latest_pose_time = None<\/p>\n\n\n\n<p>def pose_gt_callback(pose_msg):<\/p>\n\n\n\n<p>&#8220;&#8221;&#8221;\u8ba2\u9605\u771f\u503c\u4f4d\u59ff\u7684\u56de\u8c03\u51fd\u6570\uff0c\u7f13\u5b58\u6700\u65b0\u4f4d\u59ff&#8221;&#8221;&#8221;<\/p>\n\n\n\n<p>global latest_pose_gt, latest_pose_time<\/p>\n\n\n\n<p>latest_pose_gt = pose_msg<\/p>\n\n\n\n<p>latest_pose_time = pose_msg.header.stamp<\/p>\n\n\n\n<p>def lidar_to_world(cloud_msg):<\/p>\n\n\n\n<p>&#8220;&#8221;&#8221;\u5c06lidar\u5750\u6807\u7cfb\u70b9\u4e91\u8f6c\u6362\u5230\u4e16\u754c\u5750\u6807\u7cfb\uff08\u624b\u52a8\u8ba1\u7b97\uff0c\u65e0TF\uff09&#8221;&#8221;&#8221;<\/p>\n\n\n\n<p>global latest_pose_gt, latest_pose_time<\/p>\n\n\n\n<p># \u68c0\u67e5\u662f\u5426\u6709\u53ef\u7528\u7684\u771f\u503c\u4f4d\u59ff<\/p>\n\n\n\n<p>if latest_pose_gt is None:<\/p>\n\n\n\n<p>rospy.logwarn(&#8220;No ground truth pose available!&#8221;)<\/p>\n\n\n\n<p>return None<\/p>\n\n\n\n<p># \u5339\u914d\u70b9\u4e91\u548c\u4f4d\u59ff\u7684\u65f6\u95f4\uff08\u5141\u8bb8100ms\u5185\u7684\u65f6\u95f4\u5dee\uff09<\/p>\n\n\n\n<p>time_diff = abs((cloud_msg.header.stamp &#8211; latest_pose_time).to_sec())<\/p>\n\n\n\n<p>if time_diff &gt; 0.1:<\/p>\n\n\n\n<p>rospy.logwarn(f&#8221;Pose and cloud time mismatch: {time_diff}s&#8221;)<\/p>\n\n\n\n<p># \u4ecd\u7136\u4f7f\u7528\u6700\u65b0\u4f4d\u59ff\uff08\u6bd4\u8d5b\u573a\u666f\u4e0b\u53ef\u63a5\u53d7\uff09<\/p>\n\n\n\n<p># 1. \u63d0\u53d6\u771f\u503c\u4f4d\u59ff\u7684\u4f4d\u7f6e\u548c\u59ff\u6001<\/p>\n\n\n\n<p># \u4f4d\u7f6e\uff08x,y,z\uff09<\/p>\n\n\n\n<p>px = latest_pose_gt.pose.position.x<\/p>\n\n\n\n<p>py = latest_pose_gt.pose.position.y<\/p>\n\n\n\n<p>pz = latest_pose_gt.pose.position.z<\/p>\n\n\n\n<p># \u56db\u5143\u6570\uff08x,y,z,w\uff09<\/p>\n\n\n\n<p>qx = latest_pose_gt.pose.orientation.x<\/p>\n\n\n\n<p>qy = latest_pose_gt.pose.orientation.y<\/p>\n\n\n\n<p>qz = latest_pose_gt.pose.orientation.z<\/p>\n\n\n\n<p>qw = latest_pose_gt.pose.orientation.w<\/p>\n\n\n\n<p># 2. \u6784\u5efa\u65cb\u8f6c\u77e9\u9635\uff08\u56db\u5143\u6570\u2192\u65cb\u8f6c\u77e9\u9635\uff09<\/p>\n\n\n\n<p>rot_mat = tf_trans.quaternion_matrix([qx, qy, qz, qw])[:3, :3]<\/p>\n\n\n\n<p># 3. \u89e3\u6790\u539f\u59cb\u70b9\u4e91\u6570\u636e<\/p>\n\n\n\n<p># \u63d0\u53d6\u70b9\u4e91\u7684\u6bcf\u4e2a\u70b9\uff08x,y,z\uff09<\/p>\n\n\n\n<p>gen = point_cloud2.read_points(cloud_msg, field_names=(&#8220;x&#8221;, &#8220;y&#8221;, &#8220;z&#8221;), skip_nans=True)<\/p>\n\n\n\n<p>points_lidar = np.array(list(gen)).T # \u5f62\u72b6\uff1a(3, N)<\/p>\n\n\n\n<p>if points_lidar.size == 0:<\/p>\n\n\n\n<p>rospy.logwarn(&#8220;Empty point cloud!&#8221;)<\/p>\n\n\n\n<p>return None<\/p>\n\n\n\n<p># 4. \u5750\u6807\u53d8\u6362\uff1a\u4e16\u754c\u5750\u6807 = \u65cb\u8f6c\u77e9\u9635\u00d7\u96f7\u8fbe\u5750\u6807 + \u5e73\u79fb\u5411\u91cf<\/p>\n\n\n\n<p># \u65cb\u8f6c\uff1a\u96f7\u8fbe\u5750\u6807\u7cfb\u2192\u65e0\u4eba\u673a\u672c\u4f53\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>points_body = rot_mat @ points_lidar<\/p>\n\n\n\n<p># \u5e73\u79fb\uff1a\u65e0\u4eba\u673a\u672c\u4f53\u5750\u6807\u7cfb\u2192\u4e16\u754c\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>points_world = points_body + np.array([[px], [py], [pz]])<\/p>\n\n\n\n<p># 5. \u6784\u5efa\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\u65b0\u70b9\u4e91<\/p>\n\n\n\n<p># \u590d\u5236\u539f\u59cb\u70b9\u4e91\u7684header\u548c\u7ed3\u6784\uff0c\u53ea\u66ff\u6362\u70b9\u6570\u636e<\/p>\n\n\n\n<p>cloud_world = PointCloud2()<\/p>\n\n\n\n<p>cloud_world.header = cloud_msg.header<\/p>\n\n\n\n<p>cloud_world.header.frame_id = &#8220;world&#8221; # \u6807\u8bb0\u4e3a\u4e16\u754c\u5750\u6807\u7cfb<\/p>\n\n\n\n<p>cloud_world.height = cloud_msg.height<\/p>\n\n\n\n<p>cloud_world.width = cloud_msg.width<\/p>\n\n\n\n<p>cloud_world.fields = cloud_msg.fields<\/p>\n\n\n\n<p>cloud_world.is_bigendian = cloud_msg.is_bigendian<\/p>\n\n\n\n<p>cloud_world.point_step = cloud_msg.point_step<\/p>\n\n\n\n<p>cloud_world.row_step = cloud_msg.row_step<\/p>\n\n\n\n<p>cloud_world.is_dense = cloud_msg.is_dense<\/p>\n\n\n\n<p># \u5c06\u53d8\u6362\u540e\u7684\u70b9\u91cd\u65b0\u6253\u5305\u6210PointCloud2\u683c\u5f0f<\/p>\n\n\n\n<p>points_list = []<\/p>\n\n\n\n<p>for i in range(points_world.shape[1]):<\/p>\n\n\n\n<p>x = points_world[0, i]<\/p>\n\n\n\n<p>y = points_world[1, i]<\/p>\n\n\n\n<p>z = points_world[2, i]<\/p>\n\n\n\n<p># \u4fdd\u7559\u539f\u59cb\u70b9\u4e91\u7684\u5176\u4ed6\u5b57\u6bb5\uff08\u5982intensity\uff09\uff0c\u8fd9\u91cc\u53ea\u5904\u7406x\/y\/z<\/p>\n\n\n\n<p># \u5982\u679c\u4f60\u7684\u70b9\u4e91\u6709\u5176\u4ed6\u5b57\u6bb5\uff0c\u9700\u8981\u8865\u5145<\/p>\n\n\n\n<p>points_list.append((x, y, z))<\/p>\n\n\n\n<p># \u751f\u6210\u6700\u7ec8\u7684\u70b9\u4e91\u6570\u636e<\/p>\n\n\n\n<p>cloud_world.data = point_cloud2.create_cloud(<\/p>\n\n\n\n<p>cloud_world.header,<\/p>\n\n\n\n<p>cloud_msg.fields,<\/p>\n\n\n\n<p>points_list<\/p>\n\n\n\n<p>).data<\/p>\n\n\n\n<p>return cloud_world<\/p>\n\n\n\n<p>def lidar_callback(cloud_msg, pub):<\/p>\n\n\n\n<p>&#8220;&#8221;&#8221;\u8ba2\u9605\u539f\u59cb\u70b9\u4e91\u7684\u56de\u8c03\u51fd\u6570\uff0c\u8f6c\u6362\u540e\u53d1\u5e03&#8221;&#8221;&#8221;<\/p>\n\n\n\n<p>cloud_world = lidar_to_world(cloud_msg)<\/p>\n\n\n\n<p>if cloud_world is not None:<\/p>\n\n\n\n<p>pub.publish(cloud_world)<\/p>\n\n\n\n<p>if __name__ == &#8216;__main__&#8217;:<\/p>\n\n\n\n<p># \u521d\u59cb\u5316ROS\u8282\u70b9<\/p>\n\n\n\n<p>rospy.init_node(&#8216;lidar_to_world_with_pose_gt&#8217;, anonymous=True)<\/p>\n\n\n\n<p># \u53d1\u5e03\u8f6c\u6362\u540e\u7684\u4e16\u754c\u5750\u6807\u7cfb\u70b9\u4e91<\/p>\n\n\n\n<p>pub_world_lidar = rospy.Publisher(<\/p>\n\n\n\n<p>&#8216;\/drone_1\/lidar_world&#8217;, # \u65b0\u7684\u70b9\u4e91\u8bdd\u9898\u540d<\/p>\n\n\n\n<p>PointCloud2,<\/p>\n\n\n\n<p>queue_size=10<\/p>\n\n\n\n<p>)<\/p>\n\n\n\n<p># \u8ba2\u9605\u771f\u503c\u4f4d\u59ff<\/p>\n\n\n\n<p>rospy.Subscriber(<\/p>\n\n\n\n<p>&#8216;\/airsim_node\/drone_1\/debug\/pose_gt&#8217;,<\/p>\n\n\n\n<p>PoseStamped,<\/p>\n\n\n\n<p>pose_gt_callback,<\/p>\n\n\n\n<p>queue_size=10<\/p>\n\n\n\n<p>)<\/p>\n\n\n\n<p># \u8ba2\u9605\u539f\u59cblidar\u70b9\u4e91\uff08frame_id=lidar\uff09\uff0c\u5e76\u4f20\u5165\u53d1\u5e03\u5668<\/p>\n\n\n\n<p>rospy.Subscriber(<\/p>\n\n\n\n<p>&#8216;\/airsim_node\/drone_1\/lidar&#8217;, # \u66ff\u6362\u4e3a\u4f60\u7684\u539f\u59cblidar\u8bdd\u9898<\/p>\n\n\n\n<p>PointCloud2,<\/p>\n\n\n\n<p>lambda msg: lidar_callback(msg, pub_world_lidar),<\/p>\n\n\n\n<p>queue_size=10<\/p>\n\n\n\n<p>)<\/p>\n\n\n\n<p>rospy.loginfo(&#8220;Lidar to world converter (with pose GT) started!&#8221;)<\/p>\n\n\n\n<p>rospy.spin()<\/p>\n<\/details>\n<\/details>\n<\/div><\/div>\n","protected":false},"excerpt":{"rendered":"<p>SUPER\u89c4\u5212\u5668\u8981\u6c42\u8f93\u5165\u7684\u70b9\u4e91\u662f\u4e16\u754c\u5750\u6807\u7cfb\u4e0b\u7684\uff1a 2.7 Notable Known Issues \u7136\u800c\u7ed9\u51fa\u7684 [&hellip;]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"_bbp_topic_count":0,"_bbp_reply_count":0,"_bbp_total_topic_count":0,"_bbp_total_reply_count":0,"_bbp_voice_count":0,"_bbp_anonymous_reply_count":0,"_bbp_topic_count_hidden":0,"_bbp_reply_count_hidden":0,"_bbp_forum_subforum_count":0,"footnotes":""},"categories":[1],"tags":[],"class_list":["post-154","post","type-post","status-publish","format-standard","hentry","category-uncategorized"],"_links":{"self":[{"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/posts\/154","targetHints":{"allow":["GET"]}}],"collection":[{"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/comments?post=154"}],"version-history":[{"count":1,"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/posts\/154\/revisions"}],"predecessor-version":[{"id":155,"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/posts\/154\/revisions\/155"}],"wp:attachment":[{"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/media?parent=154"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/categories?post=154"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/47.104.26.10\/index.php\/wp-json\/wp\/v2\/tags?post=154"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}