modify some unused utils such as log record

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])