开始前说明GVINS相关代码链接GitHub - HKUST-Aerial-Robotics/GVINS: Tightly coupled GNSS-Visual-Inertial system for locally smooth and globally consistent state estimation in complex environment.本文章主要写如何使用自己硬件设备将GVINS跑通默认在此之前已经将所需的依赖如ceres,eigen和其他环境均装好并且已经可以在数据集上成功运行。数据集上成功运行的博客可以参考跑通GVINS——港科大新作-CSDN博客1.硬件选择之前在选择硬件之前需要首先搞清楚GVINS中相机和GNSS信息的对齐机制。在GVINS提供的数据集中相机的图像信息频率是20Hz而GNSS接收器提供的卫星定位信息为10Hz.为了将其对齐GVINS采取了跳帧的策略每两帧之间选取离GNSS信息更近的帧保留另一帧跳过。这样每帧便都会有一条GNSS信息与之对应。下图是GVINS中的跳帧策略的代码为了适配这种跳帧策略在使用我们自己的传感器运行GVINS时候有必要将相机图像的频率和GNSS信息的频率的比例调整为21如GVINS数据集中相机 20HzGNSS 10Hz。2.卫星定位天线和接收机选择GVINS需要的GNSS信息有三种分别是卫星测量数据星历和电离层参数。GVINS论文中硬件使用Zed-f9p作为卫星定位的接收机来接收GNSS原始数据再通过ublox-driver驱动来将数据转化为GVINS所需要的格式并发布到ROS上。由于GVINS需要特定的GNSS格式接收机建议选择论文中使用的Zed-f9p。市面上的其他接收机不一定能准确地转换为GVINS所使用的格式也不一定能和对应相机的频率相匹配。Zed-f9p在淘宝上就有链接Zed-f9p天线选择任意的多频段天线就好。卫星定位设备接在设备上之前需要先使用ucenter在windows上将接收机的配置修改为GVINS可以使用的形式。操作时需要将天线放到室外。如何操作ucenter修改为GVINS可使用的格式的文章我会随后再写在此之前可以先查看ucenter说明文档或者询问购买时候的客服。主要修改的有三项1接收机的接收信息参考ublox-driver项目的readme部分需要将UBX-RXM-RAWX,UBX-RXM-SFRBX和UBX-NAV-PVT三种数据输出。经过测试发现只输出UBX-RXM-RAWX,UBX-RXM-SFRBX两种数据便可以满足GVINS的需求。UBX-NAV-PVT输出的为接收机的LLA(经度维度海拔)GVINS不需要LLA。2定位信息频率频率主要需要满足1.中所说的“相机图像的频率和GNSS信息的频率为21”。我使用的相机是Realsense D455,相机图像频率是30因此我需要将GNSS频率尽可能维持在15。Zed-f9p只能通过修改时间间隔来确定GNSS频率而且时间间隔为毫秒单位的整数我这里设为66ms频率约为15.15。3波特率因为GNSS信息频率较高波特率过小可能导致信息无法解析我这里直接设为921600。配置完成并保存后需要安装gnss_comm和ublox-driver驱动,链接分别为gnss_comm : https://github.com/HKUST-Aerial-Robotics/gnss_commublox-driver: https://github.com/HKUST-Aerial-Robotics/ublox_driver编译完成后如下图所示修改ublox-driver的配置文件主要保证接收机波特率和配置文件中的一致。修改完成后运行roslaunch ublox_driver ublox_driver.launch启动后可以使用rostopic相关的命令查看是否成功获取数据。3.相机选择因为要满足“相机图像的频率和GNSS信息的频率为21”而Zed-f9p的频率最大为20Hz如果使用了Zed-f9p的话相机一定要保证图像频率≤40Hz。我使用了实验室正好闲置的Realsense D455相机。安装Reaslsense驱动并在ROS上可以成功发布图像信息的参考文章有很多,可以参考ROS1 Realsense d455 固件安装读取rostopic数据_ros1noetic部署realsensed455-CSDN博客最后修改realsense的配置文件将左红外infra1打开频率设置为两倍的GNSS频率我这里是设为30。并设置一个可行的分辨率我这里是1280*720但据说有些相机在30fps下最大只能设置为640*480。并开启imu运行roslaunch realsense2_camera FILENAME.launch查看相机的图像和Imu信息是否正常。其中FILENAME为你使用的配置文件名称。4.GVINS配置在GVINS运行之前,首先需要确保板子上的系统时间和实际的时间完全一致因为GVINS中GNSS信息的时间是从卫星读取的而相机采取到图像的时间信息是板子上的系统时间如果板子上时间与实际不一致则会导致卫星定位时间与图像时间无法匹配。推荐使用ntp相关命令或者服务对系统时间进行矫正具体可以查看Ubuntu部署NTP服务器和客户端 - lsgxeva - 博客园接下来需要修改GVINS的配置文件我的配置文件如下所示%YAML:1.0 #common parameters # imu_topic: /imu0 # image_topic: /cam1/image_raw # output_dir: ~/output/ imu_topic: /camera/imu image_topic: /camera/infra1/image_rect_raw output_dir: ~/output/ #camera calibration model_type: PINHOLE camera_name: camera image_width: 1280 image_height: 720 # mirror_parameters: # xi: 1.8476540167437447 distortion_parameters: k1: 0.0 k2: 0.0 p1: 0.0 p2: 0.0 projection_parameters: fx: 653.905 fy: 653.905 cx: 643.992 cy: 362.784 gnss_enable: 1 gnss_meas_topic: /ublox_driver/range_meas # GNSS raw measurement topic gnss_ephem_topic: /ublox_driver/ephem # GPS, Galileo, BeiDou ephemeris gnss_glo_ephem_topic: /ublox_driver/glo_ephem # GLONASS ephemeris gnss_iono_params_topic: /ublox_driver/iono_params # GNSS broadcast ionospheric parameters gnss_tp_info_topic: /ublox_driver/time_pulse_info # PPS time info gnss_elevation_thres: 30 # satellite elevation threshold (degree) gnss_psr_std_thres: 2.0 # pseudo-range std threshold gnss_dopp_std_thres: 2.0 # doppler std threshold gnss_track_num_thres: 1 # number of satellite tracking epochs before entering estimator 20 gnss_ddt_sigma: 0.1 gnss_local_online_sync: 0 # if perform online synchronization betwen GNSS and local time local_trigger_info_topic: /external_trigger # external trigger info of the local sensor, if gnss_local_online_sync is 1 gnss_local_time_diff: 18.0 # difference between GNSS and local time (s), if gnss_local_online_sync is 0 gnss_iono_default_parameters: !!opencv-matrix rows: 1 cols: 8 dt: d data: [0.1118E-07, 0.2235E-07, -0.4172E-06, 0.6557E-06, 0.1249E06, -0.4424E06, 0.1507E07, -0.2621E06] # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, dont change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. # 2 Dont know anything about extrinsic parameters. You dont 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.9999890386957373, -0.0043227774403168, 0.0017989117755288, 0.0043276579084841, 0.9999869417854389, -0.0027180205355500, -0.0017871388870994, 0.0027257758172719, 0.9999946881262878] #Translation from camera frame to imu frame, imu^T_cam extrinsicTranslation: !!opencv-matrix rows: 3 cols: 1 dt: d data: [-0.0759472920952561, -0.0039320527565750, -0.0016395029500217] #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 0 # 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: 1 # 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.016728779001102298 # accelerometer measurement noise standard deviation. #0.2 0.04 gyr_n: 0.0028414560642993623 # gyroscope measurement noise standard deviation. #0.05 0.004 acc_w: 0.0003856102285125257 # accelerometer bias random work noise standard deviation. #0.02 gyr_w: 1.1836900626761525e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.745 # gravity magnitude #unsynchronization parameters estimate_td: 0 # online estimate time offset between camera and imu td: 0.0 # initial value of time offset. unit: s. readed image clock td real image clock (IMU clock)其中主要需要修改的地方有三个地方。其一是图像和惯导的topic需要保证配置文件中的topic和你所使用相机和imu所发送数据的topic保持一致。其二是相机相关参数相机参数主要修改分辨率和标定参数。分辨率为红框表示部分根据你相机发送图像的实际分辨率来修改我使用的的分辨率为1280*720。标定参数为绿框部分首先是model_type和GVINS数据集为MEI表示它使用了MEI方式标定的标定参数。我这里使用了PINHOLE方式获取的标定参数所以需要将model_type的值修改为PINHOLE它主要包括畸变参数distortion_parameters和内参projection_parameters两部分如果对精度要求不是特别高而且相机没有严重畸变的话畸变参数可以全部设置为0.0。如果使用RealSense系列的相机内参可以通过realsense-viewer来获取。打开终端执行realsense-viewer会出现Realsense相机的图形界面点击More中的Calibration Data部分会出现标定参数相关的图形界面调整分辨率为你使用的分辨率便可获得对应的内参其中Focal Length对应fx和fy,Principal Point对应cx和cy。其三在线时间同步参数GVINS数据集中时间同步方式是在线同步它需要Zed-f9p接收机在topic中发布一个名为/external_trigger话题的脉冲来进行在线同步。我不会用所以将同步方式设为离线即gnss_local_online_sync改为0gnss_local_time_diff改为18.0。至此GVINS所有的相关配置都已完成。5.运行GVINS由于需要卫星信号GVINS一定需要在室外运行。运行GVINS需要执行4个指令首先是启动GVINS的指令roslaunch gvins visensor_f9p.launch其次是运行卫星定位采集程序的指令roslaunch ublox_driver ublox_driver.launch再次是运行相机的指令roslaunch realsense2_camera rs_camera.launch最后是启动rviz可视化界面的指令rviz -d ~/catkin_ws/src/GVINS/config/gvins_rviz_config.rviz如果觉得一个个执行太过麻烦可以将其写在一个.sh文件中执行./xxx.sh命令来启动。运行后等待运行稳定移动设备可以在rviz中看到gvins生成的轨迹绿色和蓝色绿色为未融合gnss的轨迹蓝色为融合gnss后的轨迹两者坐标系不一致会先生成绿色轨迹图像和卫星定位对齐后生成蓝色轨迹。