tof相机开发笔记
最近幫老師做tof相機(jī)的項(xiàng)目，甲方提供的代碼可以實(shí)現(xiàn)圖片數(shù)據(jù)的獲取，要求在此基礎(chǔ)上，能夠通過(guò)ros平臺(tái)實(shí)現(xiàn)slam導(dǎo)航、點(diǎn)云地圖的構(gòu)建、拍照等功能。
一、深度圖獲取
1、文件編譯
1.1編譯方式的配置
快捷鍵 ctrl + shift + B 調(diào)用編譯，選擇:catkin_make:build
可以點(diǎn)擊配置設(shè)置為默認(rèn)，修改.vscode/tasks.json 文件
{ // 有關(guān) tasks.json 格式的文檔，請(qǐng)參見(jiàn)// https://go.microsoft.com/fwlink/?LinkId=733558"version": "2.0.0","tasks": [{"label": "catkin_make:debug", //代表提示的描述性信息"type": "shell", //可以選擇shell或者process,如果是shell代碼是在shell里面運(yùn)行一個(gè)命令，如果是process代表作為一個(gè)進(jìn)程來(lái)運(yùn)行"command": "catkin_make",//這個(gè)是我們需要運(yùn)行的命令"args": [],//如果需要在命令后面加一些后綴，可以寫(xiě)在這里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”"group": {"kind":"build","isDefault":true},"presentation": {"reveal": "always"//可選always或者silence，代表是否輸出信息},"problemMatcher": "$msCompile"}] }配置好后，快捷鍵 ctrl + shift + B可快速編譯。
1.2glibc庫(kù)安裝與glibc++升級(jí)
出現(xiàn)的問(wèn)題（如圖）
?可以看到，其中的主要問(wèn)題是GLIBCXX庫(kù)和GLIBC庫(kù)版本過(guò)低。查看這兩個(gè)庫(kù)的版本。
首先明確兩個(gè)庫(kù)的位置
GLIBCXX /usr/lib/x86_64-linux-gnu/libstdc++.so.6 GLIBC /lib/x86_64-linux-gnu/libm.so.6?沒(méi)有升級(jí)前，libstdc++版本為6.0.21（對(duì)應(yīng)LIBCXX3.4.21），libsm版本為2.23（已經(jīng)升級(jí)了，沒(méi)有截圖）
?查看庫(kù)版本升級(jí)后的版本，libstdc++版本為6.0.28（對(duì)應(yīng)LIBCXX3.4.28）），這個(gè)時(shí)候包含了3.4.22，也就解決了3.4.22沒(méi)有定義的問(wèn)題。
?libstdc++庫(kù)版本的升級(jí)是通過(guò)升級(jí)gcc和g++的版本升級(jí)的。均從5.4.0升級(jí)到7.5.0的版本，安裝完成后，libstdc++也就升級(jí)了。
下面是安裝方法：
ubuntu16.04安裝gcc g++7.5.0及各個(gè)版本的切換_8BitCat的博客-CSDN博客
同理，libcm庫(kù)文件也需要升級(jí)到2.29及以上版本。
下面是升級(jí)方法
ImportError: /lib/x86_64-linux-gnu/libm.so.6: version `GLIBC_2.29‘ not found_ppipp1109的博客-CSDN博客
?安裝對(duì)應(yīng)的glibc庫(kù)
?注意：為了規(guī)范，不同的是，其中創(chuàng)建build文件夾，在build文件夾中進(jìn)行下面的操作，指定安裝目錄，其中必須要有configure文件才可以使用下面的指令，否則會(huì)出現(xiàn)無(wú)此文件的顯示。
../configure --prefix=/usr/local/glibc在執(zhí)行make -j8之前，輸入下面的指令bison是一種解析器生成器，如果不安裝，make -j8無(wú)效
sudo apt-get install bison升級(jí)后：
?還是有兩處小問(wèn)題。其中的‘__strtof128_nan@GLIBC_PRIVATE’未定義的引用，我發(fā)現(xiàn)libm.so.6中的是“__strtof128_nan@@GLIBC_PRIVATE”，而對(duì)‘glob@GLIBC_2.27’未定義的引用，我已經(jīng)向libm.so.6鏈接到2.29版本，出現(xiàn)這種情況，我也是不能理解的，計(jì)劃再次升級(jí)版本。
1.3小結(jié)
yankai@yankai-linux:/usr/lib/x86_64-linux-gnu$ ls -l libstdc++.so.6 lrwxrwxrwx 1 root root 19 6月 3 2021 libstdc++.so.6 -> libstdc++.so.6.0.28yankai@yankai-linux:/lib/x86_64-linux-gnu$ ls -l libm.so.6 lrwxrwxrwx 1 root root 33 11月 30 17:32 libm.so.6 -> /usr/local/glibc/lib/libm-2.29.sols -l? 庫(kù)文件，該指令可以查看庫(kù)文件鏈接的庫(kù)版本。
yankai@yankai-linux:/lib/x86_64-linux-gnu$ sudo ln -sf /usr/local/glibc/lib/libm-2.29.so libm.so.6 yankai@yankai-linux:/lib/x86_64-linux-gnu$ ls -l libm.so.6 lrwxrwxrwx 1 root root 33 11月 30 18:45 libm.so.6 -> /usr/local/glibc/lib/libm-2.29.so?ln -sf 需要鏈接的庫(kù)版本? 目標(biāo)庫(kù)，該指令可鏈接庫(kù)文件，用來(lái)更新庫(kù)版本。
yankai@yankai-linux:/usr/lib/x86_64-linux-gnu$ sudo find / -name "libstdc++.so.6*" [sudo] yankai 的密碼： /usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.28 /usr/lib/x86_64-linux-gnu/libstdc++.so.6 /usr/share/gdb/auto-load/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.28-gdb.py find: `/run/user/1000/doc': 權(quán)限不夠 find: `/run/user/1000/gvfs': 權(quán)限不夠 /home/yankai/qt/QtAndorid/environment/android-studio/bin/lldb/lib/libstdc++.so.6 /home/yankai/qt/QtAndorid/environment/android-sdk-linux/tools/lib/libstdc++/libstdc++.so.6 /home/yankai/qt/QtAndorid/environment/android-sdk-linux/tools/lib/libstdc++/libstdc++.so.6.0.18 /home/yankai/qt/QtAndorid/environment/android-sdk-linux/tools/lib64/libstdc++/libstdc++.so.6 /home/yankai/qt/QtAndorid/environment/android-sdk-linux/tools/lib64/libstdc++/libstdc++.so.6.0.18sudo find / -name "文件名"，可以全局搜索文件的位置，用這個(gè)搜索到庫(kù)，再進(jìn)行一些列操作。
yankai@yankai-linux:/usr/lib/x86_64-linux-gnu$ strings /usr/lib/x86_64-linux-gnu/libstdc++.so.6 | grep GLIBC GLIBCXX_3.4 GLIBCXX_3.4.1 GLIBCXX_3.4.2 GLIBCXX_3.4.3 GLIBCXX_3.4.4 GLIBCXX_3.4.5 GLIBCXX_3.4.6 GLIBCXX_3.4.7 GLIBCXX_3.4.8 GLIBCXX_3.4.9 GLIBCXX_3.4.10 GLIBCXX_3.4.11 GLIBCXX_3.4.12 GLIBCXX_3.4.13 GLIBCXX_3.4.14 GLIBCXX_3.4.15 GLIBCXX_3.4.16 GLIBCXX_3.4.17 GLIBCXX_3.4.18 GLIBCXX_3.4.19 GLIBCXX_3.4.20 GLIBCXX_3.4.21 GLIBCXX_3.4.22 GLIBCXX_3.4.23 GLIBCXX_3.4.24 GLIBCXX_3.4.25 GLIBCXX_3.4.26 GLIBCXX_3.4.27 GLIBCXX_3.4.28 GLIBC_2.2.5 GLIBC_2.3 GLIBC_2.14 GLIBC_2.6 GLIBC_2.4 GLIBC_2.18 GLIBC_2.16 GLIBC_2.3.4 GLIBC_2.17 GLIBC_2.3.2 GLIBCXX_DEBUG_MESSAGE_LENGTHstrings 文件名 | grep 想要尋找的字符，管道的使用。可以快速找到文件中的字符，這個(gè)幫助快速定位libstdc++庫(kù)中GLIBCXX的版本。
../configure --prefix=/usr/local/glibc編譯前，在build文件夾中，設(shè)置安裝路徑，這個(gè)是glibc2.29庫(kù)的安裝路徑。
ps：以上是在ubantu16.04下，并未完成編譯，在ubantu18.04下，只是將glibc升級(jí)到2.29后，就編譯成功了，下面的內(nèi)容都是在編譯成功后進(jìn)行的。
2、相機(jī)的連接
相機(jī)與電腦通過(guò)網(wǎng)線連接，通過(guò)ip進(jìn)行通信，這個(gè)時(shí)候需要注意，要想相機(jī)和電腦進(jìn)行通信，那么它們的ip必須在同一個(gè)頻段。通過(guò)軟件查閱到了相機(jī)的ip為192.168.31.3。這個(gè)時(shí)候我們要將與相機(jī)連接的網(wǎng)卡ip設(shè)置為192.168.31.1～255中的一個(gè)ip。
2.1查看網(wǎng)卡信息
指令ifconfig
?enp3s0為電腦中的網(wǎng)卡，enxf8e43b05fd67為網(wǎng)口轉(zhuǎn)usb的設(shè)備，也是一個(gè)網(wǎng)卡。
這里相機(jī)是與第二個(gè)網(wǎng)卡連接在一起的，因此需要修改第二個(gè)網(wǎng)卡的ip
修改后，可以看到inet為 192.168.31.55，此時(shí)則可以運(yùn)行程序，建立連接。
2.2小結(jié)
ip通信需要在同一個(gè)頻段下
ifconfig，查看網(wǎng)卡信息
sudo ifconfig 網(wǎng)卡名 ip，則可以修改網(wǎng)卡的指定ip
注意：拔掉網(wǎng)口后再插入，通過(guò)ifconfig可以看到，設(shè)定的ip初始化了，因此需要重新設(shè)置ip。
3、代碼解析
3.1基本思路
解析代碼的目的是獲得深度圖信息，然后再在rviz中顯示，通過(guò)rviz中Image訂閱的信息類(lèi)型為sensor_msg::Image，因此，首先查閱該信息中包含的內(nèi)容。
使用指令 rosmsg info sensor_msg/Isensor_msgs::Image。
?基本思路：現(xiàn)在就是要在代碼中發(fā)布獲取這個(gè)msg的所需要的信息，并將其通過(guò)一個(gè)topic發(fā)布出來(lái)，并由rivz訂閱顯示深度圖。
?3.2修改代碼
由于對(duì)該相機(jī)的流程并不熟悉，因此再它所給的example中，對(duì)代碼進(jìn)行修改，其中主要是代碼的添加。
example中，給了相機(jī)的整個(gè)流程框架，這個(gè)先不深究。
代碼主要是在StreamCB回調(diào)函數(shù)進(jìn)行修改的，該函數(shù)在每獲取一幀圖像時(shí)就會(huì)被調(diào)用一次，因此在這個(gè)函數(shù)中可以實(shí)時(shí)獲取信息、發(fā)布topic。
void StreamCB(void *handle, MemSinkCfg *cfg, XdynFrame_t *data) {UserHandler *user = (UserHandler *)handle;static ros::NodeHandle n;static ros::Publisher publisher = n.advertise<sensor_msgs::Image>("tof_info", 100);static sensor_msgs::Image tof_info;// wo can do some user handler// warnning : 這個(gè)回調(diào)函數(shù)內(nèi)部不適合做耗時(shí)很大的操作，請(qǐng)注意printf("get stream data: ");//檢查圖像類(lèi)型for(int i = 0; i < MEM_AGENT_SINK_MAX; i ++){printf("[%d:%d] ", i, cfg->isUsed[i]);}printf("\n");for(int i = 0; i < MEM_AGENT_SINK_MAX; i ++){if(cfg->isUsed[i] && data[i].addr){if(i == MEM_AGENT_SINK_DEPTH){SaveImageData_depth(data[i].addr, data[i].size, 100);if(data[i].ex){XdynDepthFrameInfo_t *depthInfo = (XdynDepthFrameInfo_t *)data[i].ex;//信息發(fā)布tof_info.header.seq = 1;tof_info.header.stamp = ros::Time::now();tof_info.header.frame_id = "tof_info";//圖片長(zhǎng)、寬、類(lèi)型、數(shù)據(jù)儲(chǔ)存方式、每行字節(jié)長(zhǎng)度tof_info.width = depthInfo->frameInfo.width;tof_info.height = depthInfo->frameInfo.height;tof_info.encoding = "mono16";//圖片類(lèi)型為16UC1tof_info.is_bigendian = 0;//圖片是否是大端儲(chǔ)存方式，0-不是tof_info.step = 2 *depthInfo->frameInfo.width;//圖片的步長(zhǎng)float funitofdepth = depthInfo->fUnitOfDepth;uint16_t *depth = (uint16_t *)data[i].addr;//數(shù)據(jù)類(lèi)型強(qiáng)制轉(zhuǎn)換//深度信息的獲取for(int j = 0; j < data[i].size / 2; j++){uint16_t depth_ = depth[j] * funitofdepth;//將16位的數(shù)據(jù)換成8位的進(jìn)行儲(chǔ)存tof_info.data.push_back(depth_);tof_info.data.push_back(depth_ >> 8);//注意數(shù)組與容器的區(qū)別// tof_info.data[j * 2] = depth_;// tof_info.data[j * 2 + 1] = depth_ >> 8;}publisher.publish(tof_info);printf("data_size %d: %d funitofdepth %f \n", i, tof_info.data.size(), funitofdepth);tof_info.data.clear();// printf("depth image : height = %d, width = %d, format = %d, index = %d, rs_tx = %d, tx_us = %d, temp01 = %d \n",// depthInfo->frameInfo.height, depthInfo->frameInfo.width, depthInfo->frameInfo.format,// depthInfo->frameInfo.rx_ts, depthInfo->frameInfo.tx_us, depthInfo->frameInfo.temp0);// if(!user->pcConver.IsInit()){// MemSinkInfo info;// user->stream->GetResolution(info);// user->pcConver.Init(info, depthInfo->fUnitOfDepth, depthInfo->fLensParas);// user->imgSize = info.width * info.height;// user->pcAttr = (PCPoint_t *)calloc(1, info.width * info.height * sizeof(PCPoint_t));// }// // 將深度轉(zhuǎn)化為點(diǎn)云user->pcConver.Depth2PC((uint16_t *)data[i].addr, user->pcAttr);// // 保存點(diǎn)云// SaveImageData_pointcloud(user->pcAttr, user->imgSize * sizeof(PCPoint_t));// printf("get depth size [%d], fUnitOfDepth = %f, hz[%f, %f]\n", // data[i].size, depthInfo->fUnitOfDepth, depthInfo->fModFreqMHZ[0], depthInfo->fModFreqMHZ[1]);}}else if(i == MEM_AGENT_SINK_POINT_CLOUD){SaveImageData_pointcloud(data[i].addr, data[i].size);//SavePointCloud_ply(memData[i].addr, memData[i].size);printf("get point cloud size [%d]\n", data[i].size);}else if(i == MEM_AGENT_SINK_GRAY){printf("get gray size [%d]\n", data[i].size);SaveImageData_gray(data[i].addr, data[i].size);}else if(i == MEM_AGENT_SINK_CONFID){printf("get confid size [%d]\n", data[i].size);SaveImageData_amp(data[i].addr, data[i].size);}}} }?這個(gè)是該函數(shù)的所有內(nèi)容，我在其中添加的就是信息的發(fā)布這一段。
其中注意數(shù)據(jù)類(lèi)型的強(qiáng)制轉(zhuǎn)換、16位數(shù)據(jù)如何以8位的形式儲(chǔ)存、大段與小段字節(jié)數(shù)據(jù)、數(shù)組與容器之間的區(qū)別。
數(shù)據(jù)類(lèi)型的強(qiáng)制轉(zhuǎn)換
16位數(shù)據(jù)如何以8位的形式儲(chǔ)存
uint8_t，uint16_t，uint64_t 相互轉(zhuǎn)換_Hanyang Li的博客-CSDN博客_uint8轉(zhuǎn)uint16
?大段與小段字節(jié)數(shù)據(jù)
大端與小端字節(jié)數(shù)據(jù)詳解_dosthing的博客-CSDN博客_大端數(shù)據(jù)和小端數(shù)據(jù)
數(shù)組與容器之間的區(qū)別
ch1_1 c++中的 數(shù)組array 與 容器vector_mingqian_chu的博客-CSDN博客_c++中容器中保存數(shù)組
3.3深度圖顯示
代碼修改完畢，連接到相機(jī)后，發(fā)布sensor_msgs::Image信息，啟動(dòng)rviz接收后，效果如下圖：
?二、圖片鏡像
1.灰度圖的獲取
灰度圖的獲取方式和深度圖是一樣的，由于不需要作數(shù)據(jù)類(lèi)型轉(zhuǎn)換，獲取灰度圖的代碼更加簡(jiǎn)單，依然是發(fā)布話題sensor_msgs::Image，然后在rivz中獲取信息即可得到。.
XdynFrameInfo_t *grayInfo = (XdynFrameInfo_t*)data[i].ex;tof_info_gray.header.seq = 1;tof_info_gray.header.stamp = ros::Time::now();tof_info_gray.header.frame_id = "tof_info_gray";tof_info_gray.width = 320;tof_info_gray.height =240;tof_info_gray.encoding = "mono16";tof_info_gray.is_bigendian = 0;tof_info_gray.step = 2 * 320;uint8_t *gray = (uint8_t *)data[i].addr;// printf("gray_width = %d, gray_height = %d\n",grayInfo->width, grayInfo->height);uint16_t *gray_ = (uint16_t *)data[i].addr;for(int j = 0; j < data[i].size; j++){tof_info_gray.data.push_back(gray[j]);}pub_gray.publish(tof_info_gray);printf("get gray size [%d]\n", tof_info_gray.data.size());tof_info_gray.data.clear();2.圖片鏡像
在獲取到深度圖以及灰度圖后，發(fā)現(xiàn)獲取的圖像存在鏡像問(wèn)題，只有將圖片鏡像后才能獲取正常的圖片。
2.1自己寫(xiě)代碼
自己花了一上午寫(xiě)了代碼，主要思路是將每一行的像素反過(guò)來(lái)，這樣就可以實(shí)現(xiàn)鏡像，但是事實(shí)證明我想的太簡(jiǎn)單了，根本就不是想要的效果。
2.2使用opencv鏡像
考慮到opencv中可以使用鏡像，因此使用opencv來(lái)進(jìn)行鏡像
2.2.1安裝opencv
參考以下博客安裝
ubuntu18.04安裝opencv4.5.4_趙fefe的博客-CSDN博客_ubuntu安裝opencv4.5
2.2.2配置opencv庫(kù)
在CMakeLists.txt文件中添加以下代碼,尋找opencv庫(kù)，并鏈接opencv庫(kù)
#尋找opencv頭文件 find_package(OpenCV REQUIRED) include_directories(include ${OpenCV_INCLUDE_DIRS}) ... target_link_libraries(xdyn_streamer_example${catkin_LIBRARIES}libxdyn_streamer.so${OpenCV_LIBRARIES} ) target_link_libraries(imgMir${catkin_LIBRARIES}libxdyn_streamer.so${OpenCV_LIBRARIES} )?2.2.3鏡像代碼
#include "ros/ros.h" #include "sensor_msgs/Image.h" #include "sensor_msgs/PointCloud2.h" #include "opencv2/opencv.hpp" #include "cv_bridge/cv_bridge.h" #include "cv_bridge/rgb_colors.h" #include "pcl_conversions/pcl_conversions.h" #include "pcl/io/pcd_io.h" #include "pcl/point_types.h" #include "pcl/visualization/cloud_viewer.h" #include "pcl/common/transforms.h" #include "pcl/visualization/cloud_viewer.h"using namespace cv;void operInfo_gray(const sensor_msgs::Image::ConstPtr& tof_info_gray){ROS_INFO("灰度圖的長(zhǎng)度為%d, 寬度為%d \n", tof_info_gray->height, tof_info_gray->width );static ros::NodeHandle n;static ros::Publisher publisher = n.advertise<sensor_msgs::Image>("gray_image", 100);static sensor_msgs::Image image;cv_bridge::CvImagePtr gray_pty;gray_pty = cv_bridge::toCvCopy(tof_info_gray, sensor_msgs::image_encodings::TYPE_16UC1);cv::Mat gray_img = gray_pty->image;printf("img.row:%d, img.col:%d", gray_img.rows, gray_img.cols);flip(gray_img, gray_img, 1);//沿著y軸對(duì)稱(chēng)sensor_msgs::ImagePtr msg = cv_bridge::CvImage(tof_info_gray->header, "mono16", gray_img).toImageMsg();publisher.publish(msg);}void operInfo_depth(const sensor_msgs::Image::ConstPtr& tof_info_depth){ROS_INFO("深度圖的長(zhǎng)度為%d, 寬度為%d \n", tof_info_depth->height, tof_info_depth->width );static ros::NodeHandle n;static ros::Publisher publisher = n.advertise<sensor_msgs::Image>("depth_image", 100);static sensor_msgs::Image image;cv_bridge::CvImagePtr depth_pty;depth_pty = cv_bridge::toCvCopy(tof_info_depth, sensor_msgs::image_encodings::TYPE_16UC1);cv::Mat depth_img = depth_pty->image;printf("img.row:%d, img.col:%d", depth_img.rows, depth_img.cols);flip(depth_img, depth_img, 1);//沿著y軸對(duì)稱(chēng)sensor_msgs::ImagePtr msg = cv_bridge::CvImage(tof_info_depth->header, "mono16", depth_img).toImageMsg();publisher.publish(msg);} void operInfo_pcd(const sensor_msgs::PointCloud2::ConstPtr& tof_info_pcd) {static ros::NodeHandle n;static ros::Publisher publisher = n.advertise<sensor_msgs::PointCloud2>("pcd_image", 100);pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZI> );sensor_msgs::PointCloud2 pointCloud;pcl::fromROSMsg(*tof_info_pcd, *cloud);float A = 0, B = 1, C = 1, D = 0;float e = sqrt(A * A + B * B + C * C);//為了后續(xù)將平面法向量轉(zhuǎn)化為單位向量float a = A / e, b = B / e, c = C / e;float r = D / e;Eigen::Matrix4f mirMatrix;mirMatrix << 1 - 2 * a*a, -2 * a*b, -2 * a*c, -2 * a*r,-2 * a*b, 1 - 2 * b*b, -2 * b*c, -2 * b*r,-2 * a*c, -2 * b*c, 1 - 2 * c*c, -2 * c*r,0, 0, 0, 1;// float theta = M_PI;// Eigen::Affine3f transform = Eigen::Affine3f::Identity();// transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitY()));pcl::transformPointCloud(*cloud, *transformed_cloud, mirMatrix);pcl::toROSMsg(*transformed_cloud, pointCloud);// static pcl::visualization::CloudViewer viewer("123");//直接創(chuàng)造一個(gè)顯示窗口// viewer.showCloud(transformed_cloud);//窗口顯示點(diǎn)云publisher.publish(pointCloud);}int main(int argc, char *argv[]) {setlocale(LC_ALL, "");ros::init(argc, argv, "ros2OpneCV");ros::NodeHandle nh;ros::Subscriber sub_gray = nh.subscribe<sensor_msgs::Image>("tof_info_gray", 10, operInfo_gray);ros::Subscriber sub_depth = nh.subscribe<sensor_msgs::Image>("tof_info_depth", 10, operInfo_depth);ros::Subscriber sub_pcd = nh.subscribe<sensor_msgs::PointCloud2>("tof_info_pcd", 10, operInfo_pcd);ros::spin();return 0; }其中回調(diào)函數(shù)operInfo_depth與operInfo_gray是分別將深度圖與灰度圖鏡像的函數(shù)。思路是首先將sensor_msgs::Image的話題信息轉(zhuǎn)換成cv類(lèi)型的，通過(guò)矩陣Mat接收?qǐng)D片，然后再進(jìn)行flip鏡像操作，操作完成后再將cv類(lèi)型的圖片轉(zhuǎn)換為sensor_msgs::Image類(lèi)型的消息發(fā)布即可。
2.2.4鏡像效果
?
?由效果圖可以看出，鏡像效果還是很不錯(cuò)的。
三、點(diǎn)云的獲取與鏡像
3.1點(diǎn)云數(shù)據(jù)的獲取
點(diǎn)云話題發(fā)布數(shù)據(jù)如下，經(jīng)過(guò)計(jì)算點(diǎn)云數(shù)據(jù)中的每個(gè)點(diǎn)的數(shù)據(jù)信息有16個(gè)字節(jié)，參考下面的博客
解析sensor_msgs::PointCloud2 ROS點(diǎn)云數(shù)據(jù)_KennethYangle的博客-CSDN博客_sensor_msgs/pointcloud2
?推測(cè)出16個(gè)字節(jié)中有xyz各占四個(gè)字節(jié)，還有四個(gè)字節(jié)是intensity，然后再參考相關(guān)資料，完善配了點(diǎn)云信息的發(fā)布。
}else if(i == MEM_AGENT_SINK_POINT_CLOUD){//發(fā)布點(diǎn)云信息tof_info_pcd.header.seq = 1;tof_info_pcd.header.stamp = ros::Time::now();tof_info_pcd.header.frame_id = "tof_info_pcd";tof_info_pcd.height = 1;tof_info_pcd.width = 320 * 240;tof_info_pcd.fields.resize(4);tof_info_pcd.fields[0].name = "x";tof_info_pcd.fields[0].offset = 0;tof_info_pcd.fields[0].datatype = 7;tof_info_pcd.fields[0].count = 1;tof_info_pcd.fields[1].name = "y";tof_info_pcd.fields[1].offset = 4;tof_info_pcd.fields[1].datatype = 7;tof_info_pcd.fields[1].count = 1;tof_info_pcd.fields[2].name = "z";tof_info_pcd.fields[2].offset = 8;tof_info_pcd.fields[2].datatype = 7;tof_info_pcd.fields[2].count = 1;tof_info_pcd.fields[3].name = "intensity";tof_info_pcd.fields[3].offset = 12;tof_info_pcd.fields[3].datatype = 7;tof_info_pcd.fields[3].count = 1;tof_info_pcd.is_bigendian = 0;tof_info_pcd.point_step = 16;tof_info_pcd.row_step = tof_info_pcd.point_step * tof_info_pcd.width;uint8_t *pcd = (uint8_t *)data[i].addr;for(int q = 0; q < data[i].size; q++){tof_info_pcd.data.push_back(pcd[q]);}tof_info_pcd.is_dense = 1;pub_pointCloud.publish(tof_info_pcd);tof_info_pcd.data.clear();// SaveImageData_pointcloud(data[i].addr, data[i].size);//SavePointCloud_ply(memData[i].addr, memData[i].size);// printf("get point cloud size [%d]\n", data[i].size);}else if(i == MEM_AGENT_SINK_GRAY){?3.2點(diǎn)云的鏡像
由于獲取的點(diǎn)云信息和上面的深度圖與灰度圖一樣，需要進(jìn)行鏡像，才可以正常顯示，通過(guò)查閱資料，決定將點(diǎn)云信息sensor_msgs::PointCloud2轉(zhuǎn)換成pcl::PointCloud信息后進(jìn)行鏡像操作，然后再轉(zhuǎn)換成sensor_msgs::PointCloud2信息發(fā)布，在rviz中顯示。
參考以下博客寫(xiě)出了點(diǎn)云的鏡像程序
PCL實(shí)現(xiàn)點(diǎn)云關(guān)于空間中任意平面對(duì)稱(chēng)_mengzhilv11的博客-CSDN博客_pcl::transformpointcloud
void operInfo_pcd(const sensor_msgs::PointCloud2::ConstPtr& tof_info_pcd) {static ros::NodeHandle n;static ros::Publisher publisher = n.advertise<sensor_msgs::PointCloud2>("pcd_image", 100);pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZI> );sensor_msgs::PointCloud2 pointCloud;pcl::fromROSMsg(*tof_info_pcd, *cloud);float A = 0, B = 1, C = 1, D = 0;float e = sqrt(A * A + B * B + C * C);//為了后續(xù)將平面法向量轉(zhuǎn)化為單位向量float a = A / e, b = B / e, c = C / e;float r = D / e;Eigen::Matrix4f mirMatrix;mirMatrix << 1 - 2 * a*a, -2 * a*b, -2 * a*c, -2 * a*r,-2 * a*b, 1 - 2 * b*b, -2 * b*c, -2 * b*r,-2 * a*c, -2 * b*c, 1 - 2 * c*c, -2 * c*r,0, 0, 0, 1;// float theta = M_PI;// Eigen::Affine3f transform = Eigen::Affine3f::Identity();// transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitY()));pcl::transformPointCloud(*cloud, *transformed_cloud, mirMatrix);pcl::toROSMsg(*transformed_cloud, pointCloud);// static pcl::visualization::CloudViewer viewer("123");//直接創(chuàng)造一個(gè)顯示窗口// viewer.showCloud(transformed_cloud);//窗口顯示點(diǎn)云publisher.publish(pointCloud);}通過(guò)更改ABCD的值來(lái)指定平面。
3.3鏡像效果
?三個(gè)圖均為鏡像后的點(diǎn)云圖、深度圖以灰度圖，效果還不錯(cuò)。
四、點(diǎn)云地圖的構(gòu)建
4.1 尋找思路
通過(guò)查閱一些資料，以及目前可以獲取的數(shù)據(jù)：深度圖，灰度圖以及點(diǎn)云圖。決定采用orb-slam2來(lái)進(jìn)行點(diǎn)云地圖的構(gòu)建。
前兩次構(gòu)建
參考的該博主的博客，代碼是在高博的基礎(chǔ)上更改的，不僅僅是為了構(gòu)建點(diǎn)云地圖，還有八叉樹(shù)地圖的構(gòu)建，第一版代碼運(yùn)行起來(lái)后，一旦運(yùn)行的數(shù)據(jù)變多，就會(huì)崩調(diào)，第二版代碼運(yùn)行起來(lái)后，構(gòu)建的點(diǎn)云有錯(cuò)位的情況，考慮到都是在高博代碼的基礎(chǔ)上修改的，因此決定直接跑高博的代碼。
?ORB-SLAM2 在線構(gòu)建稠密點(diǎn)云（一）_熊貓飛天的博客-CSDN博客_稠密點(diǎn)云
?4.2 運(yùn)行高博的代碼
4.2.1 配置環(huán)境
ubantu18.04 + ROS Melodic
安裝Eigen庫(kù)
安裝pangolin0.5庫(kù)
安裝sophus庫(kù)
opencv庫(kù)（無(wú)需安裝，ros自帶3.2.0版本）
pcl庫(kù)（無(wú)需安裝，ros自帶1.7版本）
參考博客
Ubuntu18.04安裝和配置ORB_SLAM2（非ROS、ROS環(huán)境）_混沌浮世的博客-CSDN博客
?4.2.2 代碼運(yùn)行
參考下面的一篇博客，我遇到的問(wèn)題基本一樣
高翔ORB-SLAM2稠密建圖編譯（添加實(shí)時(shí)彩色點(diǎn)云地圖+保存點(diǎn)云地圖）_m0_60355964的博客-CSDN博客_稠密點(diǎn)云彩色
?4.2.3 相機(jī)標(biāo)定
代碼運(yùn)行后，沒(méi)有了前兩次的bug，可以平穩(wěn)運(yùn)行，但是在繞房間轉(zhuǎn)了一圈后，發(fā)現(xiàn)墻面是圓的，推測(cè)是沒(méi)有考慮相機(jī)畸變參數(shù)的原因。
修改前k1、k2均為0，下面是修改后的，無(wú)切向畸變，有徑向畸變。
?
?標(biāo)定方法參考下面這篇博客，使用matlab工具箱進(jìn)行標(biāo)定。
相機(jī)標(biāo)定之使用Matlab工具箱_Robots.的博客-CSDN博客_matlab相機(jī)標(biāo)定
更新參數(shù)后，點(diǎn)云地圖恢復(fù)正常。
?如何卸載安裝完成的庫(kù)？
Linux下PCL的安裝與卸載_zzr1024的博客-CSDN博客_卸載pcl
?找到安裝路徑（通過(guò)再次安裝可以找到），再移除文件即可。
OpenCV庫(kù)共存問(wèn)題
?Ubuntu下多版本OpenCV共存和切換_W_Tortoise的博客-CSDN博客_ubuntu opencv多版本
Gtk-ERROR **: GTK+ 2.x symbols detected. Using GTK+ 2.x and GTK+ 3 in the same process is not supported
Gtk-ERROR **: GTK+ 2.x symbols detected. Using GTK+ 2.x and【qt吧】_百度貼吧
?論ORBSLAM_with_pointcloud_map段錯(cuò)誤（核心已轉(zhuǎn)儲(chǔ)）的另一種解決方法
論ORBSLAM_with_pointcloud_map段錯(cuò)誤（核心已轉(zhuǎn)儲(chǔ)）的另一種解決方法 - 代碼先鋒網(wǎng)
ORB-SLAM2啟動(dòng)
https://blog.csdn.net/xyt723916/article/details/89374201
?
orbslam2_pointcloud: /usr/include/pcl-1.8/pcl/conversions.h:252：void pcl::toPCLPointCloud2(const pcl::PointCloud<PointT>&, pcl::PCLPointCloud2&) [with PointT = pcl::PointXYZRGBA]: 假設(shè) ‘cloud.points.size () == cloud.width * cloud.height’ 失敗。
已放棄 (核心已轉(zhuǎn)儲(chǔ))
4.3 運(yùn)行基于ORB-SLAM2的稠密點(diǎn)云地圖
?4.3.1 代碼來(lái)源
ORB-SLAM 2+3 rgbd稠密地圖 （地圖可回環(huán)）_小白逆襲的博客-CSDN博客_orbslam2 dense map
感謝大佬！！！
4.3.2 配置運(yùn)行流程以及問(wèn)題
高翔ORB-SLAM2稠密建圖編譯（添加實(shí)時(shí)彩色點(diǎn)云地圖+保存點(diǎn)云地圖）_m0_60355964的博客-CSDN博客_orb_slam2 彩色
參考該博客
其中庫(kù)文件的問(wèn)題如下
?
?當(dāng)出現(xiàn)無(wú)法顯示點(diǎn)云地圖的情況時(shí)（最好是一開(kāi)始就改，免得需要重新編譯，浪費(fèi)時(shí)間）
將pointcloudmapping.h49行bool loopbusy 改為 bool loopbusy = false，然后依次重新運(yùn)行build.sh和build_ros.sh。
成功運(yùn)行！！！
?
總結(jié)
- 上一篇： 脑机接口基础学习19---救救我吧，Ev
- 下一篇： 物联网项目杂论