点云地图的二维投影_movebase把fastlio的点云图转为2d导航图

  通过SLAM或其他方式构建的点云地图是无法直接用于导航的，我知道的解决方案有三种：

一、将点云地图二维投影，转换为可用于导航的二维栅格地图；

二、将点云转换为Octomap八叉树地图，即可使用导航算法，比如RRT*进行三维导航；

三、将实时点云数据转换为实时激光数据，这样就可以愉快的使用ROS的move_base和acml包了。

    这里尝试使用第一种方案。

构建点云地图

    构建点云地图需要深度图和对应的位姿，这里使用高翔的<视觉SLAM14讲>的深度图和位姿。这里构建的是一个ROS功能包，代码如下：

pcl_test.cpp

#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <fstream>
 
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
 
#include <ros/ros.h>
 
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
 
#include <pcl/common/common_headers.h>
#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 
#include <pcl/filters/voxel_grid.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_cloud.h>
 
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
 
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
 
 
using namespace std;
 
void PCLTest();
ros::Publisher pub_pcs;
ros::Publisher pub_pcr;
 
int main(int argc, char** argv)
{
    ros::init(argc, argv, "pcl_test_node");//初始化节点
    ros::start();//启动节点
    ros::NodeHandle nh;
    pub_pcs=nh.advertise<sensor_msgs::PointCloud2>("/point_cloud",1);
    pub_pcr=nh.advertise<PointCloud>("/point_cloud_raw",1);
    ROS_INFO_STREAM("Initing");
    PCLTest();
    ROS_INFO_STREAM("pcl_test节点结束");
    return 0;
}
 
void PCLTest()
{
 
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿
    ifstream fin("./data/pose.txt");
    if (!fin)
    {
        cerr<<"cannot find pose file"<<endl;
        return;
    }
    
    for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "./data/%s/%d.%s" ); //图像文件格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
        double data[7] = {0};
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
        }
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
     }
     // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    cout<<"正在将图像转换为点云..."<<endl;
    // 新建一个点云
     PointCloud::Ptr pointCloud( new PointCloud );
     //pcl::visualization::CloudViewer viewer("pcd viewer");
     
     for ( int i=0; i<5; i++ )
     {
        PointCloud::Ptr current( new PointCloud );
        cout<<"转换图像中: "<<i+1<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                if ( d >= 3500 ) continue; // 深度太大时不稳定，去掉
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T*point;
                
                
                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                current->points.push_back( p );
            }
        //利用统计滤波器方法去除孤立点。
        PointCloud::Ptr tmp ( new PointCloud );
        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
        statistical_filter.setMeanK(50);
        statistical_filter.setStddevMulThresh(1.0);
        statistical_filter.setInputCloud(current);
        statistical_filter.filter( *tmp );
        (*pointCloud) += *tmp;
        //viewer.showCloud(pointCloud);
        //getchar();
    }
    
    getchar();
    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
        //体素滤波器（Voxel Filter）进行降采样
    pcl::VoxelGrid<PointT> voxel_filter;
    voxel_filter.setLeafSize( 0.01, 0.01, 0.01 );       //分辨率
    PointCloud::Ptr tmp ( new PointCloud );
    voxel_filter.setInputCloud( pointCloud );
    voxel_filter.filter( *tmp );
    tmp->swap(*pointCloud);
    cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl;
    
    
    sensor_msgs::PointCloud2 pmsg;
    pcl::toROSMsg(*pointCloud, pmsg);
    pmsg.header.frame_id = "map";
    
    pub_pcs.publish(pmsg);
    pub_pcr.publish(*pointCloud);
    
    getchar();
    
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
}

    PCLTest函数是构建点云的函数，首先从pose.txt中读取位姿，然后读取rgb图像和深度图像，接着将像素坐标转换为世界坐标，在存入点云中，每张图片构建的点云都进行统计滤波。构建完成点云之后，进行体素滤波。最后发布点云。这里发布两个消息：sensor_msgs::PointCloud2 和 PointCloud，其中sensor_msgs::PointCloud2类型的点云用于rviz点云可视化，而PointCloud则用于给cloud_to_map包转换为二维地图。

    需要注意的是，若想直接发布点云类型 pcl::PointCloud<PointT>的消息，需要导入头文件#include <pcl_ros/point_cloud.h>。若想将pcl::PointCloud<PointT>转换为sensor_msgs::PointCloud2，需要导入头文件

#include <pcl_conversions/pcl_conversions.h>。

    程序的最后将点云地图保存为map.pcd。我们可以使用pcl_viewer查看：

点云地图的二维投影

    这里通过cloud_to_map功能包将点云地图转换二维地图。cloud_to_map好象是北达科他大学( North Dakota State University)，更具体的信息我不知道了。我这里使用的是从github：https://github.com/306327680/PointCloud-to-grid-map上下载的，但是直接使用貌似有点问题，而且这个程序为了动态调参，写的太复杂了，我将其简化了。其主程序如下：

cloud_to_map_node.cpp

#include <iostream>
#include <fstream>
#include <stdint.h>
#include <math.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <nav_msgs/OccupancyGrid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/console/parse.h>
#include <pcl/filters/passthrough.h>
 
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::Normal> NormalCloud;
 
using namespace std;
 
/* 全局变量 */
PointCloud::ConstPtr currentPC;
bool newPointCloud = false;
 
double cellResolution=0.05;
double deviation = 0.785398;
int buffer_size=50;//每个栅格法线垂直的阈值
ros::Publisher pub;
 
 
void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) 
{
  pcl::PointXYZRGB minPt, maxPt;
	pcl::getMinMax3D(*currentPC, minPt, maxPt);	
 
  *xMax=maxPt.x;
  *yMax=maxPt.y;
  *xMin=minPt.x;
  *yMin=minPt.y;
}
 
//得到栅格地图
void computeGrid( std::vector<signed char> &ocGrid, double xMin, double yMin,int xCells, int yCells) 
{
	cout<<"开始计算法线"<<endl;
	//计算点云的法线
	NormalCloud::Ptr cloud_normals(new NormalCloud);
	pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
	ne.setInputCloud(currentPC);
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
	ne.setSearchMethod(tree);
	ne.setRadiusSearch(0.06);
	ne.compute(*cloud_normals);
	
	cout<<"判断法线是否垂直"<<endl;
	
	int size=xCells*yCells;
	std::vector<int> countGrid(size);
	
	//判断每个点云的法向量是否垂直
	for (size_t i = 0; i < currentPC->size(); i++)
	{
		double x = currentPC->points[i].x;
		double y = currentPC->points[i].y;
		double z = cloud_normals->points[i].normal_z;
 
		int xc = (int) ((x - xMin) / cellResolution); //取整后默认按照cellResolution将点分配到ｃｅｌｌ
		int yc = (int) ((y - yMin) / cellResolution);
 
		/*
			法线是单位向量，z是发现的z值，z越接近于1,表明法线越垂直。
			而acos(z)使得z值越接近于1,结果越小.
			即acos(z)的结果越大，z越不垂直
		*/
		double normal_value = acos(fabs(z));//值域　０－－ｐｈｉ   地面fabs(z)应该是１　acos是０，最小值
		if (normal_value > deviation)       //根据acos函数的递减性质，非地面点的值应该都比地面点大。可以设置deviation值，决定障碍物点的阈值
		  countGrid[yc * xCells + xc]++; //统计一个ｃｅｌｌ中垂直方向满足条件的点数
	}
	
	cout<<"计算占据概率"<<endl;
	
	//根据阈值计算占据概率
	for (int i = 0; i < size; i++)  //size:xCells * yCells
	{
		if (countGrid[i] < buffer_size && countGrid[i]>0) 
		  ocGrid[i] = 0;
		else if (countGrid[i] > buffer_size) 
		  ocGrid[i] = 100;
		else if (countGrid[i] == 0) 
		  ocGrid[i] = 0; // TODO Should be -1      
	}
}
 
 
void updateGrid(nav_msgs::OccupancyGridPtr grid, int xCells, int yCells,
                               double originX, double originY, std::vector<signed char> *ocGrid) 
{
	static int seq=0;
 
	grid->header.frame_id = "map";
	grid->header.seq=seq++;
	grid->header.stamp.sec = ros::Time::now().sec;
	grid->header.stamp.nsec = ros::Time::now().nsec;
	grid->info.map_load_time = ros::Time::now();
	grid->info.resolution = cellResolution;
	grid->info.width = xCells;
	grid->info.height = yCells;
	grid->info.origin.position.x = originX;  //minx
	grid->info.origin.position.y = originY;  //miny
	grid->info.origin.position.z = 0;
	grid->info.origin.orientation.w = 1;
	grid->info.origin.orientation.x = 0;
	grid->info.origin.orientation.y = 0;
	grid->info.origin.orientation.z = 0;
	grid->data = *ocGrid;
}
 
 
 
void callback(const PointCloud::ConstPtr& msg) 
{
  currentPC=msg;
  ROS_INFO_STREAM("Convertor节点——接收到点云");
	
		/*计算点云的最大和最小值*/
	double xMax = 0, yMax = 0, xMin = 0, yMin = 0;
	calcSize(&xMax, &yMax, &xMin, &yMin); 
 
	cout<<"极值："<<xMax<<" "<<yMax<<" "<<xMin<<" "<<yMin<<" "<<endl;
	
	/* 确定栅格地图的长和宽 */
	int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1;
	int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1;
	
	cout<<"地图大小："<<xCells<<" "<<yCells<<endl;
 
	/*计算栅格地图*/
	std::vector<signed char> ocGrid(yCells * xCells);  //存储每个ｃｅｌｌ的值　　０或者１００
	computeGrid(ocGrid, xMin, yMin, xCells, yCells);
	
	cout<<"成功计算得到栅格地图"<<endl;
 
	//发布地图消息
	nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
	updateGrid(grid, xCells, yCells, xMin, yMin, &ocGrid);
	pub.publish(grid);
	ROS_INFO_STREAM("Convertor节点——发布栅格地图");
}
 
 
int main(int argc, char** argv) 
	{
	setlocale(LC_ALL, "");
	
	ros::init(argc, argv, "convertor_node");
	ros::NodeHandle nh;
 
	ros::Subscriber sub = nh.subscribe<PointCloud>("/point_cloud/raw", 1, callback); 
	pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);
	
	//构造占据网格消息
	nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
	grid->header.seq = 1;
	grid->header.frame_id = "map";//父坐标系
	grid->info.origin.position.z = 0;
	grid->info.origin.orientation.w = 1;
	grid->info.origin.orientation.x = 0;
	grid->info.origin.orientation.y = 0;
	grid->info.origin.orientation.z = 0;
 
	
	ROS_INFO_STREAM("Convertor节点初始化完成");
	ros::Rate loop_rate(0.2);
	ros::Duration t(10);
	while (ros::ok()) 
	{
		ros::spinOnce();
 
		t.sleep();
	}
}

    cloud_to_map订阅点云：/point_cloud_raw，发布的栅格地图：/map。而且dynamic_reconfigure功能包动态配置参数。

    在rviz接手栅格地图：

    这个程序通过判断法向量的方法获得地图，但我觉得这种方法计算量大，而是感觉效果一半。我这里提出一种更简单的方法，直接将z轴0-0.5范围内的点云投影，得到栅格地图，程序如下：

#include <iostream>
#include <fstream>
#include <stdint.h>
#include <math.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <nav_msgs/OccupancyGrid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/console/parse.h>
#include <pcl/filters/passthrough.h>
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::Normal> NormalCloud;
 
 
using namespace std;
 
/* 全局变量 */
PointCloud::ConstPtr currentPC;
bool newPointCloud = false;
double cellResolution=0.05;
double deviation = 0.785398;
int buffer_size=50;//每个栅格法线垂直的阈值
 
ros::Publisher pub;
 
 
void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) 
{
      pcl::PointXYZRGB minPt, maxPt;
    pcl::getMinMax3D(*currentPC, minPt, maxPt);
      *xMax=maxPt.x;
      *yMax=maxPt.y;
      *xMin=minPt.x;
      *yMin=minPt.y;
}
 
 
//得到栅格地图
void computeGrid( std::vector<signed char> &ocGrid, double xMin, double yMin,int xCells, int yCells) 
{
    cout<<"开始计算法线"<<endl;
    //计算点云的法线
    NormalCloud::Ptr cloud_normals(new NormalCloud);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    ne.setInputCloud(currentPC);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
    ne.setSearchMethod(tree);
    ne.setRadiusSearch(0.06);
    ne.compute(*cloud_normals);
    cout<<"判断法线是否垂直"<<endl;
    int size=xCells*yCells;
    std::vector<int> countGrid(size);
    //判断每个点云的法向量是否垂直
    for (size_t i = 0; i < currentPC->size(); i++)
    {
    double x = currentPC->points[i].x;
    double y = currentPC->points[i].y;
    double z = cloud_normals->points[i].normal_z;
    int xc = (int) ((x - xMin) / cellResolution); //取整后默认按照cellResolution将点分配到ｃｅｌｌ
    int yc = (int) ((y - yMin) / cellResolution);
    /*
    法线是单位向量，z是发现的z值，z越接近于1,表明法线越垂直。
    而acos(z)使得z值越接近于1,结果越小.
    即acos(z)的结果越大，z越不垂直
    */
    double normal_value = acos(fabs(z));//值域　０－－ｐｈｉ   地面fabs(z)应该是１　acos是０，最小值
    if (normal_value > deviation)       //根据acos函数的递减性质，非地面点的值应该都比地面点大。可以设置deviation值，决定障碍物点的阈值
      countGrid[yc * xCells + xc]++; //统计一个ｃｅｌｌ中垂直方向满足条件的点数
    }
    cout<<"计算占据概率"<<endl;
    //根据阈值计算占据概率
    for (int i = 0; i < size; i++)  //size:xCells * yCells
    {
    if (countGrid[i] < buffer_size && countGrid[i]>0) 
      ocGrid[i] = 0;
    else if (countGrid[i] > buffer_size) 
      ocGrid[i] = 100;
    else if (countGrid[i] == 0) 
      ocGrid[i] = 0; // TODO Should be -1      
    }
}
 
 
 
void updateGrid(nav_msgs::OccupancyGridPtr grid, int xCells, int yCells,
                               double originX, double originY, std::vector<signed char> *ocGrid) 
{
    static int seq=0;
    grid->header.frame_id = "map";
    grid->header.seq=seq++;
    grid->header.stamp.sec = ros::Time::now().sec;
    grid->header.stamp.nsec = ros::Time::now().nsec;
    grid->info.map_load_time = ros::Time::now();
    grid->info.resolution = cellResolution;
    grid->info.width = xCells;
    grid->info.height = yCells;
    grid->info.origin.position.x = originX;  //minx
    grid->info.origin.position.y = originY;  //miny
    grid->info.origin.position.z = 0;
    grid->info.origin.orientation.w = 1;
    grid->info.origin.orientation.x = 0;
    grid->info.origin.orientation.y = 0;
    grid->info.origin.orientation.z = 0;
    grid->data = *ocGrid;
}
 
 
void computeGrid2(std::vector<signed char> &ocGrid, double xMin, double yMin,int xCells, int yCells)
{
    PointCloud::Ptr cpc(new PointCloud);
    pcl::PassThrough<PointT> *passFilter=new pcl::PassThrough<PointT>;
    passFilter->setFilterFieldName("z");
    passFilter->setFilterLimitsNegative(false);//保留此区间内的数据
    passFilter->setFilterLimits(0,0.5);
    passFilter->setInputCloud(currentPC);
    passFilter->filter(*cpc);
    int size=xCells*yCells;
    std::vector<int> countGrid(size);
    //将每个点云分配到各个网格
    for (size_t i = 0; i < cpc->size(); i++)
    {
    PointT p=cpc->points[i];
    int xc = (int) ((p.x - xMin) / cellResolution); //取整后默认按照cellResolution将点分配到ｃｅｌｌ
    int yc = (int) ((p.y - yMin) / cellResolution);
    countGrid[yc * xCells + xc]++; //统计一个ｃｅｌｌ中垂直方向满足条件的点数
    }
    for (int i = 0; i < size; i++)  //size:xCells * yCells
    {
    if (countGrid[i] < 10 && countGrid[i]>0) 
      ocGrid[i] = 0;
    else if (countGrid[i] > 10) 
      ocGrid[i] = 100;
    else if (countGrid[i] == 0) 
      ocGrid[i] = 0; // TODO Should be -1      
    }
}
 
 
void callback(const PointCloud::ConstPtr& msg) 
{
  currentPC=msg;
  ROS_INFO_STREAM("Convertor节点——接收到点云");
    /*计算点云的最大和最小值*/
    double xMax = 0, yMax = 0, xMin = 0, yMin = 0;
    calcSize(&xMax, &yMax, &xMin, &yMin); 
    cout<<"极值："<<xMax<<" "<<yMax<<" "<<xMin<<" "<<yMin<<" "<<endl;
    /* 确定栅格地图的长和宽 */
    int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1;
    int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1;
    cout<<"地图大小："<<xCells<<" "<<yCells<<endl;
    /*计算栅格地图*/
    std::vector<signed char> ocGrid(yCells * xCells);  //存储每个ｃｅｌｌ的值　　０或者１００
    //computeGrid(ocGrid, xMin, yMin, xCells, yCells);
    computeGrid2(ocGrid, xMin, yMin, xCells, yCells);
    cout<<"成功计算得到栅格地图"<<endl;
    //发布地图消息
    nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
    updateGrid(grid, xCells, yCells, xMin, yMin, &ocGrid);
    pub.publish(grid);
    ROS_INFO_STREAM("Convertor节点——发布栅格地图");
}
 
 
int main(int argc, char** argv) 
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "convertor_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<PointCloud>("/point_cloud/raw", 1, callback); 
    pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);
    //构造占据网格消息
    nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
    grid->header.seq = 1;
    grid->header.frame_id = "map";//父坐标系
    grid->info.origin.position.z = 0;
    grid->info.origin.orientation.w = 1;
    grid->info.origin.orientation.x = 0;
    grid->info.origin.orientation.y = 0;
    grid->info.origin.orientation.z = 0;
    ROS_INFO_STREAM("Convertor节点初始化完成");
    ros::Rate loop_rate(0.2);
    ros::Duration t(10);
    while (ros::ok()) 
    {
    ros::spinOnce();
    t.sleep();
    }
}

    有了栅格地图，就可以进行全局路径规划了。