modify some unused utils such as log record
This commit is contained in:
TJU_Lu
parent
38f37fd7a2
commit
73908cc899
@ -7,17 +7,17 @@
|
||||
#include <pcl/search/kdtree.h>
|
||||
#include <ros/ros.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "flightlib/controller/ctrl_ref.h"
|
||||
|
||||
namespace traj_eval {
|
||||
|
||||
// Evaluate the performance of the trajectory from the origin [start_record, y, z] to destination [finish_record, y, z]
|
||||
float start_record = -39;
|
||||
float finish_record = 18;
|
||||
float ctrl_dt = 0.02;
|
||||
// eval
|
||||
std::ofstream log_, log_x, ctrl_log;
|
||||
std::ofstream dist_log, ctrl_log;
|
||||
Eigen::Vector3f pose_last;
|
||||
Eigen::Vector3f acc_last;
|
||||
ros::Time start, end;
|
||||
@ -67,7 +67,7 @@ void odom_cb(const nav_msgs::Odometry::Ptr odom_msg) {
|
||||
if (!odom_init && odom_msg->pose.pose.position.x > start_record) {
|
||||
odom_init = true;
|
||||
pose_last = Eigen::Vector3f(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
|
||||
start = ros::Time::now();
|
||||
start = odom_msg->header.stamp;
|
||||
std::cout << "start!" << std::endl;
|
||||
return;
|
||||
}
|
||||
@ -77,18 +77,19 @@ void odom_cb(const nav_msgs::Odometry::Ptr odom_msg) {
|
||||
Eigen::Vector3f pose_cur(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z);
|
||||
length_ += (pose_cur - pose_last).norm();
|
||||
dist_ += distance_at(pose_cur);
|
||||
log_ << distance_at(pose_cur) << std::endl;
|
||||
log_x << pose_cur(0) << std::endl;
|
||||
|
||||
dist_log << (odom_msg->header.stamp - start).toSec() << ',';
|
||||
dist_log << pose_cur(0) << ',';
|
||||
dist_log << distance_at(pose_cur) << std::endl;
|
||||
|
||||
pose_last = pose_cur;
|
||||
num_++;
|
||||
if (odom_msg->pose.pose.position.x > finish_record) {
|
||||
odom_finish = true;
|
||||
end = ros::Time::now();
|
||||
std::cout << "finish! \n time:" << (end - start).toSec() << " s,\nlength:" << length_ << " m,\ndist:" << dist_ / num_
|
||||
end = odom_msg->header.stamp;
|
||||
std::cout << "finish! \ntime:" << (end - start).toSec() << " s,\nlength:" << length_ << " m,\ndist:" << dist_ / num_
|
||||
<< " m,\nctrl cost:" << ctrl_cost_ << " m2/s5" << std::endl;
|
||||
log_.close();
|
||||
log_x.close();
|
||||
dist_log.close();
|
||||
ctrl_log.close();
|
||||
}
|
||||
}
|
||||
@ -103,9 +104,9 @@ void ctrl_cb(const quad_pos_ctrl::ctrl_ref& ctrl_msg) {
|
||||
}
|
||||
|
||||
Eigen::Vector3f cur_acc(ctrl_msg.acc_ref[0], -ctrl_msg.acc_ref[1], -ctrl_msg.acc_ref[2]);
|
||||
Eigen::Vector3f d_acc = (acc_last - cur_acc) / 0.02;
|
||||
Eigen::Vector3f d_acc = (acc_last - cur_acc) / ctrl_dt;
|
||||
float acc_norm2 = d_acc.dot(d_acc);
|
||||
ctrl_cost_ += 0.02 * acc_norm2;
|
||||
ctrl_cost_ += ctrl_dt * acc_norm2;
|
||||
acc_last = cur_acc;
|
||||
|
||||
ctrl_log << ctrl_msg.pos_ref[0] << ',';
|
||||
@ -126,16 +127,13 @@ void ctrl_cb(const quad_pos_ctrl::ctrl_ref& ctrl_msg) {
|
||||
using namespace traj_eval;
|
||||
int main(int argc, char** argv) {
|
||||
map_input();
|
||||
// Move to the same log file.
|
||||
std::string log_file = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist.csv");
|
||||
|
||||
std::string log_file = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist_log.csv");
|
||||
std::cout << "log path:" << log_file << std::endl;
|
||||
log_.open(log_file.c_str(), std::ios::out);
|
||||
dist_log.open(log_file.c_str(), std::ios::out);
|
||||
|
||||
std::string log_file2 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/dist_x.csv");
|
||||
log_x.open(log_file2.c_str(), std::ios::out);
|
||||
|
||||
std::string log_file3 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/ctrl_log.csv");
|
||||
ctrl_log.open(log_file3.c_str(), std::ios::out);
|
||||
std::string log_file2 = getenv("FLIGHTMARE_PATH") + std::string("/run/utils/ctrl_log.csv");
|
||||
ctrl_log.open(log_file2.c_str(), std::ios::out);
|
||||
|
||||
ros::init(argc, argv, "traj_eval");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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')
|
||||
|
||||
@ -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])
|
||||
Loading…
x
Reference in New Issue
Block a user