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