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

yankai@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("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";//图片类型为16UC1
                    tof_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("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("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("pcd_image", 100);
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud );
    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("tof_info_gray", 10, operInfo_gray);
    ros::Subscriber sub_depth = nh.subscribe("tof_info_depth", 10, operInfo_depth);
    ros::Subscriber sub_pcd = nh.subscribe("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("pcd_image", 100);
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud );
    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&, pcl::PCLPointCloud2&) [with PointT = pcl::PointXYZRGBA]: 假设 ‘cloud.points.size () == cloud.width * cloud.height’ 失败。
已放弃 (核心已转储)