2016年ICRA的一篇创建语义地图的论文:
Place Categorization and Semantic Mapping on a Mobile Robot
作者已经将代码打包成ROS包开源,直接运行就有不错的效果。这里记录下我自己的调试过程。
1 ros-semantic-mapper 的运行步骤:
git网址: https://github.com/fdayoub/ros-semantic-mapper/blob/master/README.md
操作步骤git里说的比较清楚了已经,这里只解释下后面不太好理解的几步:
categoryIndex_places205.csv
里存的是能够识别的场景及其label,我们可以把需要的场景和label拷贝到my_cats.txt文件中,并给它一个特定的颜色.- 在
semantic_label_publisher/launch/default.launch
文件里,需要全部改成自己的路径. - 启动launch,并播放bag后,打开rviz,把map, odom, semantic_mapper/cloud都显示出来,调整semantic_mapper/cloud的大小和形状,就可以看见涂抹的颜色了,在显示/sem_label_image,还能实时看见image和识别出各种场景的概率
在终端中输入下面的命令,即可启动ros-sematic-mapper
:
roscore
rosbag play rosbag_G6_big_1.bag --clock
roslaunch semantic_mapper run_system_amcl_office.launch
所有话题如下:
nlsde@nlsde-desktop:~$ rostopic list
/amcl_pose
/camera/rgb/image_mono
/clock
/initialpose
/map
/odom
/oneLabel_cloud
/particlecloud
/rosout
/rosout_agg
/scan
/sem_label_image
/semantic_label
/semantic_mapper/cloud
/tf
/tf_static
可以看到我的数据集发布的话题为/scan /odom /camera/rgb/image_mono /tf
由于和作者给出的数据集的话题不同,所以需要在源码中把话题名称改了,改变的文件如下:
semantic_mapper.h:
/**
Copyright (C) 2016, by
Feras Dayoub (feras.dayoub@gmail.com)
This is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This software package is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Leser General Public License.
If not, see <http://www.gnu.org/licenses/>.
**/
#include "semantic_mapper/SemLabel.h"
#include <math.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/Image.h>
#include "message_filters/cache.h"
#include <message_filters/subscriber.h>
#include <laser_geometry/laser_geometry.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
// #include <pcl/ros/conversions.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <octomap/OcTree.h>
#include <octomap/OcTreeKey.h>
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap_ros/conversions.h>
#include <octomap/octomap_types.h>
#include "semantic_mapper/GetSemMap.h"
boost::mutex m_lock_;
typedef pcl::PCLPointCloud2 PclCloud;
typedef pcl::PCLPointCloud2::Ptr PclCloudPtr;
typedef sensor_msgs::PointCloud2 RosCloud;
typedef sensor_msgs::PointCloud2Ptr RosCloudPtr;
typedef octomap::ColorOcTreeNode SemCell;
typedef octomap::ColorOcTree SemMap;
typedef octomap::ColorOcTreeNode::Color CellColor;
#define Pr 0.299;
#define Pg 0.587;
#define Pb 0.114;
using namespace message_filters ;
class SemanticMapper{
private:
std::string inputLabelTopic_;
std::string outputCloudTopic_;
std::string laserTopic_;
ros::NodeHandle nh_;
ros::NodeHandle local_nh_;
ros::Subscriber labelSub_;
ros::Subscriber laserSub_;
ros::Publisher cloudPub_;
ros::Publisher oneLabel_pub_;
sensor_msgs::Image image_;
ros::ServiceServer save_image_srv;
tf::TransformListener *tf_listener_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr label_cloud_ptr_;
bool send_semantic_cloud;
laser_geometry::LaserProjection projector_;
int current_color[3];
ros::Time label_time;
message_filters::Cache<sensor_msgs::LaserScan> cache;
SemMap *semMap;
double cell_resolution_;
double sem_ProbHit_;
double sem_ProbMiss_;
double prob_thres_;
public:
SemanticMapper(
ros::NodeHandle nh,
ros::NodeHandle local_nh
):
local_nh_(local_nh),
nh_(nh)
{
tf_listener_ = new tf::TransformListener(local_nh_, ros::Duration(100));
local_nh_.param<std::string>("label_topic",inputLabelTopic_,"/semantic_label");
local_nh_.param<std::string>("output_cloud",outputCloudTopic_,"/semantic_mapper/cloud");
local_nh_.param<std::string>("laser_topic",laserTopic_,"/scan");
local_nh_.param<double>("prob_thres",prob_thres_,0.5);
labelSub_ = nh_.subscribe(inputLabelTopic_, 1,&SemanticMapper::label_CB,this);
laserSub_ = nh_.subscribe(laserTopic_, 1,&SemanticMapper::scanCallback,this);
cloudPub_ = nh_.advertise<sensor_msgs::PointCloud2> (outputCloudTopic_,1);
save_image_srv = local_nh_.advertiseService("get_semantic_map",&SemanticMapper::get_sem_map_CB_, this);
oneLabel_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("oneLabel_cloud", 30);
send_semantic_cloud = false;
current_color[0] = 0;
current_color[1] = 0;
current_color[2] = 0;
cache.setCacheSize(1000);
cell_resolution_ = 0.1;
semMap = new SemMap(cell_resolution_);
semMap->setClampingThresMax(1.0);
semMap->setOccupancyThres(0);
}
bool get_sem_map_CB_(semantic_mapper::GetSemMap::Request& req,semantic_mapper::GetSemMap::Response& res){
std::ofstream outFile;
outFile.open("~/semMap.txt");
std::stringstream ss;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pOut(new pcl::PointCloud<pcl::PointXYZRGB>());
std::cout << "receive request for label_id " << req.label_id << "\n";
for( SemMap::iterator itr = semMap->begin_leafs(), end = semMap->end_leafs(); itr != end; itr++){
ss << itr.getCoordinate().x() <<" "<<itr.getCoordinate().y()<<" "<<itr.getCoordinate().z()<<" ";
SemCell *cell0 = semMap->search(itr.getCoordinate());
CellColor c = cell0->getColor();
uint8_t r = (uint8_t)c.r;
uint8_t g = (uint8_t)c.g;
uint8_t b = (uint8_t)c.b;
this->changeSaturation(r,g,b, cell0->getOccupancy());
ss << cell0->getOccupancy() <<" "<<(int)r<<" "<<(int)g<<" "<< (int)b <<"\n";
if (int(itr.getCoordinate().z()) == (req.label_id-1) ){
SemCell *cell = semMap->search(itr.getCoordinate());
pcl::PointXYZRGB pcl_point;
pcl_point.x = itr.getCoordinate().x();pcl_point.y = itr.getCoordinate().y();pcl_point.z = cell->getOccupancy();
CellColor c = cell->getColor();
pcl_point.r = c.r; pcl_point.g = c.g; pcl_point.b = c.b;
this->changeSaturation(pcl_point.r,pcl_point.g,pcl_point.b, cell->getOccupancy());
pOut->push_back(pcl_point);
}
}