通过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将点分配到cell int yc = (int) ((y - yMin) / cellResolution); /* 法线是单位向量,z是发现的z值,z越接近于1,表明法线越垂直。 而acos(z)使得z值越接近于1,结果越小. 即acos(z)的结果越大,z越不垂直 */ double normal_value = acos(fabs(z));//值域 0--phi 地面fabs(z)应该是1 acos是0,最小值 if (normal_value > deviation) //根据acos函数的递减性质,非地面点的值应该都比地面点大。可以设置deviation值,决定障碍物点的阈值 countGrid[yc * xCells + xc]++; //统计一个cell中垂直方向满足条件的点数 } 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); //存储每个cell的值 0或者100 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将点分配到cell int yc = (int) ((y - yMin) / cellResolution); /* 法线是单位向量,z是发现的z值,z越接近于1,表明法线越垂直。 而acos(z)使得z值越接近于1,结果越小. 即acos(z)的结果越大,z越不垂直 */ double normal_value = acos(fabs(z));//值域 0--phi 地面fabs(z)应该是1 acos是0,最小值 if (normal_value > deviation) //根据acos函数的递减性质,非地面点的值应该都比地面点大。可以设置deviation值,决定障碍物点的阈值 countGrid[yc * xCells + xc]++; //统计一个cell中垂直方向满足条件的点数 } 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将点分配到cell int yc = (int) ((p.y - yMin) / cellResolution); countGrid[yc * xCells + xc]++; //统计一个cell中垂直方向满足条件的点数 } 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); //存储每个cell的值 0或者100 //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