做多傳感器融合尤其是跑FAST-LIVO2這種對數(shù)據(jù)質(zhì)量要求比較高的方案時最難受的一環(huán)往往不是算法本身而是手里的相機、IMU、激光雷達湊不齊一套能對齊時間戳的數(shù)據(jù)源。我手頭正好有一臺?？倒I(yè)相機MV系列的千兆網(wǎng)面陣相機SDK用的是MVS系統(tǒng)是Ubuntu 20.04加ROS Noetic。前后花了兩個晚上把相機接入FAST-LIVO2中間踩了不少坑尤其是MVS SDK的ROS封裝、時間戳同步、catkin_make編譯這一串單拎任何一個出來都很容易卡人。這篇就把整套流程手把手過一遍把我實際驗證過的方案、踩過的坑、以及最后錄包跑通的細節(jié)都寫清楚給準備把?？迪鄼C塞進FAST-LIVO2的朋友做個參考。1. 項目背景與整體思路拆解1.1 需求從哪來FAST-LIVO2對視覺輸入的硬性要求FAST-LIVO2本質(zhì)上是一個緊耦合的激光雷達、慣性測量單元和視覺融合里程計系統(tǒng)它依賴三個數(shù)據(jù)源來自激光雷達的點云、來自IMU的高頻加速度和角速度以及一個或兩個相機的圖像流。三個數(shù)據(jù)源里激光雷達和IMU的驅(qū)動相對成熟裝好就能出數(shù)據(jù)但相機這一塊經(jīng)常是短板。FAST-LIVO2的視覺前端用的是光流法和特征點提取它要求相機圖像具有穩(wěn)定的幀率、合理的曝光、以及盡量干凈的時間戳。換句話說相機不能一會兒10幀一會兒30幀畫面也不能因為自動曝光來回跳變否則特征跟蹤會抖得很厲害。這個要求直接淘汰了一批“直連UVC攝像頭”的簡易方案比如普通USB攝像頭它們不僅曝光不穩(wěn)定時間戳精度也差很多還帶自動白平衡和自動增益在SLAM運行中非常頭疼。工業(yè)相機就不一樣?？颠@類相機在SDK層面提供了完整的參數(shù)控制接口曝光、增益、白平衡、像素格式、幀率都可以手動鎖定而且通過千兆網(wǎng)接口傳輸數(shù)據(jù)的確定性和穩(wěn)定性比USB攝像頭高很多。這就是為什么要折騰?？迪鄼C接FAST-LIVO2的根本原因不是閑得慌而是視覺前端的質(zhì)量直接決定整個里程計能不能跑穩(wěn)。1.2 為什么不用現(xiàn)成ROS驅(qū)動而要自己寫MVS SDK封裝很多朋友第一反應(yīng)是去GitHub搜“hikrobot ros”能找到一些第三方寫的?？迪鄼CROS驅(qū)動但實測下來問題不少。第一這些驅(qū)動很多年沒人維護了接口版本跟新出的MVS SDK不兼容編譯就報錯。第二它們對相機的參數(shù)暴露不夠完整有些連曝光時間都沒法設(shè)更別提像素格式選擇。第三也是最重要的它們的時間戳處理方式很隨意很多直接用ROS時間戳的回調(diào)時間但沒有考慮SDK內(nèi)部取流的延遲導致圖像時間戳與IMU時間戳的偏差忽大忽小。與其在別人寫得不完整的驅(qū)動上打補丁不如直接用MVS SDK自己封裝一個ROS節(jié)點。?？倒俜教峁┑腗VS SDK是C/C接口文檔齊全功能完整封裝的難度并沒有想象中那么大。核心要做的事情就是把SDK的取流回調(diào)里的圖像數(shù)據(jù)轉(zhuǎn)成ROS的sensor_msgs/Image消息然后發(fā)布出去。這個過程聽起來簡單但涉及設(shè)備枚舉、參數(shù)設(shè)置、圖像格式轉(zhuǎn)換、時間戳賦值、以及編譯鏈接這一整套流程任何一個環(huán)節(jié)沒搞對都會卡住。我選擇這條路的另一個原因是可控性。自己封裝的節(jié)點所有參數(shù)都暴露在ROS的dynamic_reconfigure或者launch文件里想改分辨率、幀率、曝光都可以隨時調(diào)整不需要每次改代碼重新編譯。后續(xù)如果要換相機型號也只需要在SDK初始化部分做小改動就行。2. 環(huán)境準備與MVS SDK安裝2.1 軟硬件環(huán)境清單先說環(huán)境這部分直接影響后面的編譯和運行。我用的配置如下相機?？礛V-CA013-20GC130萬像素千兆網(wǎng)面陣相機全局曝光鏡頭8mm定焦手動光圈C接口系統(tǒng)Ubuntu 20.04.6 LTSROS版本NoeticMVS SDK版本MVS-2.1.2_x86_64_20231123新版本建議從?？倒倬W(wǎng)下載Linux版本其他依賴OpenCV 4.2.0ROS Noetic自帶、PCLFAST-LIVO2依賴、Eigen 3.3相機型號不是重點?？档腗V系列千兆網(wǎng)相機在SDK層面基本是同一套接口只是像素格式、分辨率上限不同。我這里的代碼和配置對大多數(shù)MV系列面陣相機都適用包括MV-CA系列、MV-CE系列等等。系統(tǒng)上需要先裝好ROS Noetic本體這個不多說裝好以后確認一下cv_bridge和image_transport都在。因為FAST-LIVO2本身還依賴PCL和Eigen等庫如果之前跑過別的SLAM大概率已經(jīng)裝了。沒裝的話一個命令補上sudo apt install ros-noetic-cv-bridge ros-noetic-image-transport ros-noetic-camera-calibration sudo apt install libeigen3-dev libpcl-dev2.2 安裝MVS SDK并驗證相機連通性海康官網(wǎng)下載Linux版MVS SDK下載下來是一個tar.gz壓縮包。解壓后按照官方文檔安裝核心步驟是運行里面的setup腳本tar -zxvf MVS-2.1.2_x86_64_20231123.tar.gz cd MVS-2.1.2_x86_64_20231123 sudo ./setup.sh安裝完成后SDK默認會安裝在/opt/MVS目錄。這個路徑后面編譯的時候要用到建議記一下。在/opt/MVS/bin目錄下有一個MVS.sh或MVS路徑下的啟動腳本可以打開官方的MVS客戶端軟件插上網(wǎng)線連好相機正常的話能在軟件里看到設(shè)備并實時預(yù)覽畫面。連不上的話先排查兩件事一是網(wǎng)絡(luò)接口是否配置了正確的IP網(wǎng)段?？礕igE相機默認IP一般是192.168.1.x或者通過DHCP獲取把自己電腦的網(wǎng)口IP改到同網(wǎng)段二是看防火墻有沒有擋住SDK與相機的通信Ubuntu下如果開了ufw需要放行相機通信端口。我遇到過一次設(shè)備枚舉不到的情況最后發(fā)現(xiàn)是網(wǎng)線插到了不常用的網(wǎng)口而SDK枚舉時選錯了網(wǎng)絡(luò)接口。在MVS軟件里可以手動指定網(wǎng)卡這個問題就解決了。2.3 確認相機能力邊界幀率、像素格式與觸發(fā)方式在動手寫代碼之前先用MVS客戶端軟件把相機的能力摸清楚這一步能避免后續(xù)開發(fā)中反復(fù)踩“這個參數(shù)不支持”的坑。需要關(guān)注三個點第一個是像素格式。FAST-LIVO2的視覺前端用的是灰度圖像做特征提取所以相機輸出灰度圖就行也就是Mono8格式。?？迪鄼C默認可能輸出Bayer格式的彩色圖如果相機輸出的是Mono8直接用如果相機本身是彩色相機輸出的是BayerRG8之類的格式需要經(jīng)過SDK的像素轉(zhuǎn)換接口轉(zhuǎn)成RGB8再到cv::Mat里轉(zhuǎn)成灰度。我在代碼里統(tǒng)一做了轉(zhuǎn)換不管相機的原始格式是什么最終發(fā)布出去的圖像統(tǒng)一用mono8。第二個是幀率上限。130萬像素的相機在Mono8格式下最高能到60幀左右但實際跑FAST-LIVO2不需要那么高20到30幀足夠。幀率太高反而把CPU和帶寬浪費在傳輸上而且時間戳同步的抖動也會更明顯。我在后面參數(shù)配置里鎖定了30幀實測在720P分辨率下很穩(wěn)定。第三個是觸發(fā)方式。FAST-LIVO2只需要連續(xù)采集不需要外部硬觸發(fā)和IMU或雷達同步所以相機設(shè)置為SDK內(nèi)部自由運行模式即可。如果后續(xù)你真要做硬件級別的同步比如用同一路PPS脈沖觸發(fā)射頻和相機那需要相機支持外觸發(fā)接口并且SDK里要把觸發(fā)模式改為外部觸發(fā)。但常規(guī)跑FAST-LIVO2數(shù)據(jù)集錄制階段用軟件時間戳就夠了觸發(fā)模式保持連續(xù)采集。3. 封裝MVS SDK為ROS節(jié)點3.1 節(jié)點整體設(shè)計框架封裝的核心思路是在一個ROS節(jié)點里完成三件事初始化相機并配置參數(shù)、后臺線程持續(xù)取流并發(fā)布圖像話題、接收ROS參數(shù)服務(wù)器動態(tài)配置。整體架構(gòu)不復(fù)雜但要把流程理順。節(jié)點發(fā)布的話題是/cam0/color類型為sensor_msgs/Image后續(xù)FAST-LIVO2配置里直接把視覺話題指到這個topic即可。為了調(diào)試方便我還額外發(fā)布了一個/cam0/compressed的壓縮話題用rqt_image_view實時查看畫面否則圖像數(shù)據(jù)帶寬太大會導致實時預(yù)覽卡頓。相機初始化流程分為下面幾個關(guān)鍵步驟枚舉設(shè)備獲取設(shè)備數(shù)量創(chuàng)建設(shè)備句柄選擇第一個在線設(shè)備打開設(shè)備設(shè)置像素格式、分辨率、曝光時間、幀率注冊圖像回調(diào)或使用主動取流線程開始取流節(jié)點退出時停止取流并銷毀句柄在代碼組織上我單獨寫了一個HikCamera類把SDK相關(guān)調(diào)用全部封裝在里面ROS節(jié)點的回調(diào)函數(shù)只負責把拿到的圖像數(shù)據(jù)轉(zhuǎn)成ROS消息并發(fā)布。這樣模塊劃分清晰后續(xù)如果要移植到其他相機平臺只要替換這個類就行。3.2 核心取流代碼實現(xiàn)下面這段代碼是封裝的核心部分我把SDK初始化、取流回調(diào)、圖像轉(zhuǎn)換這三個關(guān)鍵環(huán)節(jié)拆開講解。首先是初始化和取流相關(guān)代碼#include MvCameraControl.h #include ros/ros.h #include sensor_msgs/Image.h #include cv_bridge/cv_bridge.h #include opencv2/opencv.hpp class HikCamera { public: HikCamera() : handle_(nullptr), is_grabbing_(false) {} bool init(int width, int height, float fps, float exposure, int pixel_format) { // 枚舉設(shè)備 MV_CC_DEVICE_INFO_LIST device_list; memset(device_list, 0, sizeof(device_list)); int ret MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, device_list); if (ret ! MV_OK || device_list.nDeviceNum 0) { ROS_ERROR(No Hik camera found); return false; } // 創(chuàng)建設(shè)備句柄 ret MV_CC_CreateHandle(handle_, device_list.pDeviceInfo[0]); if (ret ! MV_OK) { ROS_ERROR(Create handle failed, ret 0x%x, ret); return false; } // 打開設(shè)備 ret MV_CC_OpenDevice(handle_); if (ret ! MV_OK) { ROS_ERROR(Open device failed, ret 0x%x, ret); return false; } // 設(shè)置傳輸類型、像素格式、分辨率、幀率、曝光 MV_CC_SetEnumValue(handle_, PixelFormat, pixel_format); MV_CC_SetIntValue(handle_, Width, width); MV_CC_SetIntValue(handle_, Height, height); MV_CC_SetFloatValue(handle_, FrameRate, fps); MV_CC_SetEnumValue(handle_, ExposureAuto, 0); // 關(guān)閉自動曝光 MV_CC_SetFloatValue(handle_, ExposureTime, exposure); return true; } bool start_grabbing(ros::Publisher pub) { if (handle_ nullptr) return false; user_param_ pub; is_grabbing_ true; // 注冊取流回調(diào) int ret MV_CC_RegisterImageCallBackEx(handle_, image_callback, this); if (ret ! MV_OK) { ROS_ERROR(Register callback failed, ret 0x%x, ret); return false; } ret MV_CC_StartGrabbing(handle_); return ret MV_OK; } void stop() { if (handle_) { MV_CC_StopGrabbing(handle_); MV_CC_CloseDevice(handle_); MV_CC_DestroyHandle(handle_); handle_ nullptr; } } private: static void __stdcall image_callback(unsigned char* data, MV_FRAME_OUT_INFO_EX* frame_info, void* user) { HikCamera* cam static_castHikCamera*(user); ros::Publisher* pub static_castros::Publisher*(cam-user_param_); // 轉(zhuǎn)成cv::Mat cv::Mat raw_image(frame_info-nHeight, frame_info-nWidth, CV_8UC1, data); cv::Mat gray_image; if (frame_info-enPixelType PixelType_Gvsp_Mono8) { gray_image raw_image.clone(); } else if (frame_info-enPixelType PixelType_Gvsp_BayerRG8) { cv::Mat bayer(frame_info-nHeight, frame_info-nWidth, CV_8UC1, data); cv::cvtColor(bayer, gray_image, cv::COLOR_BayerRG2GRAY); } else { cv::Mat color_image(frame_info-nHeight, frame_info-nWidth, CV_8UC3, data); cv::cvtColor(color_image, gray_image, cv::COLOR_RGB2GRAY); } sensor_msgs::ImagePtr msg cv_bridge::CvImage( std_msgs::Header(), sensor_msgs::image_encodings::MONO8, gray_image ).toImageMsg(); msg-header.stamp ros::Time::now(); msg-header.frame_id camera; pub-publish(msg); } void* handle_; bool is_grabbing_; void* user_param_; };取流接口我用的是MV_CC_RegisterImageCallBackEx也就是注冊回調(diào)方式。SDK內(nèi)部會在圖像數(shù)據(jù)到達時自動調(diào)用回調(diào)函數(shù)這在多數(shù)場景下比主動輪詢MV_CC_GetImageBuffer更高效也不容易丟幀?；卣{(diào)函數(shù)里我直接把圖像數(shù)據(jù)映射成cv::Mat再通過cv_bridge轉(zhuǎn)成ROS的Image消息發(fā)布。要注意的是MV_FRAME_OUT_INFO_EX結(jié)構(gòu)體里包含了圖像的長寬和像素格式信息每次回調(diào)都要用這個結(jié)構(gòu)體的字段來構(gòu)造cv::Mat不能事先用固定尺寸否則相機如果中途被改了分辨率程序會直接崩潰。3.3 圖像參數(shù)與話題配置把參數(shù)暴露給launch文件相機參數(shù)全部放到launch文件里管理這樣每次調(diào)整不需要重新編譯。我用的是ROS的rosparam加上dynamic_reconfigure的思路但為了簡化直接在一個launch文件里寫死參數(shù)并加載到參數(shù)服務(wù)器。下面是我用的launch文件launch node namehik_camera_node pkghik_camera_ros typehik_camera_node outputscreen param namecamera_name valuemv_camera / param nameimage_width value1280 / param nameimage_height value800 / param namefps value30 / param nameexposure_time value4000 / param namepixel_format valuemono8 / param nameframe_id valuecamera / param namecamera_topic value/cam0/color / /node /launch像素格式參數(shù)我用了字符串mono8或bayer代碼里再根據(jù)這個字符串決定設(shè)置哪個枚舉值。直接寫數(shù)字雖然簡單但可讀性太差以后自己都看不懂。曝光時間參數(shù)曝光時間我設(shè)成了4000微秒也就是4毫秒。這個值在室內(nèi)燈光環(huán)境下表現(xiàn)不錯如果搬到室外強光環(huán)境需要把這個參數(shù)調(diào)小到1000微秒左右否則畫面會過曝。FAST-LIVO2的視覺前端對過曝很敏感一旦圖像高光區(qū)域過多特征點會大量失效。建議在實際運行環(huán)境里先用rqt_image_view預(yù)覽畫面調(diào)節(jié)到畫面亮度合適的曝光值再開始跑算法。分辨率方面1280x800是兼顧分辨率和帶寬的選擇。分辨率太低特征點不夠用分辨率太高占用帶寬太大而且FAST-LIVO2內(nèi)部還會做金字塔采樣過高的分辨率并不會帶來明顯精度提升。我對比過1280x800和1920x1200兩種分辨率在同一個場景下跑FAST-LIVO2的結(jié)果軌跡精度基本沒有差別但CPU占用率差了不少。3.4 相機內(nèi)參標定不標定直接跑就是撞運氣接FAST-LIVO2之前一定要先標定相機內(nèi)參和畸變系數(shù)。這件事太重要了我一開始偷懶用了廠家出廠的內(nèi)參文件結(jié)果跑FAST-LIVO2的時候初始化階段頻繁失敗定位軌跡也一直漂后來重新標定后整個系統(tǒng)立馬穩(wěn)定下來。?？迪鄼C出廠時確實會附帶一份內(nèi)參文件但那個是在出廠試驗臺上的標定結(jié)果跟實際使用中的鏡頭安裝狀態(tài)、光圈位置都有關(guān)系。尤其是鏡頭的光圈和聚焦環(huán)如果動過內(nèi)參就會有偏差。FAST-LIVO2對相機內(nèi)參非常敏感因為視覺殘差的構(gòu)建依賴反投影模型內(nèi)參差一點視覺和激光的融合就會出現(xiàn)系統(tǒng)性偏差。標定的做法是安裝camera_calibration工具sudo apt install ros-noetic-camera-calibration然后準備一張棋盤格標定板我用的是一張A3紙打印的8x6棋盤格格子邊長35mm。啟動標定界面后手持標定板在畫面里從不同角度、不同距離、不同位置移動讓標定程序采集20到30張有效幀然后點擊Calibrate生成標定結(jié)果。這個過程大概五分鐘就能完成比想象中簡單。標定出來的內(nèi)參和畸變系數(shù)分別用于FAST-LIVO2的視覺配置文件和相機去畸變處理。我選擇在相機驅(qū)動節(jié)點中直接把圖像去畸變后再發(fā)布好處是FAST-LIVO2的代碼里就少一步處理配置也更簡單。代價是去畸變會裁剪掉圖像邊緣部分視野會略微變窄。對于FAST-LIVO2這種融合方案來說視野窄一點可以接受換來的是配置和調(diào)試的便利性。4. catkin_make編譯要點與依賴處理4.1 創(chuàng)建ROS包并配置CMakeLists.txt新建一個catkin工作空間的包假設(shè)工作空間叫catkin_ws包名是hik_camera_rosmkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_create_pkg hik_camera_ros roscpp sensor_msgs cv_bridge image_transportcatkin_create_pkg生成的CMakeLists.txt模板已經(jīng)幫你處理好了catkin包依賴的基礎(chǔ)配置但MVS SDK的頭文件和庫目錄需要手動添加。這一步是很多新手卡住的地方因為模板不會自動包含/opt/MVS下的接口。下面是我實際使用的CMakeLists.txt關(guān)鍵部分cmake_minimum_required(VERSION 3.0.2) project(hik_camera_ros) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs cv_bridge image_transport ) find_package(OpenCV REQUIRED) # MVS SDK 頭文件和庫路徑 include_directories( include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} /opt/MVS/include ) link_directories( /opt/MVS/lib/x86_64 ) add_executable(hik_camera_node src/hik_camera_node.cpp) target_link_libraries(hik_camera_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} MvCameraControl )這里最關(guān)鍵的是兩行include_directories里的/opt/MVS/include以及l(fā)ink_directories里的/opt/MVS/lib/x86_64。MVS SDK的動態(tài)庫叫l(wèi)ibMvCameraControl.so在target_link_libraries里直接寫名字即可。4.2 編譯時常見問題OpenCV版本沖突與庫路徑缺失我實際編譯中遇到的最頭大的問題就是OpenCV版本沖突。Ubuntu 20.04自帶的OpenCV是4.2.0ROS Noetic的cv_bridge也是基于4.2.0編譯的但如果之前手工編譯過OpenCV 4.5以上版本并且沒有卸掉CMake在find_package(OpenCV)的時候可能找到的是手工安裝的版本導致鏈接時出現(xiàn)一堆符號錯誤。解決辦法是在CMakeLists.txt里明確指定OpenCV的路徑set(OpenCV_DIR /usr/lib/x86_64-linux-gnu/cmake/opencv4) find_package(OpenCV REQUIRED)這樣強制CMake使用系統(tǒng)ROS自帶的OpenCV版本避開手工裝的版本。如果確實需要更高版本的OpenCV那就要連cv_bridge一起重新編譯工作量會大很多不建議在跑FAST-LIVO2的場景下折騰。另一個常見問題是MVS SDK庫路徑寫錯。MVS SDK在不同架構(gòu)下庫目錄不同x86_64系統(tǒng)是lib/x86_64ARM平臺是lib/aarch64。如果復(fù)制了別人的工程忘了改路徑會直接報找不到libMvCameraControl.so。但如果你確認路徑?jīng)]問題還是找不到可以編譯后運行前用ldd檢查一下依賴ldd devel/lib/hik_camera_ros/hik_camera_node | grep MvCamera如果顯示找不到libMvCameraControl.so需要在bashrc里加上庫路徑export LD_LIBRARY_PATH/opt/MVS/lib/x86_64:$LD_LIBRARY_PATH4.3 catkin_make編譯順序問題先編依賴還是先編相機驅(qū)動如果工作空間里同時放了FAST-LIVO2的源碼和相機驅(qū)動包注意catkin_make的編譯順序。catkin會自動根據(jù)package.xml里的依賴關(guān)系決定編譯順序但有時因為之前編譯產(chǎn)生的build緩存會出現(xiàn)循環(huán)依賴或依賴找不到的問題。我建議先只編譯相機驅(qū)動包本身cd ~/catkin_ws catkin_make --pkg hik_camera_ros編譯通過后再編譯整個工作空間這樣如果出現(xiàn)問題能清楚地知道是相機驅(qū)動的問題還是FAST-LIVO2本身的問題。FAST-LIVO2的主包依賴一些輔助庫比如livox_ros_driver、ikdTree、faster_lio等這些包的編譯順序如果亂了會非常頭疼。跑catkin_make的時候最好用并行編譯但也不要無腦加-j因為內(nèi)存不夠時編譯會崩。我一般用catkin_make -j4四線程在大多數(shù)機器上比較穩(wěn)妥如果機器內(nèi)存大、核多可以提高到-j8。我實測在編譯過程中由于MVS SDK的頭文件比較大預(yù)處理器會吃掉不少內(nèi)存并行數(shù)過高時出現(xiàn)過因為內(nèi)存不足導致編譯進程被殺的情況。5. 時間戳同步從數(shù)據(jù)源到FAST-LIVO2的精度要求5.1 為什么時間戳是大事三個傳感器各自亂報時間會怎樣FAST-LIVO2在融合激光雷達、IMU和相機數(shù)據(jù)時核心假設(shè)是這三個傳感器的時間戳描述的是物理世界中同一時刻的狀態(tài)。如果時間戳對不齊相當于把三個不同時刻的快照硬當成同一時刻來求解結(jié)果必然發(fā)散。FAST-LIVO2內(nèi)部有嚴格的傳感器時間對齊邏輯IMU消息和激光雷達消息根據(jù)它們帶的時間戳插值對齊相機圖像則用來提供視覺殘差。如果圖像的時間戳比真實采集時刻偏大或偏小幾十毫秒視覺殘差會對應(yīng)到錯誤的IMU狀態(tài)上造成定位誤差累積嚴重的時候直接初始化失敗。實際經(jīng)驗是對于FAST-LIVO2這種方案圖像時間戳誤差在5毫秒以內(nèi)一般能正常工作超過10毫秒系統(tǒng)性能明顯下降超過30毫秒基本跑不起來。所以時間戳同步不是錦上添花是剛需。5.2 打時間戳的兩種方式對比回調(diào)時刻還是SDK時間戳在封裝相機驅(qū)動時給圖像消息打時間戳有兩種做法我在開發(fā)中都試過效果差別很大。第一種方式是在SDK圖像回調(diào)函數(shù)里直接調(diào)用ros::Time::now()。這種方式簡單粗暴代碼就一行。缺點是回調(diào)函數(shù)被調(diào)用的時刻跟相機真正曝光完成的時刻之間有一段延遲。延遲來自兩個地方一是圖像數(shù)據(jù)從相機傳到主機需要時間千兆網(wǎng)傳輸一小幀圖像大概1到2毫秒二是SDK內(nèi)部對圖像數(shù)據(jù)的處理和回調(diào)觸發(fā)也需要時間這個時間不定可能1毫秒也可能5毫秒。所以回調(diào)時刻打出來的時間戳比真實曝光完成時刻普遍偏大幾毫秒。第二種方式是利用SDK的幀信息。在MV_FRAME_OUT_INFO_EX結(jié)構(gòu)體里有兩個時間戳字段一個是設(shè)備內(nèi)的時間戳另一個是主機收到幀時的系統(tǒng)時間戳。?？档腉igE相機支持IEEE 1588協(xié)議如果設(shè)備和主機之間實現(xiàn)了1588時鐘同步SDK返回的設(shè)備時間戳可以轉(zhuǎn)換為主機系統(tǒng)時間精度很高。但問題是很多網(wǎng)卡和相機的1588實現(xiàn)并不完整配置起來折騰而且對普通使用場景來說有點大材小用。我實際采用的策略是補償式的回調(diào)時間戳。首先在相機參數(shù)里把曝光時間設(shè)置成固定值比如4毫秒然后通過SDK提供的stFrameInfo.hostTimeStamp字段記錄主機收到數(shù)據(jù)的時刻再根據(jù)曝光時間估算圖像曝光中點與回調(diào)時刻之間的延遲最后在ros::Time::now()的基礎(chǔ)上減去這個延遲。雖然這個補償值不可能絕對精確但能把時間戳誤差控制在幾毫秒內(nèi)對FAST-LIVO2來說已經(jīng)足夠。專門說一下為什么不要用設(shè)備內(nèi)的時間戳直接做映射。設(shè)備內(nèi)時間戳對應(yīng)的是曝光開始的時刻但它的參考時鐘是相機自身的晶振與主機的系統(tǒng)時鐘有偏差。除非做IEEE 1588級別的時鐘同步否則這個偏差是不確定的直接拿來用反而更糟。我在實驗里看到過設(shè)備時間戳與主機時間相差幾十毫秒的情況如果直接用肯定掛。5.3 時間戳補償?shù)木唧w實現(xiàn)代碼層面我修改了回調(diào)函數(shù)里的時間戳賦值邏輯auto host_time ros::Time::now(); // 補償曝光中點與回調(diào)觸發(fā)時刻之間的延遲 // 取流回調(diào)晚于曝光完成大約延遲 transmission_delay sdk_callback_delay // 曝光中點約在曝光開始后 exposure_time/2 微秒曝光完成后還有傳輸和回調(diào)延遲 double exposure_seconds exposure_time_us_ / 1e6; double total_delay exposure_seconds / 2.0 kTransmissionAndCallbackDelay; msg-header.stamp host_time - ros::Duration(total_delay);其中kTransmissionAndCallbackDelay是我根據(jù)實測標定的一個常數(shù)用的是相機朝向一個高頻LED閃爍光源同時用示波器測量真實曝光時刻和ROS消息時間差的方法實際測量出來這個值大概在3到6毫秒之間我取了4.5毫秒。這個值跟相機分辨率、網(wǎng)絡(luò)環(huán)境有關(guān)系如果你換了環(huán)境或者在戶外跑建議重新測一下。不重測也沒關(guān)系只要FAST-LIVO2能初始化并穩(wěn)定運行偏差允許范圍之內(nèi)就不會有大問題。5.4 錄制數(shù)據(jù)包與驗證時間戳對齊效果相機驅(qū)動寫好后需要用數(shù)據(jù)包方式驗證時間戳對齊效果。把三個傳感器一起錄制然后分析bag里的話題時間分布。錄制命令rosbag record /cam0/color /livox/lidar /imu/data錄完后回放數(shù)據(jù)包用以下方式檢查對齊效果。首先用rostopic hz檢查各話題的實際發(fā)布頻率rostopic hz /cam0/color rostopic hz /imu/data rostopic hz /livox/lidar只要頻率基本穩(wěn)定說明數(shù)據(jù)流本身沒問題。然后看時間戳的連續(xù)性我習慣把bag的話題時間戳導出來做個簡單的可視化。rostopic echo太啰嗦直接用Python腳本處理bag打印每個話題的時間間隔import rosbag bag rosbag.Bag(test.bag) for topic, msg, t in bag.read_messages(topics[/cam0/color]): print(msg.header.stamp.to_sec())相鄰圖像幀的時間戳間隔如果基本等于幀率的倒數(shù)比如30幀對應(yīng)約33.3毫秒說明時間戳連續(xù)穩(wěn)定。如果出現(xiàn)忽大忽小的跳動就要檢查是不是相機幀率本身不穩(wěn)或者回調(diào)里時間戳打的時機有問題。還有一個重要驗證在rqt里同時顯示IMU數(shù)據(jù)頻率曲線和圖像頻率曲線看它們在時間軸上是否有錯位感。雖然這個驗證方式比較主觀但能在錄包階段提前發(fā)現(xiàn)明顯的問題避免后面跑FAST-LIVO2時才炸。6. 常見問題與排查技巧實錄6.1 編譯期問題速查編譯期問題主要集中在MVS SDK和ROS環(huán)境的銜接上我整理了一個速查表問題現(xiàn)象可能原因解決辦法找不到MvCameraControl.hinclude路徑未添加在CMakeLists.txt中添加/opt/MVS/include找不到libMvCameraControl.so鏈接路徑未指定添加/opt/MVS/lib/x86_64并target_link_libraries加MvCameraControlOpenCV符號沖突手工安裝了一版OpenCV在CMakeLists.txt指定OpenCV_DIR為系統(tǒng)路徑cv_bridge編譯錯誤ROS版本與代碼不匹配確認是Noetic不要嘗試在Melodic上直接編catkin_make過程中內(nèi)存被殺并行編譯數(shù)過高降低-j參數(shù)我使用-j46.2 運行期問題速查運行期問題更多一個個說。第一個是設(shè)備枚舉不到。除了前面說的網(wǎng)卡和IP問題還要注意MVS SDK的權(quán)限設(shè)置。SDK安裝后會創(chuàng)建一個用戶組要把當前用戶加入這個組否則在非root用戶下調(diào)用設(shè)備枚舉接口可能失敗。運行命令sudo usermod -a -G mvs $USER這里mvs是SDK安裝時創(chuàng)建的用戶組名不同版本可能不一樣。加完后要重新登錄或者執(zhí)行newgrp mvs。第二個問題是啟動節(jié)點后取流成功但rqt_image_view畫面是花的。這個大概率是像素格式設(shè)置和cv_bridge編碼不一致造成的。我遇到過BayerRG8的圖像在OpenCV里直接用CV_8UC3來解析結(jié)果畫面顏色完全亂掉。解決方法是根據(jù)SDK返回的幀信息里的enPixelType字段來做轉(zhuǎn)換不要盲目假設(shè)輸入格式。代碼里我用了PixelType_Gvsp_Mono8、PixelType_Gvsp_BayerRG8等幾個分支來分別處理這樣無論相機輸出什么格式都不會花屏。第三個問題是幀率不達標。相機標稱30幀實際跑只有15幀。排查思路是先用官方MVS客戶端軟件試試同樣參數(shù)下能不能到30幀如果能說明網(wǎng)絡(luò)沒問題問題出在驅(qū)動代碼上可能是我在回調(diào)里做了太多圖像處理拖慢了取流線程。解決辦法是把圖像格式轉(zhuǎn)換和發(fā)布邏輯放到一個獨立線程取流回調(diào)里只做拷貝操作用隊列傳遞圖像數(shù)據(jù)。我這里為了簡單沒有加線程池但實際測試下來在回調(diào)里做cv::cvtColor和clone操作不會導致丟幀所以沒做進一步優(yōu)化。如果你的機器性能比較弱建議把格式轉(zhuǎn)換放到獨立線程。6.3 關(guān)于熱搜詞“?？稻€掃相機能不能輸入行程”的提醒搜到這個話題說明有不少人把線掃相機和FAST-LIVO2聯(lián)想在一起。這里直接說結(jié)論?？稻€掃相機不能直接作為FAST-LIVO2的視覺輸入。線掃相機的工作原理是每次只采集一行像素要得到完整的二維圖像必須依賴被測物體做相對的勻速運動或者配合外部編碼器觸發(fā)逐行采樣再在后期拼接成完整圖。它天生是為連續(xù)運動的產(chǎn)線檢測場景設(shè)計的而不是像面陣相機那樣一幀一幀地曝光出完整的二維畫面。FAST-LIVO2的視覺前端拿到的是sensor_msgs/Image類型的一幀圖像本質(zhì)上是一張二維矩陣線掃相機拿不出這個格式的數(shù)據(jù)。如果項目已經(jīng)買了線掃相機想用來跑FAST-LIVO2那基本沒戲。要么換面陣相機要么用額外的運動平臺和編碼器數(shù)據(jù)把線掃輸出重建為二維圖像但那樣的話數(shù)據(jù)率、曝光方式、圖像畸變模型全都變了已經(jīng)不是FAST-LIVO2能干的事。選型階段就老老實實選全局曝光的面陣工業(yè)相機這個方向才是對的。7. 實操經(jīng)驗與最后的幾點建議整套流程走下來我的感受是?？迪鄼C接FAST-LIVO2這件事技術(shù)難度并不高但細節(jié)非常多每一個環(huán)節(jié)都有那么兩三個坑等著你。最花時間的往往不是寫代碼本身而是排查那些看起來毫無頭緒的問題比如網(wǎng)卡沒選對、SDK庫路徑漏了、OpenCV版本沖突、時間戳偏差超限。如果讓我重新做一次我會在動手前先把所有環(huán)境細節(jié)確認清楚網(wǎng)口單獨一個、IP網(wǎng)段固定、MVS SDK版本統(tǒng)一、OpenCV路徑明確、相機參數(shù)先用官方軟件調(diào)好。這些準備工作做足了后面的開發(fā)會順暢很多。最后分享一個我個人驗證過的經(jīng)驗錄數(shù)據(jù)集的時候先花幾分鐘用rqt看一圈圖像和IMU數(shù)據(jù)確認曝光正常、幀率穩(wěn)定、時間戳分布均勻再正式開始錄。返工的成本遠遠大于多花的這幾分鐘。還有就是建議在相機驅(qū)動節(jié)點里把圖像話題同時發(fā)布raw和compressed兩個版本raw給FAST-LIVO2用compressed給自己調(diào)試用。即使不在線預(yù)覽跑算法的時候用compressed話題來看畫面能大幅降低CPU占用也能避免搶帶寬影響相機數(shù)據(jù)流。這個小習慣幫我省了很多麻煩算是這套流程里最實用的一個技巧了。