通过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();
}
} 有了栅格地图，就可以进行全局路径规划了。
完整的代码我上传到github
