#include <pcl_conversions/pcl_conversions.h>。

程式的最後將點雲地圖儲存為map.pcd。我們可以使用pcl_viewer檢視：

點雲地圖的二維投影

這裡通過cloud_to_map功能包將點雲地圖轉換二維地圖。cloud_to_map好象是北達科他大學( North Dakota State University)，更具體的資訊我不知道了。下載地址為：PointCloud-to-grid-map上下載的，但是直接使用貌似有點問題，而且這個程式為了動態調參，寫的太複雜了，我將其簡化了。其主程式如下：

#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 <dynamic_reconfigure/server.h>
#include <cloud_to_map/cloud_to_map_nodeConfig.h>  //autogenerated for the cloud_to_map package by the dynamic_reconfigure package.

/* Define the two point cloud types used in this code */
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::Normal> NormalCloud;

/* Global */
PointCloud::ConstPtr currentPC;
bool newPointCloud = false;
bool reconfig = false;

// -----------------------------------------------
// -----Define a structure to hold parameters-----
// -----------------------------------------------
struct Param 
{
  std::string frame;
  double searchRadius;
  double deviation;
  int buffer;
  double loopRate;
  double cellResolution;
};

// -----------------------------------
// -----Define default parameters-----
//If the value cannot be retrieved from the server, default_val is used instead.
// -----------------------------------
Param param;
boost::mutex mutex;  //多執行緒
void loadDefaults(ros::NodeHandle& nh) 
{
  nh.param<std::string>("frame", param.frame, "world");
  nh.param("search_radius", param.searchRadius, 0.05);
  nh.param("deviation", param.deviation, 0.78539816339);
  nh.param("buffer", param.buffer, 5);
  nh.param("loop_rate", param.loopRate, 10.0);
  nh.param("cell_resolution", param.cellResolution, 0.05);
}


// ------------------------------------------
// -----Callback for Dynamic Reconfigure-----呼叫動態引數配置
// ------------------------------------------
void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level) 
{
  
  boost::unique_lock<boost::mutex>(mutex);
  param.frame = config.frame.c_str();
  param.searchRadius = config.search_radius;
  param.deviation = config.deviation;
  param.buffer = config.buffer;
  param.loopRate = config.loop_rate;
  //param.cellResolution = config.cell_resolution;
  param.cellResolution=0.05;
  reconfig = true;


    ROS_INFO("\n\nReconfigure Request: \nframe:%s \ndeviation:%f \nloop_rate:%f \ncell_resolution:%f \nsearch_radius:%f", 
                  param.frame.c_str(), param.deviation,param.loopRate,param.cellResolution, param.searchRadius);

}

// ------------------------------------------------------
// -----Update current PointCloud if msg is received-----
// ------------------------------------------------------
void callback(const PointCloud::ConstPtr& msg) 
{
  boost::unique_lock<boost::mutex>(mutex); //unique_lock是write lock。被鎖後不允許其他執行緒執行被shared_lock或unique_lock的程式碼。
                                           //在寫操作時，一般用這個，可以同時限制unique_lock的寫和share_lock的讀。
  //ROS_INFO("callback is finshed \n");
  currentPC = msg;
  newPointCloud = true;
}

// ----------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.05-----
// ----------------------------------------------------------------
void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud<pcl::Normal>::Ptr normals) 
{
  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
  ne.setInputCloud(cloud);
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
  ne.setSearchMethod(tree);
  ne.setRadiusSearch(param.searchRadius);
  ne.compute(*normals);
}

// ---------------------------------------
// -----Initialize Occupancy Grid Msg-----
// ---------------------------------------
void initGrid(nav_msgs::OccupancyGridPtr grid) 
{
  grid->header.seq = 1;
  grid->header.frame_id = param.frame;  //參考frame
  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;
  std::cout<<"initGrid finshed!\n"<< std::endl;
/*  nav_msgs/OccupancyGrid
# This represents a 2-D grid map, in which each cell represents the probability of occupancy.
Header header 

#MetaData for the map
MapMetaData info

# The map data, in row-major order, starting with (0,0).  Occupancy
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data

Header header
    uint32 seq
    time stamp
    string frame_id
MapMetaData info
    time map_load_time
    float32 resolution
    uint32 width
    uint32 height
    geometry_msgs/Pose origin
        geometry_msgs/Point position
            float64 x
            float64 y
            float64 z
        geometry_msgs/Quaternion orientation
            float64 x
            float64 y
            float64 z
            float64 w
int8[] data  */
}

// ------------------------------------------
// -----Calculate size of Occupancy Grid-----
// ------------------------------------------
void calcSize(double *xMax, double *yMax, double *xMin, double *yMin) 
{
  for (size_t i = 0; i < currentPC->size(); i++) 
  {
    double x = currentPC->points[i].x;
    double y = currentPC->points[i].y;
    if (*xMax < x) {
      *xMax = x;
    }
    if (*xMin > x) {
      *xMin = x;
    }
    if (*yMax < y) {
      *yMax = y;
    }
    if (*yMin > y) {
      *yMin = y;
    }
  }
}

// ---------------------------------------
// -----Populate 填充 map with cost values-----
// ---------------------------------------
void populateMap(NormalCloud::Ptr cloud_normals, std::vector<int> &countGrid, double xMin, double yMin,
                                                      double cellResolution, int xCells, int yCells) 
{
  double deviation = param.deviation;

  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 xCell, yCell;
  //TODO implement cutoff height
    xCell = (int) ((x - xMin) / cellResolution); //取整後預設按照cellResolution將點分配到ｃｅｌｌ
    yCell = (int) ((y - yMin) / cellResolution);
    if ((yCell * xCells + xCell) > (xCells * yCells)) 
      std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell<< "\n";

    double normal_value = acos(fabs(z));//值域　０－－ｐｈｉ   地面fabs(z)應該是１　acos是０，最小值
    if (normal_value > deviation)       //根據acos函式的遞減性質，非地面點的值應該都比地面點大。可以設定deviation值，決定障礙物點的閾值
      countGrid[yCell * xCells + xCell]++; //統計一個ｃｅｌｌ中垂直方向滿足條件的點數
    //else 
      //countGrid[yCell * xCells + xCell]--; 
  }
}

// ---------------------------------
// -----Generate Occupancy Grid-----
// ---------------------------------
void genOccupancyGrid(std::vector<signed char> &ocGrid, std::vector<int> &countGrid, int size) 
{
  int buf = param.buffer;
  for (int i = 0; i < size; i++)  //size:xCells * yCells
  {
 if (countGrid[i] < buf && countGrid[i]>0) 
      ocGrid[i] = 0;
    else if (countGrid[i] > buf) 
      ocGrid[i] = 100;
    else if (countGrid[i] == 0) 
      ocGrid[i] = 0; // TODO Should be -1      
  }
  //std::cout<<" genOccupancyGrid finshed!"<< std::endl;

}
// -----------------------------------
// -----Update Occupancy Grid Msg-----
// -----------------------------------
void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells,
                               double originX, double originY, std::vector<signed char> *ocGrid) 
{
  grid->header.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 = cellRes;
  grid->info.width = xCells;
  grid->info.height = yCells;
  grid->info.origin.position.x = originX;  //minx
  grid->info.origin.position.y = originY;  //miny
  grid->data = *ocGrid;
  std::cout<<"updateGrid finshed!!"<< std::endl;

}

int main(int argc, char** argv) 
{
  /* Initialize ROS */
  ros::init(argc, argv, "cloud_to_map_node");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<PointCloud>("/point_cloud_raw", 1, callback); 
  ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);

  /* Initialize Dynamic Reconfigure 　ＲＯＳ動態引數配置*/
  dynamic_reconfigure::Server<cloud_to_map::cloud_to_map_nodeConfig> server;
  dynamic_reconfigure::Server<cloud_to_map::cloud_to_map_nodeConfig>::CallbackType f;
  f = boost::bind(&callbackReconfig, _1, _2);
  //std::cout<<"*************************\n";  //test
  server.setCallback(f);

  /* Initialize Grid */
  nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
  initGrid(grid);
  /* Finish initializing ROS */
  mutex.lock();
  ros::Rate loop_rate(param.loopRate);
  mutex.unlock();

  /* Wait for first point cloud */
  while(ros::ok() && newPointCloud == false)
  {
    ros::spinOnce();//ROS訊息回撥處理函式。它倆通常會出現在ROS的主迴圈中，程式需要不斷呼叫ros::spin() 或 ros::spinOnce()，
                     //兩者區別在於前者呼叫後不會再返回，也就是你的主程式到這兒就不往下執行了，而後者在呼叫後還可以繼續執行之後的程式。
                     //所接到的訊息並不是立刻就被處理，而是必須要等到ros::spin()或ros::spinOnce()執行的時候才被呼叫，
    loop_rate.sleep();
  }

  /* Begin processing point clouds */
  while (ros::ok()) 
  {
    ros::spinOnce();//用後還可以繼續執行之後的程式。
    boost::unique_lock<boost::mutex> lck(mutex);
    if (newPointCloud || reconfig) 
    {
      /* Update configuration status */
      reconfig = false;  //呼叫動態引數配置時　＝true
      newPointCloud = false;  //在回撥函式=true

      /* Record start time */
      uint32_t sec = ros::Time::now().sec; //秒
      uint32_t nsec = ros::Time::now().nsec;//納秒

      /* Calculate Surface Normals */
      NormalCloud::Ptr cloud_normals(new NormalCloud);
      calcSurfaceNormals(currentPC, cloud_normals);

      /* Figure out size of matrix needed to store data. */
      double xMax = 0, yMax = 0, xMin = 0, yMin = 0;
      calcSize(&xMax, &yMax, &xMin, &yMin);  //Calculate size of Occupancy Grid
      //std::cout <<"size of matrix needed to store data:"<<"xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: "<< yMin << "\n";

      /* Determine resolution of grid (m/cell) */
      double cellResolution = param.cellResolution;
      int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1;
      int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1;
      std::cout <<"the number of cells: " <<xCells*yCells<<"\n";

      /* Populate Map */
      std::vector<int> countGrid(yCells * xCells);
      populateMap(cloud_normals, countGrid, xMin, yMin, cellResolution, xCells, yCells);

      /* Generate OccupancyGrid Data Vector */
      std::vector<signed char> ocGrid(yCells * xCells);  //儲存每個ｃｅｌｌ的值　　０或者１００
      genOccupancyGrid(ocGrid, countGrid, xCells * yCells);

      /* Update OccupancyGrid Object */
      updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid);

      /* Release lock */
      lck.unlock();

      /* Record end time */
      uint32_t esec = ros::Time::now().sec;
      uint32_t ensec = ros::Time::now().nsec;
      std::cout << "Seconds: " << esec - sec << "  NSeconds: " << ensec - nsec << "\n\n";

      /* Publish Occupancy Grid */
      pub.publish(grid);
    }
    loop_rate.sleep();
  }
}

有了柵格地圖，就可以進行全域性路徑規劃了，完整的程式碼可以在此處獲取；

總結

此案例其實就是一個有限有高度範圍內的點雲按照解析度投影的案例，相較之下octomap在動態障礙物環境下優勢明顯。