【ROS】利用ROS将KITTI数据集点云数据投影到2D图像

课题涉及到感知融合，首先需要将点云投影到图像上，本文利用ROS实现投影。分为两个ROS节点，一个节点负责不断读取点云原始bin文件，并以 sensor_msgs::PointCloud2 的消息格式发布话题；另一个节点对接收到的点云消息进行处理，把点云投影到相机图像上。

目录

• 笔者环境
• 一、创建ROS工作空间
  *
• 1. 创建工作空间目录
• 2. 编译工作空间
• 3. 设置环境变量
• 二、创建投影功能包
  *
• 1. 统一格式
• 2. 创建点云投影功能包
• 3. 编写节点cpp文件
• 三、创建launch功能包
  *
• 1. 创建launch包
• 2. 创建 run.launch 文件
• 3. 编写 run.launch 文件
• 四、在project功能包添加其他依赖
  *
• 1. 修改project目录下的CMakeLists.txt
• 2. 修改project目录下的package.xml
• 五、编译project功能包
• 六、投影效果展示

笔者环境

Ubuntu18.04+ ROS Melodic + Opencv 3.2.0 + Eigen 3.3.7 + PCL 1.8.0
测试数据集是Kitti的raw_data的2011_09_26（如果使用其他天的数据，需要在lidtocamp.cpp中修改标定数据）

一、创建ROS工作空间

1. 创建工作空间目录

这步比较基础，目的是在src文件中创建了一个 CMakeLists.txt 的文件，告诉系统这个是ROS的工作空间

mkdir -p ~/catkin_ws/src

cd ~/catkin_ws/src
catkin_init_workspace

2. 编译工作空间

在catkin_ws文件夹下进行编译：

cd ~/catkin_ws/
catkin_make

编译完成后，catkin_ws中新增build 和 devel

3. 设置环境变量

安装ROS的时候是将整个ros系统的环境变量设置到bash脚本中，现在我们需要把我们工作空间的环境变量设置到bash中。

gedit ~/.bashrc

source ~/catkin_ws/devel/setup.bash

source ~/.bashrc

如果想查看ros的环境变量： echo $ROS_PACKAGE_PATH
终端输出： /home/wdd/catkin_ws/src : /opt/ros/kinetic/share
第一个为我们刚创建工作空间的，第二个是ROS系统的

二、创建投影功能包

1. 统一格式

在src目录下创建

catkin_create_pkg   package_name   depend1 depend2 depend2

package_name：功能包名称
depend：依赖项

2. 创建点云投影功能包

在src目录下使用 catkin_create_pkg 创建一个 package 包并添加 roscpp 和 std_msgs 依赖项

 catkin_create_pkg Lid_project_cam roscpp std_msgs

std_msgs：包含常见消息类型
roscpp：使用C++实现ROS各种功能
(如果使用python实现ROS各种功能，添加rospy)

3. 编写节点cpp文件

在功能包目录的src文件夹下创建节点文件：

cd Lid_project_cam/src

touch laserhandle.cpp

touch lidtocamp.cpp

感谢这位博主提供的代码：https://blog.csdn.net/qq_33287871/article/details/107587233

laserhandle.cpp：

ros::Publisher pubLaserCloud;

void laserCloudHandler(pcl::PointCloud<pcl::PointXYZ> laserCloudIn)
{

  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
  int cloudSize = laserCloudIn.points.size();

  sensor_msgs::PointCloud2 cornerPointsMsg;
  pcl::toROSMsg(laserCloudIn, cornerPointsMsg);

  pubLaserCloud.publish(cornerPointsMsg);

}

std::string getFrameStr(unsigned int frame)
{
    if(frame>9999)
        return "00000"+std::to_string(frame);
    else if(frame>999)
        return "000000"+std::to_string(frame);
    else if(frame>99)
        return "0000000"+std::to_string(frame);
    else if(frame>9)
        return "00000000"+std::to_string(frame);
    else if(frame9)
        return "000000000"+std::to_string(frame);
}

void my_handler(int s){
    std::cout<<"Finishing program with signal "<<s<<std::endl;
    exit(1);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserhandle");
  ros::NodeHandle nh;

  struct sigaction sigIntHandler;
  sigIntHandler.sa_handler = my_handler;
  sigemptyset(&sigIntHandler.sa_mask);
  sigIntHandler.sa_flags = 0;
  sigaction(SIGINT, &sigIntHandler, NULL);

  pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>
                                 ("/velodyne_cloud", 10);

  pcl::PointCloud<pcl::PointXYZ> cloud;

  // allocate 4 MB buffer (only ~130*4*4 KB are needed)
  int32_t num = 1000000;
  float *data = (float*)malloc(num*sizeof(float)); //equal to float *data = float[num];分配内存空间

  // pointers
  float *px = data+0;
  float *py = data+1;
  float *pz = data+2;
  float *pr = data+3;

  unsigned int currentFrame = 0;

  std::string file = "点云数据data路径" + getFrameStr(currentFrame) + ".bin";

  FILE *stream;
  stream = fopen (file.c_str(),"rb");
  //std::cout<<file<<std::endl;

  ros::Rate r(10);

  while(stream!=NULL)
  {
      num = fread(data,sizeof(float),num,stream)/4;

      cloud.clear();

      for (int32_t i=0; i<num; i++) {
        pcl::PointXYZ point;
        point.x = *px;
        point.y = *py;
        point.z = *pz;

        cloud.push_back(point);
        px+=4; py+=4; pz+=4; pr+=4;
      }

      laserCloudHandler(cloud);

      //reset variables to read a new sweep
      fclose(stream);
      currentFrame++;
      file = "点云数据data路径" + getFrameStr(currentFrame) + ".bin";
      //std::cout<<file<<std::endl;
      fflush(stream);
      stream = fopen (file.c_str(),"rb");
      free(data);
      num = 1000000;
      data = (float*)malloc(num*sizeof(float));
      px = data+0;
      py = data+1;
    pz = data+2;
    pr = data+3;

    r.sleep();
  }

  free(data);

  return 0;
}

lidtocamp.cpp：

using namespace cv;
using namespace std;

unsigned int currentFrame = 0;

//3x3 rectifying rotation to make image planes co-planar, R_rect_0X:3x3
Eigen::Matrix<double,4,4> R_rect_02;

//3x4 projection matrix after rectification, P_rect_02:3x4
Eigen::Matrix<double,3,4>  P_rect_02;

//Transform from velo to cam0, T:4x4
Eigen::Matrix<double,4,4> extrinsicT;

Mat image ;

string getFrameStr(unsigned int frame)
{
    if(frame>9999)
        return "00000"+to_string(frame);
    else if(frame>999)
        return "000000"+to_string(frame);
    else if(frame>99)
        return "0000000"+to_string(frame);
    else if(frame>9)
        return "00000000"+to_string(frame);
    else if(frame9)
        return "000000000"+to_string(frame);
}

//把激光点云坐标投影到相机二维平面
Eigen::Vector3d transformProject(const Eigen::Vector4d& P_lidar)
{   Eigen::Vector3d z_P_uv = P_rect_02*R_rect_02*extrinsicT*P_lidar;
    return Eigen::Vector3d( int( z_P_uv[0]/z_P_uv[2] + 0.5 ) , int( z_P_uv[1]/z_P_uv[2] + 0.5 ), 1 );
}

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &PointCloudMsg)
{
    std::string image_name = "图像image_02 data路径" + getFrameStr(currentFrame) + ".png";
    cout << "currentFrame:"<< currentFrame << endl;
    image = imread(image_name);//原图
    Mat image_show = image.clone();

    //将点云转为pcl格式
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
    pcl::fromROSMsg(*PointCloudMsg, laserCloudIn);

    int pointcloudsize = laserCloudIn.size(); //点云个数
    cout<<"received pointcloud feature size per sweep = "<<pointcloudsize<<endl;

    vector<Point2f> point;

    int count = pointcloudsize;

    for(int i = 0; i < pointcloudsize; i++){

        Point2f cv_point;
        Eigen::Vector4d P_lidar(laserCloudIn.points[i].x,
                                        laserCloudIn.points[i].y,
                                            laserCloudIn.points[i].z,
                                                1);

        Eigen::Vector3d P_uv = transformProject(P_lidar);

        //去除不在图像上的投影点,并把点转为cv::Point2f类型
        if(P_uv[0] >= 0 && P_uv[1] >= 0 && P_uv[0]1242 && P_uv[1]375){

            cv_point.x = P_uv[0];
            cv_point.y = P_uv[1];
            circle(image_show,cv_point,1,Scalar(0,255,0));
            point.push_back(cv_point);

        }
        else{
            count--;
        }
    }
    int num = point.size();

    cout<<"pointcloudsize after verify = "<<num<<endl<<endl;

    imshow("fusion" , image_show);
    waitKey(10);

    currentFrame++;
}

int main( int argc, char** argv)
{
    ros::init(argc, argv, "lidtocam");
    ros::NodeHandle nh;

    R_rect_02 <<  9.999758e-01, -5.267463e-03, -4.552439e-03, 0,
                5.251945e-03, 9.999804e-01, -3.413835e-03, 0,
                 4.570332e-03, 3.389843e-03, 9.999838e-01, 0,
                 0  ,0  ,  0,  1;

    P_rect_02 << 7.215377e+02, 0.000000e+00, 6.095593e+02, 4.485728e+01,
                 0.000000e+00, 7.215377e+02, 1.728540e+02, 2.163791e-01,
                  0.000000e+00, 0.000000e+00, 1.000000e+00, 2.745884e-03;

    extrinsicT << 7.533745e-03, -9.999714e-01, -6.166020e-04, -4.069766e-03,
            1.480249e-02, 7.280733e-04, -9.998902e-01, -7.631618e-02,
            9.998621e-01, 7.523790e-03, 1.480755e-02, -2.717806e-01,
            0 , 0,  0,  1;

    ros::Subscriber pointCloudSub = nh.subscribe("/velodyne_cloud", 10 ,pointCloudCallback );
    ros::spin();

    return 0;
}

三、创建launch功能包

1. 创建launch包

在catkin_ws/src下用 catkin_create_pkg 创建 launch 包，不添加任何依赖项

catkin_create_pkg launch

2. 创建 run.launch 文件

进入 ~/catkin_ws/src/launch 目录，创建 run.launch 文件

cd launch
touch run.launch

3. 编写 run.launch 文件

分为两个ROS节点，一个节点负责不断读取点云原始bin文件，并以 sensor_msgs::PointCloud2 的消息格式发布话题；另一个节点对接收到的点云消息进行处理，把点云投影到相机图像上。
内容如下：

<launch>
    <node pkg="Lid_project_cam" name = "laserhandle" type = "laserhandle" output="screen" />
    <node pkg="Lid_project_cam" name = "lidtocamp" type = "lidtocamp" output="screen" />
</launch>

四、在project功能包添加其他依赖

因为投影过程中要用到PCL，可视化要用到Opencv，所以我们需要加入PCL和Opencv的依赖到投影功能包中

1. 修改project目录下的CMakeLists.txt

（1）修改 find_package

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  cv_bridge
  image_transport
  pcl_conversions
  pcl_ros
)
find_package(OpenCV REQUIRED)

（2）修改 include_directories

include_directories(

  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

（3）生成可执行文件并连接到库

add_executable(laserhandle src/laserhandle.cpp)
target_link_libraries(laserhandle ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(lidtocamp src/lidtocamp.cpp)
target_link_libraries(lidtocamp ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

2. 修改project目录下的package.xml

在package.xml添加：

  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>

五、编译project功能包

cd ~/catkin_ws/
catkin_make

显示下图则编译成功，在/catkin_ws/devel/lib/project下有了两个可执行文件：laserhandle和lidtocamp

六、投影效果展示

启动run.launch文件，实现节点收发

roslaunch launch run.launch

投影效果如下图所示：

参考致谢：
https://blog.csdn.net/qq_43944331/article/details/117470536
https://blog.csdn.net/EchoChou428/article/details/105257293
https://blog.csdn.net/qq_28306361/article/details/85142192
https://blog.csdn.net/qq_33287871/article/details/107587233

Original: https://blog.csdn.net/weixin_46118817/article/details/125434457
Author: Duuu7
Title: 【ROS】利用ROS将KITTI数据集点云数据投影到2D图像