代码，cmake，launch等都在网盘链接中

链接: https://pan.baidu.com/s/14hWBDnRZC41xkqAk6FirtA 提取码: 1een
--来自百度网盘超级会员v4的分享

1、简介

3D激光雷达和相机的融合指的是将激光雷达获得的3D点，投影到相机图像上，从而给图像每个像素点添加深度信息或者为雷达获取到的3D点添加RGB信息。效果如图：

可以知道，要将3D点投影到图像需要知道相机和雷达的坐标系的变换矩阵以及相机的内参

2、雷达相机融合理论介绍

理论简单来说就算一句话：通过雷达相机的变换矩阵将雷达系/世界坐标系的3D点投影到相机

2.1、Airsim中各个矩阵获取

外参

外参包括旋转和平移，将世界/雷达坐标系下的3D点转换到相机坐标系下

此处主要参考

AirSim相机、IMU内外参分析(VIO、vSLAM)_智能之欣的博客-CSDN博客_airsim 相机

该旋转矩阵是相机到世界坐标系的，我们要的是世界坐标系到相机坐标系的。所有应该求该矩阵的转置，如下：

 平移矩阵可以根据settings中相机处的x,y,z给出

内参

仍参考上文博客

 其中width，heigth，fov都可以在settings中进行设置

雷达系/世界系
点云返回是雷达系还是世界系可以通过在settings中雷达参数进行设置

本文中用的是雷达系，但是不知道雷达与世界坐标系的关系，经过尝试雷达系与世界坐标系各个坐标轴朝向应该算一致的，仅仅是根据结果猜测，不确定是否正确

现在内参外参都知道了，下面可以根据录制数据进行测试了

3、Airsim录制bag数据

录制点云数据的过程可以参考我的另一篇文章，除了settings文件有所不同其他都是相同的

Airsim+Lego-Loam_111111111112454545的博客-CSDN博客

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "ViewMode": "SpringArmChase",
  "ClockSpeed": 1,
  "Vehicles": {
    "imulink": {
      "VehicleType": "SimpleFlight",
      "DefaultVehicleState": "Armed",
      "EnableCollisionPassthrogh": false,
      "EnableCollisions": true,
      "AllowAPIAlways": true,
      "Sensors": {
        "Imu" : {
          "SensorType": 2,
          "Enabled": true,
          "AngularRandomWalk": 0.3,
          "GyroBiasStabilityTau": 500,
          "GyroBiasStability": 4.6,
          "VelocityRandomWalk": 0.24,
          "AccelBiasStabilityTau": 800,
          "AccelBiasStability": 36
        },        
        "LidarCustom": {
          "SensorType": 6,
          "Range": 50,
          "Enabled": true,
          "NumberOfChannels": 16,
          "PointsPerSecond": 288000,
          "RotationsPerSecond": 10,
          "VerticalFOVUpper": 15,
          "VerticalFOVLower": -15,
          "HorizontalFOVStart": -180,
          "HorizontalFOVEnd": 180,
          "X": 2, "Y": 0, "Z": -1,
          "DrawDebugPoints": false,
          "Pitch":0, "Roll": 0, "Yaw": 0,
          "DataFrame": "SensorLocalFrame"
        }
      },
	"Cameras": {
	"camera_1": {
          "CaptureSettings": [
            {
              "ImageType": 0,
              "Width": 640,
              "Height": 480,
              "FOV_Degrees": 90,
              "TargetGamma": 1.5
            }
          ],
          "X": 2, "Y": 0, "Z": -1,
          "Pitch": 0, "Roll": 0, "Yaw": 0
        }
	}
    }
  },
 
	"SubWindows": [
	   
	  ]
}

3.1、控制飞机飞行

3.1.1、代码控制

飞机飞行路线可以通过代码设置飞行路线参见我的另一篇博客(来自于宁子安大佬)；

3.1.2、键盘控制

AirSim无人机键盘控制_Sabersaber-的博客-CSDN博客_python控制airsim

也可以通过pygame用键盘控制无人机更加方便，但是有一点需要注意:UE4那边的设置，你需要取消勾选非焦点窗口时占用较少计算资源的选项否则当你鼠标出了UE4窗口到键盘控制那个窗口时UE4会变得特别卡；

在编辑->偏好设置->性能->处于背景使用较少CPU

4、代码实现

代码主要包括以下几大部分内容

4.1、构造函数

pcl_fusion():it(n)
    {
        subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, &pcl_fusion::getcloud, this); //接收velodyne点云数据，进入回调函数getcloud()
        pubCloud = n.advertise<sensor_msgs::PointCloud2>("/fusion_cloud", 100);  //建立了一个发布器，主题是/adjustd_cloud，方便之后发布加入颜色之后的点云
        image_sub_ = it.subscribe("/airsim_node/imulink/camera_1/Scene", 100, &pcl_fusion::imageCallback,this);
        //image_transport::Publisher img_pub = it.advertise("camera/image", 100);
        R<<0,1,0,
        0,0,1,
        1,0,0;
        K<<320,0,320,
                0,320,240,
                0,0,1;
        t<<0,0,0;
    }

1、初始化订阅点云的变量

2、订阅订阅图像的变量

3、初始化发布融合RGB信息的变量

4、初始化内外参

4.2、订阅图像函数

  void imageCallback(const sensor_msgs::ImageConstPtr& msg){
  try{
        image  = cv_bridge::toCvShare(msg, "bgr8")->image;
        //cv::Mat image  = cv_bridge::toCvShare(msg, "bgr8")->image; //image_raw就是我们得到的图像了
 
    // cv::circle(image,cv::Point(100,250),5,cv::Scalar(0,0,255),3); //注意先列后行
    for (int row = 0; row < 480; row++ )
    {
      for (int  col= 0; col< 640; col++ )
      {
        image_color[row][col] = (cv::Vec3b)image.at<cv::Vec3b>(row, col);
      }
    }
    // cv::imshow("view", image);
  }
    catch (cv_bridge::Exception& e){
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

主要功能有两个

1、将接收到的ros格式的图像通过cv_bridge转换成cv::Mat

2、将cv::Mat转换成cv::Vec3b(这一步不要应该也行，通过cv::Mat获取RGB信息)

4.3、点云订阅函数(融合)

  //点云回调函数
  void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  fusion_pcl_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);   //放在这里是因为，每次都需要重新初始化，舍弃了原有但没用的两个通道intensity、ring
    //pcl::PointCloud<PointXYZIR>::Ptr   raw_pcl_ptr (new pcl::PointCloud<PointXYZIR>);   //VLP-16的点云消息包含xyz和intensity、ring的，这里没有加ring不知道为啥也可以，需要的话要自己定义一个点类型PointXYZIR
    pcl::PointCloud<pcl::PointXYZ>::Ptr   raw_pcl_ptr (new pcl::PointCloud<pcl::PointXYZ>);  
    pcl::fromROSMsg(*laserCloudMsg, *raw_pcl_ptr);  //把msg消息指针转化为点云指正
 
    //另一种做法
    Eigen::Vector3f c_tmp;
    Eigen::Vector3f p_result;
    for (int i = 0; i <  raw_pcl_ptr->points.size(); i++)
    {
      c_tmp<<raw_pcl_ptr->points[i].x, raw_pcl_ptr->points[i].y, raw_pcl_ptr->points[i].z;
    
      p_result = K * (R* c_tmp+t);  //坐标转换
      cv::Point pt;
      pt.x =  (int)((p_result[0] / p_result[2])) ;
      pt.y =  (int)((p_result[1] / p_result[2])) ;
      // std::cout<<  pt << std::endl;
 
      if ( pt.x >=0 &&pt.x <  640 && pt.y>=0 && pt.y<480 && raw_pcl_ptr->points[i].x>0) //&& raw_pcl_ptr->points[i].x>0去掉图像后方的点云
      {
        // PointXYZRGBIR  p;
        pcl::PointXYZRGB p;
        p.x=raw_pcl_ptr->points[i].x;
        p.y=raw_pcl_ptr->points[i].y;
        p.z=raw_pcl_ptr->points[i].z;
        
        //点云颜色由图像上对应点确定
        p.b = image_color[pt.y][pt.x][0];
        p.g = image_color[pt.y][pt.x][1];
        p.r = image_color[pt.y][pt.x][2];
        //将点云画在图像上
        // std::cout<<pt.y<<"   "<<pt.x<<std::endl;
        // cv::Point center(pt.y,pt.x);
        // cv::Scalar colorCircle1(0, 0, 255);
        // cv::circle(image_copy, center, 0.1, colorCircle1, 0.1 );
        //若点云类型为XYZRGBIR则需要添加IR信息
        //p.label = (rand() % (10+1));  //设置10个标签，标签随机
        //p.intensity = raw_pcl_ptr->points[i].intensity;  //继承之前点云的intensity
        //p.ring = raw_pcl_ptr->points[i].ring;  //继承之前点云的ring
 
        fusion_pcl_ptr->points.push_back(p);
      }
    }
    //发布画了点云的图像
    // sensor_msgs::ImagePtr msg2 = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_copy).toImageMsg();
    // img_pub.publish(msg2);
 
    fusion_pcl_ptr->width = 1;
    fusion_pcl_ptr->height = fusion_pcl_ptr->points.size();
    // std::cout<<  fusion_pcl_ptr->points.size() << std::endl;
    pcl::toROSMsg( *fusion_pcl_ptr, fusion_msg);  //将点云转化为消息才能发布
    fusion_msg.header.frame_id = "world_enu";//帧id改成和velodyne一样的
    pubCloud.publish( fusion_msg); //发布调整之后的点云数据，主题为/adjustd_cloud
  }

主要功能有

1、创建XYZRGB类型点云集和用于储存融合好的点云

2、将接收到的ros类型的点云转换成pcl::pointxyz类型

3、点云投影到相机

4、融合储存

5、将pcl点云转换成ros格式并发布

5、运行效果

launch

首先可以创建一个简单的launch文件用于启动，避免每次都添加新节点

<launch>
    <node pkg="selfslam" type="fucon2" name="fucon2" output="screen" />
       <arg name="rviz" default="true" />
    <group if="$(arg rviz)">
        <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find selfslam)/rviz_cfg/aloam_velodyne.rviz" />
    </group>
</launch>

运行

打开一个终端
cd ~/work/study/ros/my_slam（这是我的目录避免忘记，记录一下）
roscore
打开另一个终端
catkin_make
source devel/setup.bash
roslaunch selfslam xxx（selfslam是我自己的功能包的名称你都需要换成你自己的）
 
再打开一个终端
cd ~/work/study/ros
rosbag play xxxx.bag (换成你自己的)
rosbag play yuantu+nengyong.bag （这是我的）

结果

如果觉得自己的自己的点云在rviz里是反着的且调不过来可以通过这个按钮调节反转z轴