當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

tof相机开发笔记

發布時間：2024/1/1 编程问答 36 豆豆

生活随笔收集整理的這篇文章主要介紹了 tof相机开发笔记小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

最近幫老師做tof相機的項目，甲方提供的代碼可以實現圖片數據的獲取，要求在此基礎上，能夠通過ros平臺實現slam導航、點云地圖的構建、拍照等功能。

一、深度圖獲取

1、文件編譯

1.1編譯方式的配置

快捷鍵 ctrl + shift + B 調用編譯，選擇:catkin_make:build

可以點擊配置設置為默認，修改.vscode/tasks.json 文件

{ // 有關 tasks.json 格式的文檔，請參見// https://go.microsoft.com/fwlink/?LinkId=733558"version": "2.0.0","tasks": [{"label": "catkin_make:debug", //代表提示的描述性信息"type": "shell", //可以選擇shell或者process,如果是shell代碼是在shell里面運行一個命令，如果是process代表作為一個進程來運行"command": "catkin_make",//這個是我們需要運行的命令"args": [],//如果需要在命令后面加一些后綴，可以寫在這里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”"group": {"kind":"build","isDefault":true},"presentation": {"reveal": "always"//可選always或者silence，代表是否輸出信息},"problemMatcher": "$msCompile"}] }

配置好后，快捷鍵 ctrl + shift + B可快速編譯。

1.2glibc庫安裝與glibc++升級

出現的問題（如圖）

?可以看到，其中的主要問題是GLIBCXX庫和GLIBC庫版本過低。查看這兩個庫的版本。

首先明確兩個庫的位置

GLIBCXX /usr/lib/x86_64-linux-gnu/libstdc++.so.6 GLIBC /lib/x86_64-linux-gnu/libm.so.6

?沒有升級前，libstdc++版本為6.0.21（對應LIBCXX3.4.21），libsm版本為2.23（已經升級了，沒有截圖）

?查看庫版本升級后的版本，libstdc++版本為6.0.28（對應LIBCXX3.4.28）），這個時候包含了3.4.22，也就解決了3.4.22沒有定義的問題。

?libstdc++庫版本的升級是通過升級gcc和g++的版本升級的。均從5.4.0升級到7.5.0的版本，安裝完成后，libstdc++也就升級了。

下面是安裝方法：

ubuntu16.04安裝gcc g++7.5.0及各個版本的切換_8BitCat的博客-CSDN博客

同理，libcm庫文件也需要升級到2.29及以上版本。

下面是升級方法

ImportError: /lib/x86_64-linux-gnu/libm.so.6: version `GLIBC_2.29‘ not found_ppipp1109的博客-CSDN博客

?安裝對應的glibc庫

?注意：為了規范，不同的是，其中創建build文件夾，在build文件夾中進行下面的操作，指定安裝目錄，其中必須要有configure文件才可以使用下面的指令，否則會出現無此文件的顯示。

../configure --prefix=/usr/local/glibc

在執行make -j8之前，輸入下面的指令bison是一種解析器生成器，如果不安裝，make -j8無效

sudo apt-get install bison

升級后：

?還是有兩處小問題。其中的‘__strtof128_nan@GLIBC_PRIVATE’未定義的引用，我發現libm.so.6中的是“__strtof128_nan@@GLIBC_PRIVATE”，而對‘glob@GLIBC_2.27’未定義的引用，我已經向libm.so.6鏈接到2.29版本，出現這種情況，我也是不能理解的，計劃再次升級版本。

1.3小結

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.so

ls -l? 庫文件，該指令可以查看庫文件鏈接的庫版本。

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 需要鏈接的庫版本? 目標庫，該指令可鏈接庫文件，用來更新庫版本。

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': 權限不夠 find: `/run/user/1000/gvfs': 權限不夠 /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.18

sudo find / -name "文件名"，可以全局搜索文件的位置，用這個搜索到庫，再進行一些列操作。

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_LENGTH

strings 文件名 | grep 想要尋找的字符，管道的使用。可以快速找到文件中的字符，這個幫助快速定位libstdc++庫中GLIBCXX的版本。

../configure --prefix=/usr/local/glibc

編譯前，在build文件夾中，設置安裝路徑，這個是glibc2.29庫的安裝路徑。

ps：以上是在ubantu16.04下，并未完成編譯，在ubantu18.04下，只是將glibc升級到2.29后，就編譯成功了，下面的內容都是在編譯成功后進行的。

2、相機的連接

相機與電腦通過網線連接，通過ip進行通信，這個時候需要注意，要想相機和電腦進行通信，那么它們的ip必須在同一個頻段。通過軟件查閱到了相機的ip為192.168.31.3。這個時候我們要將與相機連接的網卡ip設置為192.168.31.1～255中的一個ip。

2.1查看網卡信息

指令ifconfig

?enp3s0為電腦中的網卡，enxf8e43b05fd67為網口轉usb的設備，也是一個網卡。

這里相機是與第二個網卡連接在一起的，因此需要修改第二個網卡的ip

修改后，可以看到inet為 192.168.31.55，此時則可以運行程序，建立連接。

2.2小結

ip通信需要在同一個頻段下

ifconfig，查看網卡信息

sudo ifconfig 網卡名 ip，則可以修改網卡的指定ip

注意：拔掉網口后再插入，通過ifconfig可以看到，設定的ip初始化了，因此需要重新設置ip。

3、代碼解析

3.1基本思路

解析代碼的目的是獲得深度圖信息，然后再在rviz中顯示，通過rviz中Image訂閱的信息類型為sensor_msg::Image，因此，首先查閱該信息中包含的內容。

使用指令 rosmsg info sensor_msg/Isensor_msgs::Image。

?基本思路：現在就是要在代碼中發布獲取這個msg的所需要的信息，并將其通過一個topic發布出來，并由rivz訂閱顯示深度圖。

?3.2修改代碼

由于對該相機的流程并不熟悉，因此再它所給的example中，對代碼進行修改，其中主要是代碼的添加。

example中，給了相機的整個流程框架，這個先不深究。

代碼主要是在StreamCB回調函數進行修改的，該函數在每獲取一幀圖像時就會被調用一次，因此在這個函數中可以實時獲取信息、發布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 : 這個回調函數內部不適合做耗時很大的操作，請注意printf("get stream data: ");//檢查圖像類型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;//信息發布tof_info.header.seq = 1;tof_info.header.stamp = ros::Time::now();tof_info.header.frame_id = "tof_info";//圖片長、寬、類型、數據儲存方式、每行字節長度tof_info.width = depthInfo->frameInfo.width;tof_info.height = depthInfo->frameInfo.height;tof_info.encoding = "mono16";//圖片類型為16UC1tof_info.is_bigendian = 0;//圖片是否是大端儲存方式，0-不是tof_info.step = 2 *depthInfo->frameInfo.width;//圖片的步長float funitofdepth = depthInfo->fUnitOfDepth;uint16_t *depth = (uint16_t *)data[i].addr;//數據類型強制轉換//深度信息的獲取for(int j = 0; j < data[i].size / 2; j++){uint16_t depth_ = depth[j] * funitofdepth;//將16位的數據換成8位的進行儲存tof_info.data.push_back(depth_);tof_info.data.push_back(depth_ >> 8);//注意數組與容器的區別// 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));// }// // 將深度轉化為點云user->pcConver.Depth2PC((uint16_t *)data[i].addr, user->pcAttr);// // 保存點云// 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);}}} }

?這個是該函數的所有內容，我在其中添加的就是信息的發布這一段。

其中注意數據類型的強制轉換、16位數據如何以8位的形式儲存、大段與小段字節數據、數組與容器之間的區別。

數據類型的強制轉換

16位數據如何以8位的形式儲存

uint8_t，uint16_t，uint64_t 相互轉換_Hanyang Li的博客-CSDN博客_uint8轉uint16

?大段與小段字節數據

大端與小端字節數據詳解_dosthing的博客-CSDN博客_大端數據和小端數據

數組與容器之間的區別

ch1_1 c++中的數組array 與容器vector_mingqian_chu的博客-CSDN博客_c++中容器中保存數組

3.3深度圖顯示

代碼修改完畢，連接到相機后，發布sensor_msgs::Image信息，啟動rviz接收后，效果如下圖：

?二、圖片鏡像

1.灰度圖的獲取

灰度圖的獲取方式和深度圖是一樣的，由于不需要作數據類型轉換，獲取灰度圖的代碼更加簡單，依然是發布話題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.圖片鏡像

在獲取到深度圖以及灰度圖后，發現獲取的圖像存在鏡像問題，只有將圖片鏡像后才能獲取正常的圖片。

2.1自己寫代碼

自己花了一上午寫了代碼，主要思路是將每一行的像素反過來，這樣就可以實現鏡像，但是事實證明我想的太簡單了，根本就不是想要的效果。

2.2使用opencv鏡像

考慮到opencv中可以使用鏡像，因此使用opencv來進行鏡像

2.2.1安裝opencv

參考以下博客安裝

ubuntu18.04安裝opencv4.5.4_趙fefe的博客-CSDN博客_ubuntu安裝opencv4.5

2.2.2配置opencv庫

在CMakeLists.txt文件中添加以下代碼,尋找opencv庫，并鏈接opencv庫

#尋找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("灰度圖的長度為%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軸對稱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("深度圖的長度為%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軸對稱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);//為了后續將平面法向量轉化為單位向量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");//直接創造一個顯示窗口// viewer.showCloud(transformed_cloud);//窗口顯示點云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; }

其中回調函數operInfo_depth與operInfo_gray是分別將深度圖與灰度圖鏡像的函數。思路是首先將sensor_msgs::Image的話題信息轉換成cv類型的，通過矩陣Mat接收圖片，然后再進行flip鏡像操作，操作完成后再將cv類型的圖片轉換為sensor_msgs::Image類型的消息發布即可。

2.2.4鏡像效果

?由效果圖可以看出，鏡像效果還是很不錯的。

三、點云的獲取與鏡像

3.1點云數據的獲取

點云話題發布數據如下，經過計算點云數據中的每個點的數據信息有16個字節，參考下面的博客

解析sensor_msgs::PointCloud2 ROS點云數據_KennethYangle的博客-CSDN博客_sensor_msgs/pointcloud2

?推測出16個字節中有xyz各占四個字節，還有四個字節是intensity，然后再參考相關資料，完善配了點云信息的發布。

}else if(i == MEM_AGENT_SINK_POINT_CLOUD){//發布點云信息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點云的鏡像

由于獲取的點云信息和上面的深度圖與灰度圖一樣，需要進行鏡像，才可以正常顯示，通過查閱資料，決定將點云信息sensor_msgs::PointCloud2轉換成pcl::PointCloud信息后進行鏡像操作，然后再轉換成sensor_msgs::PointCloud2信息發布，在rviz中顯示。

參考以下博客寫出了點云的鏡像程序

PCL實現點云關于空間中任意平面對稱_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);//為了后續將平面法向量轉化為單位向量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");//直接創造一個顯示窗口// viewer.showCloud(transformed_cloud);//窗口顯示點云publisher.publish(pointCloud);}

通過更改ABCD的值來指定平面。

3.3鏡像效果

?三個圖均為鏡像后的點云圖、深度圖以灰度圖，效果還不錯。

四、點云地圖的構建

4.1 尋找思路

通過查閱一些資料，以及目前可以獲取的數據：深度圖，灰度圖以及點云圖。決定采用orb-slam2來進行點云地圖的構建。

前兩次構建

參考的該博主的博客，代碼是在高博的基礎上更改的，不僅僅是為了構建點云地圖，還有八叉樹地圖的構建，第一版代碼運行起來后，一旦運行的數據變多，就會崩調，第二版代碼運行起來后，構建的點云有錯位的情況，考慮到都是在高博代碼的基礎上修改的，因此決定直接跑高博的代碼。

?ORB-SLAM2 在線構建稠密點云（一）_熊貓飛天的博客-CSDN博客_稠密點云

?4.2 運行高博的代碼

4.2.1 配置環境

ubantu18.04 + ROS Melodic

安裝Eigen庫

安裝pangolin0.5庫

安裝sophus庫

opencv庫（無需安裝，ros自帶3.2.0版本）

pcl庫（無需安裝，ros自帶1.7版本）

參考博客

Ubuntu18.04安裝和配置ORB_SLAM2（非ROS、ROS環境）_混沌浮世的博客-CSDN博客

?4.2.2 代碼運行

參考下面的一篇博客，我遇到的問題基本一樣

高翔ORB-SLAM2稠密建圖編譯（添加實時彩色點云地圖+保存點云地圖）_m0_60355964的博客-CSDN博客_稠密點云彩色

?4.2.3 相機標定

代碼運行后，沒有了前兩次的bug，可以平穩運行，但是在繞房間轉了一圈后，發現墻面是圓的，推測是沒有考慮相機畸變參數的原因。

修改前k1、k2均為0，下面是修改后的，無切向畸變，有徑向畸變。

?標定方法參考下面這篇博客，使用matlab工具箱進行標定。

相機標定之使用Matlab工具箱_Robots.的博客-CSDN博客_matlab相機標定

更新參數后，點云地圖恢復正常。

?如何卸載安裝完成的庫？

Linux下PCL的安裝與卸載_zzr1024的博客-CSDN博客_卸載pcl

?找到安裝路徑（通過再次安裝可以找到），再移除文件即可。

OpenCV庫共存問題

?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段錯誤（核心已轉儲）的另一種解決方法

論ORBSLAM_with_pointcloud_map段錯誤（核心已轉儲）的另一種解決方法 - 代碼先鋒網

ORB-SLAM2啟動

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]: 假設 ‘cloud.points.size () == cloud.width * cloud.height’ 失敗。
已放棄 (核心已轉儲)

4.3 運行基于ORB-SLAM2的稠密點云地圖

?4.3.1 代碼來源

ORB-SLAM 2+3 rgbd稠密地圖（地圖可回環）_小白逆襲的博客-CSDN博客_orbslam2 dense map

感謝大佬！！！

4.3.2 配置運行流程以及問題

高翔ORB-SLAM2稠密建圖編譯（添加實時彩色點云地圖+保存點云地圖）_m0_60355964的博客-CSDN博客_orb_slam2 彩色

參考該博客

其中庫文件的問題如下

?當出現無法顯示點云地圖的情況時（最好是一開始就改，免得需要重新編譯，浪費時間）

將pointcloudmapping.h49行bool loopbusy 改為 bool loopbusy = false，然后依次重新運行build.sh和build_ros.sh。

成功運行！！！

總結

以上是生活随笔為你收集整理的tof相机开发笔记的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇：脑机接口基础学习19---救救我吧，Ev
下一篇：物联网项目杂论