modify some unused utils such as log record

run/data_collection_realworld.py

@@ -1,7 +1,8 @@
 """
-# 收集实飞数据，记录位置、姿态、图像，用于离线fine-tuning (保存至save_dir)
-# 注意： 由于里程计漂移，可能utils/pointcloud_clip需要对地图进行微调，需对无人机位置和yaw, pitch, roll做相同的变换
-# 注意保证地图和里程计处于同一坐标系，同时录包+保存地图
+算法具有有一定的Sim2Real的泛化能力, 如果有条件可用雷达+深度相机收集数据, 合并至仿真数据集中一同训练, 以进一步保证实飞的可靠性
+# (1) 运行雷达里程计以记录无人机状态和地图真值. 注意保证地图和里程计处于同一坐标系，请在一次运行中同时记录图像与里程计的rosbag + 保存地图
+# (可选) 由于里程计漂移，可用utils/pointcloud_clip对地图进行微调和降噪，本文件需对无人机位置translation_no和姿态R_no(yaw, pitch, roll)做相同的变换
+# (2) 播包rosbag, 运行本文件: 记录位置、姿态、图像，保存至save_dir
 """
 import cv2
 import numpy as np
@@ -12,9 +13,21 @@ from sensor_msgs.msg import Image
 from nav_msgs.msg import Odometry
 from scipy.spatial.transform import Rotation
 
-depth_img = np.zeros([270, 480])
-pos = np.array([0, 0, 0])
-quat = np.array([1, 0, 0, 0])
+# IMPORTANT PARAM
+save_dir = os.environ["FLIGHTMARE_PATH"] + "/run/yopo_realworld"
+label_path = save_dir + "/label.npz"
+if not os.path.exists(save_dir):
+    os.mkdir(save_dir)
+# Due to odometry drift, the map is adjusted, and the drone's position is also adjusted accordingly.
+R_no = Rotation.from_euler('ZYX', [0.0, 0.0, 0.0], degrees=True)  # yaw, pitch, roll
+translation_no = np.array([0.0, 0.0, 0.0])
+img_height = 270
+img_width = 480
+
+# VARIABLES
+depth_img = None
+pos = None
+quat = None
 positions = []
 quaternions = []
 frame_id = 0
@@ -22,13 +35,6 @@ new_depth = False
 new_odom = False
 first_frame = True
 last_time = time.time()
-save_dir = os.environ["FLIGHTMARE_PATH"] + "/run/depth_realworld"
-label_path = save_dir + "/label.npz"
-if not os.path.exists(save_dir):
-    os.mkdir(save_dir)
-# Due to odometry drift, the map is adjusted, and the drone's position is also adjusted accordingly.
-R_no = Rotation.from_euler('ZYX', [15, 3, 0.0], degrees=True)  # yaw, pitch, roll
-translation_no = np.array([0, 0, 2])
 
 
 def callback_odometry(data):
@@ -49,17 +55,17 @@ def callback_odometry(data):
 
 
 def callback_depth(data):
-    global depth_img, new_depth
+    global depth_img, new_depth, img_height, img_width
     max_dis = 20.0
     min_dis = 0.03
-    height = 270
-    width = 480
+    height = img_height
+    width = img_width
     scale = 0.001
     bridge = CvBridge()
     try:
         depth_ = bridge.imgmsg_to_cv2(data, "32FC1")
     except:
-        print("CV_bridge ERROR: Your ros and python path has something wrong!")
+        print("CV_bridge ERROR: Possible solutions may be found at https://github.com/TJU-Aerial-Robotics/YOPO/issues/2")
 
     if depth_.shape[0] != height or depth_.shape[1] != width:
         depth_ = cv2.resize(depth_, (width, height), interpolation=cv2.INTER_NEAREST)
@@ -106,14 +112,10 @@ def save_data(_timer):
     frame_id = frame_id + 1
 
 
-def main():
+if __name__ == "__main__":
     rospy.init_node('data_collect', anonymous=False)
     odom_ref_sub = rospy.Subscriber("/odometry/imu", Odometry, callback_odometry, queue_size=1)
     depth_sub = rospy.Subscriber("/camera/depth/image_rect_raw", Image, callback_depth, queue_size=1)
     timer = rospy.Timer(rospy.Duration(0.033), save_data)
     print("Data Collection Node Ready!")
     rospy.spin()
-
-
-if __name__ == "__main__":
-    main()

run/utils/log_plot.py

@@ -1,18 +1,17 @@
 import numpy as np
 import matplotlib.pyplot as plt
+import os
 
 if __name__ == '__main__':
-    file_path = "/home/lu/flightmare/flightmare/run/utils/dist.csv"
-    temp = np.loadtxt(file_path, dtype=float, delimiter=",")
-    file_path = "/home/lu/flightmare/flightmare/run/utils/dist_x.csv"
-    tempX = np.loadtxt(file_path, dtype=float, delimiter=",")
-    plt.plot(tempX, temp)
+    file_path = os.environ["FLIGHTMARE_PATH"] + "/run/utils/dist_log.csv"
+    dist_log = np.loadtxt(file_path, dtype=float, delimiter=",")
+    plt.plot(dist_log[:, 0], dist_log[:, 2])
     plt.show()
-    print("dist min:", np.min(temp))
-    file_path = "/home/lu/flightmare/flightmare/run/utils/ctrl_log.csv"
+    print("dist min:", np.min(dist_log[:, 2]))
+
+    file_path = os.environ["FLIGHTMARE_PATH"] + "/run/utils/ctrl_log.csv"
     ctrl_log = np.loadtxt(file_path, dtype=float, delimiter=",")
-    v_total = np.sqrt(
-        ctrl_log[:, 3] * ctrl_log[:, 3] + ctrl_log[:, 4] * ctrl_log[:, 4] + ctrl_log[:, 5] * ctrl_log[:, 5])
+    v_total = np.sqrt(ctrl_log[:, 3] * ctrl_log[:, 3] + ctrl_log[:, 4] * ctrl_log[:, 4] + ctrl_log[:, 5] * ctrl_log[:, 5])
     print("v max: ", np.max(v_total))
     plt.plot(ctrl_log[:, 3], label='vx')
     plt.plot(ctrl_log[:, 4], label='vy')

run/utils/pointcloud_clip.py

@@ -1,52 +1,38 @@
-# 实飞数据训练：将全局地图裁剪并保存
-# 1、注意数据收集时，地面尽量平，且需要为z=0
-# 2、收集数据不平时，修改yaw_angle_radians, pitch_angle_radians平移，并与data collection一致
-# 3、bug：需要打开保存的文件，手动把前面几行的double改成float...
+"""
+算法具有一定的Sim2Real的泛化能力, 如果有条件可用雷达+深度相机收集数据, 合并至仿真数据集中一同训练, 以进一步保证实飞的可靠性
+# (1) 运行雷达里程计以记录无人机状态和地图真值. 注意保证地图和里程计处于同一坐标系，请在一次运行中同时记录图像与里程计的rosbag + 保存地图
+# (可选) 运行本文件对地图进行降噪, 并可修改translation_no和R_no(yaw, pitch, roll)对地图进行变换，修正里程计漂移导致的地图倾斜，注意与data_collection_realworld一致
+        (BUG: 打开保存的地图ply文件，手动把前面几行的double改成float)
+# (3) 播包rosbag, 运行data_collection_realworld, 记录位置、姿态、图像，保存至save_dir
+"""
 
 import open3d as o3d
 import numpy as np
+from scipy.spatial.transform import Rotation
 
-# 1. 加载点云数据
-point_cloud = o3d.io.read_point_cloud("1.pcd")  # 替换为点云文件的路径
+R_no = Rotation.from_euler('ZYX', [0.0, 0.0, 0.0], degrees=True)  # yaw, pitch, roll
+translation_no = np.array([0.0, 0.0, 0.0])
 
+# 0. 加载点云数据
+point_cloud = o3d.io.read_point_cloud("map_original.pcd")  # 替换为点云文件的路径
 
-# # 统计离群点移除滤波
-# cl, ind = cropped_point_cloud.remove_statistical_outlier(nb_neighbors=5, std_ratio=1.0)  # 调整参数以控制移除离群点的程度
-# filtered_cloud = cropped_point_cloud.select_by_index(ind)
+# 1. 统计离群点移除滤波
+cl, ind = point_cloud.remove_statistical_outlier(nb_neighbors=6, std_ratio=2.0)
+point_cloud = point_cloud.select_by_index(ind)
 
-# 2. 定义旋转角度（偏航角和俯仰角）
-yaw_angle_degrees = -15  # 偏航角（以度为单位）
-pitch_angle_degrees = -3  # 俯仰角（以度为单位）
-# 3. 将角度转换为弧度
-yaw_angle_radians = np.radians(yaw_angle_degrees)
-pitch_angle_radians = np.radians(pitch_angle_degrees)
-
-yaw_rotation = np.array([[np.cos(yaw_angle_radians), -np.sin(yaw_angle_radians), 0],
-                         [np.sin(yaw_angle_radians), np.cos(yaw_angle_radians), 0],
-                         [0, 0, 1]])
-
-pitch_rotation = np.array([[np.cos(pitch_angle_radians), 0, np.sin(pitch_angle_radians)],
-                           [0, 1, 0],
-                           [-np.sin(pitch_angle_radians), 0, np.cos(pitch_angle_radians)]])
-# 4. 平移2米到Z方向
-translation_no = np.array([0, 0, 2])  # 平移2米到Z方向
-
-# 5. 组合旋转矩阵 R old->new
-R_on = np.dot(yaw_rotation, pitch_rotation)  # 内旋是右乘，先yaw后pitch
+# 2. 旋转地图以进行矫正
 #  P_n = (R_no * P_o.T).T + t_no = P_o * R_on + t_no
+R_on = R_no.inv().as_matrix()
 point_cloud.points = o3d.utility.Vector3dVector(np.dot(np.asarray(point_cloud.points), R_on) + translation_no)
 
 # o3d.visualization.draw_geometries([point_cloud])
 
+# 3. 裁剪点云无关区域
+min_bound = np.array([-50.0, -50.0, -1])
+max_bound = np.array([50.0, 50.0, 6])
 
-# 2. 定义裁剪范围
-# 例如，裁剪一个立方体范围，这里给出立方体的最小点和最大点坐标
-min_bound = np.array([-5.0, -18.0, 0])  # 最小点坐标
-max_bound = np.array([150.0, 25.0, 6])    # 最大点坐标
-
-# 3. 使用crop函数裁剪点云
 cropped_point_cloud = point_cloud.crop(o3d.geometry.AxisAlignedBoundingBox(min_bound, max_bound))
 
-o3d.io.write_point_cloud("realworld.ply", cropped_point_cloud, write_ascii=True)
+o3d.io.write_point_cloud("map_processed.ply", cropped_point_cloud, write_ascii=True)
 
 o3d.visualization.draw_geometries([cropped_point_cloud])