自己部署运行PL-VIO 20220626

诗筱涵

已于 2024-02-25 16:14:32 修改

阅读量386

点赞数 10

分类专栏：点线SLAM 文章标签： SLAM

于 2024-02-25 16:08:56 首次发布

本文链接：https://blog.csdn.net/sinat_16643223/article/details/136283603

版权

点线SLAM 专栏收录该内容

14 篇文章 0 订阅

订阅专栏

我在笔记本的Ubuntu18.04上部署，这个Ubuntu上之前已经装过vins-fusion了所以我直接跳到这步运行，一次性编译通过

输入图片说明

这个也跑起来了

输入图片说明

它这订阅的话题名称似乎是死的，所以我把D435i的话题名称改为它对应订阅的，这么来跑

rosrun topic_tools throttle messages /camera/imu 200.0 /imu0
rosrun topic_tools throttle messages /camera/color/image_raw 30 /cam0/image_raw

roslaunch plvio_estimator euroc_fix_extrinsic.launch

输入图片说明

跑了作者自己提供的bag包

输入图片说明

搜了下是有人在D435i上跑成PL-VIO的 Ubuntu18.04用D435i运行PL-VIO_pl_vio与d435i-CSDN博客

输入图片说明

初步跑成了基于D435i的 PL-VIO ，之前没跑成是因为参考错了launch文件，应该参考euroc_fix_extrinsic.launch，而不是euroc_fix_offline.launch ，导致启动的时候说订阅不到imu和image话题

roslaunch realsense2_camera rs_camera_vins.launch
roslaunch plvio_estimator d435i.launch
roslaunch plvio_estimator vins_rviz.launch

输入图片说明

跑成时用的d435i.launch

<launch>
    <arg name="config_path" default = "$(find feature_tracker)/../config/realsense/realsense_color_config.yaml" />
      <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />

    <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="screen">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

    <node name="linefeature_tracker" pkg="feature_tracker" type="LineFeature_tracker" output="screen">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>    

    <node name="plvio_estimator" pkg="plvio_estimator" type="plvio_estimator" output="screen">
       <param name="config_file" type="string" value="$(arg config_path)" />
       <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

</launch>

跑成时用的realsense_color_config.yaml

%YAML:1.0

#common parameters
imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"
#imu_topic: "/imu0"
#image_topic: "/cam0/image_raw"
output_path: "/home/maxi/output/"

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 6.1040124511718750e+02
   fy: 6.1039495849609380e+02
   cx: 3.2079638671875000e+02
   cy: 2.2985256958007812e+02

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 0.99964621,  0.01105994,  0.02418954,
           -0.01088975,  0.9999151,  -0.00715601, 
           -0.02426663,  0.00689006,  0.99968178]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.07494282, -0.01077138, -0.00641822]

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 25            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 0             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.0002         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805       # gravity magnitude

#loop closure parameters
loop_closure: 0                    # start loop closure
fast_relocalization: 0             # useful in real-time and large project
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/maxi/output/pose_graph/" # save and load path

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0.033               # unit: s. rolling shutter read out time per frame (from data sheet). 

#visualization parameters
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4      # size of camera marker in RVIZ

是可以不飘的

输入图片说明

我刚才真的拿着D435i走了一圈，效果还可以！！！！

输入图片说明

放回到原点后偏的也还好

输入图片说明

走的时候基本是实时的。

我又走了一遍，初始化很重要

输入图片说明

是有位姿话题打印出来的 /plvio_estimator/odometry

输入图片说明

maxi@maxi:~$ rostopic list
/benchmark_publisher/path
/cam0/image_raw
/camera/accel/imu_info
/camera/align_to_color/parameter_descriptions
/camera/align_to_color/parameter_updates
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/aligned_depth_to_color/image_raw/compressed
/camera/aligned_depth_to_color/image_raw/compressed/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressed/parameter_updates
/camera/aligned_depth_to_color/image_raw/compressedDepth
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_updates
/camera/aligned_depth_to_color/image_raw/theora
/camera/aligned_depth_to_color/image_raw/theora/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/theora/parameter_updates
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/extrinsics/depth_to_infra2
/camera/gyro/imu_info
/camera/imu
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra1/image_rect_raw/compressed
/camera/infra1/image_rect_raw/compressed/parameter_descriptions
/camera/infra1/image_rect_raw/compressed/parameter_updates
/camera/infra1/image_rect_raw/compressedDepth
/camera/infra1/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra1/image_rect_raw/compressedDepth/parameter_updates
/camera/infra1/image_rect_raw/theora
/camera/infra1/image_rect_raw/theora/parameter_descriptions
/camera/infra1/image_rect_raw/theora/parameter_updates
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/infra2/image_rect_raw/compressed
/camera/infra2/image_rect_raw/compressed/parameter_descriptions
/camera/infra2/image_rect_raw/compressed/parameter_updates
/camera/infra2/image_rect_raw/compressedDepth
/camera/infra2/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra2/image_rect_raw/compressedDepth/parameter_updates
/camera/infra2/image_rect_raw/theora
/camera/infra2/image_rect_raw/theora/parameter_descriptions
/camera/infra2/image_rect_raw/theora/parameter_updates
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/clicked_point
/diagnostics
/feature_tracker/feature
/feature_tracker/feature_img
/initialpose
/linefeature_tracker/linefeature
/linefeature_tracker/linefeature_img
/move_base_simple/goal
/plvio_estimator/camera_pose
/plvio_estimator/camera_pose_visual
/plvio_estimator/history_cloud
/plvio_estimator/history_lines_cloud
/plvio_estimator/history_lines_cloud_array
/plvio_estimator/imu_propagate
/plvio_estimator/key_poses
/plvio_estimator/lines_cloud
/plvio_estimator/lines_cloud_array
/plvio_estimator/odometry
/plvio_estimator/path
/plvio_estimator/path_no_loop
/plvio_estimator/point_cloud
/plvio_estimator/pose_graph
/rosout
/rosout_agg
/tf
/tf_static
/vins_estimator/camera_pose_visual
/vins_estimator/history_cloud
/vins_estimator/point_cloud
/vins_estimator/pose_graph
maxi@maxi:~$

整个走的过程中，线特征确实是非常明显的

输入图片说明