在DJI无人机上运行VINS-FUISON（PSDK 转 ROS）

#coding:utf-8
import matplotlib.pyplot as plt

# 用于提取position_vins和position_true的数据
def extract_position_data(filename):
    position_vins_x = []
    position_vins_y = []
    position_true_x = []
    position_true_y = []

    with open(filename, 'r') as file:
        lines = file.readlines()

        for i in range(0, len(lines), 4):
            # 每四行数据，分别为position_vins和position_true
            if 'position_vins' in lines[i]:
                # 提取x, y坐标
                vins_coords = list(map(float, lines[i].split()[1:3]))
                position_vins_x.append(vins_coords[0])
                position_vins_y.append(vins_coords[1])

            if 'position_true' in lines[i+1]:
                # 提取x, y坐标
                true_coords = list(map(float, lines[i+1].split()[1:3]))
                position_true_x.append(true_coords[0])
                position_true_y.append(true_coords[1])

    return position_vins_x, position_vins_y, position_true_x, position_true_y

# 设置图形
plt.figure(figsize=(10, 8))

# 处理每个文件并绘制数据
for i in range(6):
    filename = '/home/tju/code/uav_code/data/25_114_all/position_compare' + str(i) + '.txt'
    position_vins_x, position_vins_y, position_true_x, position_true_y = extract_position_data(filename)

    # 绘制position_vins和position_true的x, y数据
    if i == 0 or i ==1 or i == 2:
        for j in range(len(position_vins_y)):
            temp = position_vins_x[j]
            position_vins_x[j] = position_vins_y[j]
            position_vins_y[j] = temp

            temp = position_true_x[j]
            position_true_x[j] = position_true_y[j]
            position_true_y[j] = temp

    elif i == 3:
        for j in range(len(position_vins_y)):
            position_vins_x[j] = position_vins_x[j]*1.3 + 0
            position_true_x[j] = position_true_x[j]*1.3 + 0
            position_vins_y[j] = position_vins_y[j] - 7
            position_true_y[j] = position_true_y[j] - 7
    if i == 4:
        for j in range(len(position_vins_y)):
            position_vins_x[j] = position_vins_x[j]*0.9 + 0
            position_true_x[j] = position_true_x[j]*0.9 + 0
            position_vins_y[j] = (position_vins_y[j]-3)*0.8 + 6
            position_true_y[j] = (position_true_y[j]-3)*0.8 + 6
    if i == 5:
        for j in range(len(position_vins_y)):
            position_vins_x[j] = position_vins_x[j] 
            position_true_x[j] = position_true_x[j] 
            position_vins_y[j] = -position_vins_y[j] - 9
            position_true_y[j] = -position_true_y[j] - 9
    
    plt.plot(position_vins_x, position_vins_y, label='file'+ str(i) +' position_vins', linestyle='-', marker='o')
    plt.plot(position_true_x, position_true_y, label='file'+ str(i) +' position_true', linestyle='--', marker='x')
    

# 添加标签和标题
plt.xlabel('X')
plt.ylabel('Y')
plt.title('Position Data Comparison: position_vins vs position_true')

# 显示图例
plt.legend()

# 显示图像
plt.grid(True)
plt.show()

import matplotlib.pyplot as plt

# 用于提取position_vins和position_true的数据
def extract_position_data(filename):
    position_vins_x = []
    position_vins_y = []
    position_true_x = []
    position_true_y = []

    with open(filename, 'r') as file:
        lines = file.readlines()

        for i in range(0, len(lines), 4):
            # 每四行数据，分别为position_vins和position_true
            if 'position_vins' in lines[i]:
                # 提取x, y坐标
                vins_coords = list(map(float, lines[i].split()[1:3]))
                position_vins_x.append(vins_coords[0])
                position_vins_y.append(vins_coords[1])

            if 'position_true' in lines[i+1]:
                # 提取x, y坐标
                true_coords = list(map(float, lines[i+1].split()[1:3]))
                position_true_x.append(true_coords[0])
                position_true_y.append(true_coords[1])

    return position_vins_x, position_vins_y, position_true_x, position_true_y

# 设置图形
plt.figure(figsize=(10, 8))

# 处理每个文件并绘制数据
for i in range(6):
    filename = f'file{i}.txt'
    position_vins_x, position_vins_y, position_true_x, position_true_y = extract_position_data(filename)

    # 绘制position_vins和position_true的x, y数据
    plt.plot(position_vins_x, position_vins_y, label=f'file{i} position_vins', linestyle='-', marker='o')
    plt.plot(position_true_x, position_true_y, label=f'file{i} position_true', linestyle='--', marker='x')

# 添加标签和标题
plt.xlabel('X')
plt.ylabel('Y')
plt.title('Position Data Comparison: position_vins vs position_true')

# 显示图例
plt.legend()

# 显示图像
plt.grid(True)
plt.show()

def process_file(input_file, output_file):
    # 读取文件内容
    with open(input_file, 'r') as file:
        lines = file.readlines()

    # 创建一个新的列表来存储需要保留的行
    filtered_lines = []

    # 每4行为一组，处理每组数据
    group_size = 4
    for i in range(0, len(lines), group_size * 5):  # 每5组处理一次
        # 保留每5组的第1、3、5组
        for j in range(i, i + group_size * 5, group_size):
            if (j // group_size) % 5 == 0 or (j // group_size) % 5 == 2:  # 保留1, 3组
                filtered_lines.extend(lines[j:j + group_size])
    
    # 将处理后的内容写入新的文件
    with open(output_file, 'w') as file:
        file.writelines(filtered_lines)

# 使用函数处理文件
input_file = 'file.txt'
output_file = 'file1.txt'
process_file(input_file, output_file)

position_vins 2.84912247698 0.0446324738242 2.00463062006 
position_true 2.85163504303 0.0447696928743 2.08136236072 
orientation_vins 0.999615327697 0.00260711206212 -0.00216247289634 0.0275267744291 
orientation_true 0.999531822868 0.0045715227515 -0.0122801073325 0.0276484216059 
position_vins 2.84912247698 0.0446324738242 2.00463062006 
position_true 2.85122524206 0.045977640623 2.08300243258 
orientation_vins 0.999615327697 0.00260711206212 -0.00216247289634 0.0275267744291 
orientation_true 0.999542971805 0.00443534362677 -0.0113719704139 0.0276559854341 
position_vins 2.84653644361 0.0470195727081 2.01099070785 
position_true 2.85019187546 0.0467633082124 2.08438475901 
orientation_vins 0.999553932316 0.00409873603733 -0.0107486784506 0.0275608901519 
orientation_true 0.999561682082 0.00422145648037 -0.00967730964391 0.0276581397761 
position_vins 2.84653644361 0.0470195727081 2.01099070785 
position_true 2.84926480317 0.0471198606585 2.08523224714 
orientation_vins 0.999553932316 0.00409873603733 -0.0107486784506 0.0275608901519 
orientation_true 0.999573278513 0.00405237640002 -0.00846691930475 0.0276613521864 
position_vins 2.84599056365 0.0501542811605 2.01363349549 
position_true 2.84705727083 0.0475243804426 2.08682458035 
orientation_vins 0.999525128715 0.0041709415652 -0.0131166470877 0.0275694374753 
orientation_true 0.99959121302 0.00368888474616 -0.00622949272797 0.0276584960358 
position_vins 2.84599056365 0.0501542811605 2.01363349549 
position_true 2.8451848357 0.047618266003 2.08794672663 
orientation_vins 0.999525128715 0.0041709415652 -0.0131166470877 0.0275694374753 
orientation_true 0.999599462331 0.00356949918128 -0.00480684019671 0.0276598602883 
position_vins 2.84601595498 0.0512795973944 2.01575531078 
position_true 2.84314411141 0.0475584461858 2.08903289771 
orientation_vins 0.999547362797 0.0039462720193 -0.0112762771378 0.0276105421146 
orientation_true 0.999604560753 0.00346763903849 -0.00358370039537 0.0276740798132 
position_vins 2.8451015541 0.0524412606438 2.01714807722 
position_true 2.84079622434 0.0473283683005 2.09018682492 
orientation_vins 0.999579186619 0.0034655025422 -0.00814827646463 0.0276232793281 
orientation_true 0.999607701898 0.00328812759771 -0.00246479652447 0.0277047884058 
position_vins 2.8451015541 0.0524412606438 2.01714807722 
position_true 2.83934571939 0.0471128351959 2.09085651925 
orientation_vins 0.999579186619 0.0034655025422 -0.00814827646463 0.0276232793281 
orientation_true 0.999608645621 0.00315090526737 -0.00191562999632 0.0277300875712 
position_vins 2.8451015541 0.0524412606438 2.01714807722 
position_true 2.83540564047 0.0462821610057 2.09247558858 
orientation_vins 0.999579186619 0.0034655025422 -0.00814827646463 0.0276232793281 
orientation_true 0.999610107562 0.00274874523658 -0.00101793495479 0.0277676262491 

安装ceres出现以下报错，将2版本的ceres换成1版本的ceres

CMake did not find one.

  Could not find a package configuration file provided by "absl" with any of
  the following names:

    abslConfig.cmake
    absl-config.cmake

  Add the installation prefix of "absl" to CMAKE_PREFIX_PATH or set
  "absl_DIR" to a directory containing one of the above files.  If "absl"
  provides a separate development package or SDK, be sure it has been
  installed.

衔接桥梁

dajiang SDK psdk
https://developer.dji.com/doc/payload-sdk-api-reference/cn/

psdk->ros2
https://github.com/umdlife/psdk_ros2
https://umdlife.github.io/psdk_ros2/api/library_root.html

ros2 <-> ros1
ros1_bridge

ROS 1 和 ROS 2 的环境变量不能同时写入 .bashrc 文件中，因为它们的环境变量会相互冲突=>>手动切换环境

sudo apt update
sudo apt install ros-galactic-ros1-bridge

https://blog.csdn.net/2302_80225397/article/details/141167175

开启录制

# 1
screen
sudo su

cd /home/nvidia/psdk_ros2
sh /home/nvidia/change_device_mode.sh 

source /opt/ros/galactic/setup.bash 
source install/setup.bash 
ros2 launch psdk_wrapper wrapper.launch.py 

# 2
screen 
cd /home/nvidia/psdk_ros2
source /opt/ros/galactic/setup.bash 
source install/setup.bash 
ros2 service call /wrapper/psdk_ros2/start_perception psdk_interfaces/srv/PerceptionStereoVisionSetup \
    "{stereo_cameras_direction: 'FRONT', start_stop: true}"

DATE_STR=$(date +%Y%m%d_%H%M%S)

ros2 bag record -o outputs/$DATE_STR \
    /wrapper/psdk_ros2/acceleration_body_raw \
    /wrapper/psdk_ros2/angular_rate_body_raw \
    /wrapper/psdk_ros2/perception_stereo_left_stream \
    /wrapper/psdk_ros2/perception_stereo_right_stream 

## ros2 bag to ros1
rosbags-convert outputs/$DATE_STR --dst $DATE_STR.bag

• screen是后台运行一个终端

查看所有后台的终端

screen -ls 

关闭某个名为172199.pts-1.nvidia-desktop的终端

screen -X -S 172199.pts-1.nvidia-desktop quit  

• 远程拷贝录制bag

scp 20250103_102425.bag liuchengyi@192.168.111.171:/home/liuchengyi/Desktop

• vscode远程连接断掉

fn + f1重启

• roscore端口占用

查看端口占用情况

sudo lsof -i :11311

终止占用端口的进程

sudo kill <PID>

标定

• IMU标定

IMU标定参考港科大imu_utils框架：https://blog.csdn.net/qq_35616298/article/details/116190164

要先下载code_util编译，再下载imu_utils

code_util可能出现opencv版本号不符合的报错，更换opencv写法

注意：当工作空间里面有非ros包时，要用catkin_make_isolated，而非catkin_make

编译imu_utils出现以下报错，在mu_an.cpp加入#include <fstream>的头文件

/home/liuchengyi/imuCali_ws/src/imu_utils/src/imu_an.cpp:90:19: error: aggregate ‘std::ofstream out_t’ has incomplete type and cannot be defined
   90 |     std::ofstream out_t;

先录制静止的imu包

cd ~/Desktop/imuCal_ws
source ~/Desktop/imuCal_ws/devel/setup.bash
roslaunch imu_utils dji_imu.launch

数据存储到了~/Desktop/imuCal_ws/src/imu_utils/dji文件夹，参考其中的dji_imu_imu_param.yaml文件

• 双目标定

https://blog.csdn.net/m0_71523511/article/details/139969032

# d435
kalibr_calibrate_cameras --target checkerboard.yaml --bag mult_cam_d435i.bag 
--models pinhole-radtan pinhole-radtan pinhole-radtan --topics /color /infra_left /infra_right
#t265
kalibr_calibrate_cameras --target checkerboard.yaml --bag camer_t265.bag 
--models omni-radtan omni-radtan --topics /fisheye1 /fisheye2
#stereo
rosrun kalibr kalibr_calibrate_cameras --target src/kalibr/april_6_6.yaml --bag src/kalibr/stereo_4hz.bag 
--models pinhole-radtan pinhole-radtan --topics /image_raw_right /image_raw_right --show-extraction

source ~/Desktop/kalibr_ws/devel/setup.bash 
rosrun kalibr kalibr_calibrate_cameras --target ~/Desktop/kalibr_ws/src/apriltag.yaml --bag ~/Desktop/transfer_imu_stereo_2025-01-03-17-32-28.bag --models pinhole-radtan pinhole-radtan --topics /dji/left/camera /dji/right/camera  --approx-sync 0.01

标定的相机没有畸变，更换模型pinhole-radtan->pinhole

rosrun kalibr kalibr_calibrate_cameras --target ~/Desktop/kalibr_ws/src/apriltag.yaml --bag ~/Desktop/20250103_155443.bag --models pinhole pinhole --topics /wrapper/psdk_ros2/perception_stereo_left_stream /wrapper/psdk_ros2/perception_stereo_right_stream  --approx-sync 0.01

kalibr_calibrate_imu_camera --target ~/Desktop/kalibr_ws/src/apriltag.yaml --cam camchain-mult_cam_d435i.yaml --imu imu.yaml --bag imu_stereo.bag

rosbags-convert outputs/$DATE_STR --dst $DATE_STR.bag

foxglove显示，比rviz稳定
https://foxglove.dev/download

因为从无人机上录制的imu的AccelStamped类型的加速度数据和Vector3Stamped类型的陀螺仪数据是分开的，需要把这两个类型数据合并为sensor_msgs/Imu类型
运行

python3 ~/Desktop/IMU_stereo_transfer.py

然后开启录制

rosbag record /dji/imu /dji/left/camera /dji/right/camera

然后用生成的数据包跑vins会报错

[ WARN] [1736218109.682700794]: waiting for image and imu...
terminate called after throwing an instance of 'cv_bridge::Exception'
  what():  Image is wrongly formed: height * step != size  or  480 * 480 != 233472
Aborted (core dumped)

重新保存相机数据，运行

python3 ~/Desktop/deal_image.py

然后重新录制数据包

rosbag record /dji/imu /dji/left/camera/modify /dji/right/camera/modify

vins

source ~/Desktop/vins_ws/devel/setup.bash
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/Desktop/vins_ws/src/VINS-Fusion/config/dji/dji_stereo_imu_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
rosbag play ~/Desktop/2025-01-07-11-47-31.bag

vins有很大的drift，直接双目版本可能由于双目+imu的版本，因为后者对时间戳对齐非常精细

    roslaunch vins vins_rviz.launch
    rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
    rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag