Gmapping Slam主要代码分析
目录
main主函数
startLiveSlam函数
laserCallback激光处理回调函数
initMapper初始化地图函数
addScan激光数据处理函数
processScan函数
drawFromMotion运动模型数据更新函数
scanMatch扫描匹配函数
optimize位姿优化函数
score函数
likelihoodAndScore函数
invalidateActiveArea函数
computeActiveArea计算有效区域函数
updateTreeWeights更新粒子权重函数
normalize归一化函数
resetTree重置粒子树函数
propagateWeights计算权重函数
registerScan地图状态更新函数
resample粒子重采样函数
updateMap地图更新函数
Gmapping主要流程
main函数
#include startLiveSlam函数
/*订阅一些主题 发布一些主题*/
void SlamGMapping::startLiveSlam()
{
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
{
//订阅激光数据 同时和odom_frame之间的转换同步
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;
}
/*发布转换关系的线程*/
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
laserCallback函数
/*
* 接受到激光雷达数据的回调函数 在这里面调用addScan()函数
* 如果addScan()函数调用成功，也就是说激光数据被成功的插入到地图中后，
* 如果到了地图更新的时间，则对地图进行更新，通过调用updateMap()函数来进行相应的操作。
*
* laserCallback()->addScan()->gmapping::processScan()
* ->updateMap()
*
*/
void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))//进入激光数据处理函数
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
/*如果没有地图那肯定需要直接更新，如果有地图了则需要到时间了，才更新地图了*/
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
/*多久更新一次地图*/
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
}
else
ROS_DEBUG("cannot process scan");
}
initMapper函数
/**
* @brief SlamGMapping::initMapper gmapping算法的初始化
* 这个函数在收到的第一帧激光雷达数据的时候会被调用一次，之后就再也不会被调用了。
* 这个函数的功能主要就是对gmapping算法中需要的一些参数进行赋值，即对gmapping算法进行初始化
* 这个参数一开始由gmapping wrapper读取，但是在这个函数里面真正的会赋值给gmapping算法
* 1.判断激光雷达是否是水平放置的，如果不是 则报错。
* 2.假设激光雷达数据的角度是对称的 & 递增的 为每个激光束分配角度。
* 3.为gmapping算法设置各种需要的参数。
*
*
* @param scan
* @return
*/
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
//得到激光雷达相对于车身坐标系(base_link)的位姿
laser_frame_ = scan.header.frame_id;
// Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;
ident.stamp_ = scan.header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
// create a point 1m above the laser position and transform it into the laser-frame
// 创造一个比激光雷达高1m的一个点，然后把这个点转换到激光雷达坐标系 这个点主要用来检测激光雷达和车身的位姿关系
// 来判断激光雷达是否是倾斜的 判断条件：如果激光雷达是水平放置的，那么转换回去之后，z的坐标也依然是1
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
base_frame_);
try
{
tf_.transformPoint(laser_frame_, up, up);
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to determine orientation of laser: %s",
e.what());
return false;
}
// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
// 如果激光雷达不是水平放置的，则会进行报错。
if (fabs(fabs(up.z()) - 1) > 0.001)
{
ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
up.z());
return false;
}
//激光雷达的数量
gsp_laser_beam_count_ = scan.ranges.size();
//激光雷达的中间角度
double angle_center = (scan.angle_min + scan.angle_max)/2;
//判断激光雷达是正着放置的还是反着放置的。
if (up.z() > 0)
{
do_reverse_range_ = scan.angle_min > scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upwards.");
}
else
{
do_reverse_range_ = scan.angle_min < scan.angle_max;
centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
ROS_INFO("Laser is mounted upside down.");
}
// Compute the angles of the laser from -x to x, basically symmetric and in increasing order
// 这里认为激光雷达数据的角度是对称的，且是从小到大递增的。
// 因此根据激光雷达数据中的angle_min,angle_max,angle_increment等数据为每个激光束分配角度。
laser_angles_.resize(scan.ranges.size());
// Make sure angles are started so that they are centered
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
laser_angles_[i]=theta;
theta += std::fabs(scan.angle_increment);
}
ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
scan.angle_increment);
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
laser_angles_.back(), std::fabs(scan.angle_increment));
GMapping::OrientedPoint gmap_pose(0, 0, 0);
// setting maxRange and maxUrange here so we can set a reasonable default
// 设置激光雷达的最大观测距离和最大使用距离
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment. If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
// 根据上面得到的激光雷达的数据信息 来初始化一个激光传感器
gsp_laser_ = new GMapping::RangeSensor("FLASER",
gsp_laser_beam_count_,
fabs(scan.angle_increment),
gmap_pose,
0.0,
maxRange_);
ROS_ASSERT(gsp_laser_);
//为gmapping算法设置sensormap
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
//初始化里程计传感器
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);
/// @todo Expose setting an initial pose
/// 得到里程计的初始位姿，如果没有 则把初始位姿设置为(0,0,0)
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
{
ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}
//为gmapping算法设置各种参数
gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
kernelSize_, lstep_, astep_, iterations_,
lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange. It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);
// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");
return true;
}
addScan激光数据处理函数
/*
* 加入一个激光雷达的数据 主要函数 这里面会调用processScan()函数
* 这个函数被laserCallback()函数调用
*/
bool SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
//得到与激光的时间戳相对应的机器人的里程计的位姿
if(!getOdomPose(gmap_pose, scan.header.stamp))
return false;
//检测是否所有帧的数据都是相等的 如果不相等就进行计算 不知道为什么 感觉完全没必要啊。
//特别是对于champion_nav_msgs的LaserScan来说 激光束的数量经常变
if(scan.ranges.size() != gsp_laser_beam_count_)
return false;
// GMapping wants an array of doubles...
double* ranges_double = new double[scan.ranges.size()];
// If the angle increment is negative, we have to invert the order of the readings.
// 如果激光是反着装的，这激光的顺序需要反过来，同时这里会排除掉所有激光距离小于range_min的值。
// 排除的方式是把他们设置为最大值
if (do_reverse_range_)
{
ROS_DEBUG("Inverting scan");
int num_ranges = scan.ranges.size();
for(int i=0; i < num_ranges; i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[num_ranges - i - 1] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
}
}
else
{
for(unsigned int i=0; i < scan.ranges.size(); i++)
{
// Must filter out short readings, because the mapper won't
if(scan.ranges[i] < scan.range_min)
ranges_double[i] = (double)scan.range_max;
else
ranges_double[i] = (double)scan.ranges[i];
}
}
//把ROS的激光雷达数据信息 转换为 GMapping算法看得懂的形式
GMapping::RangeReading reading(scan.ranges.size(),
ranges_double,
gsp_laser_,
scan.header.stamp.toSec());
// ...but it deep copies them in RangeReading constructor, so we don't
// need to keep our array around.
// 上面的初始话是进行深拷贝 因此申请的内存可以直接释放。
delete[] ranges_double;
//设置和激光数据的时间戳匹配的机器人的位姿
reading.setPose(gmap_pose);
ROS_DEBUG("processing scan");
//调用gmapping算法进行处理
return gsp_->processScan(reading);
}
processScan函数
/*
* 在scanmatcherprocessor里面也有一个这样的函数 但是那个函数是没有使用的
@desc 处理一帧激光数据 这里是gmapping算法的主要函数。
在这个函数里面调用其他的函数，包括里程计预测、激光测量更新、粒子采样等等步骤。
主要步骤如下：
利用运动模型更新里程计分布
利用最近的一次观测来提高proposal分布。(scan-match)
利用proposal分布+激光雷达数据来确定各个粒子的权重
对粒子进行重采样
*/
bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
{
/**retireve the position from the reading, and compute the odometry*/
/*得到当前的里程计的位置*/
OrientedPoint relPose=reading.getPose();
//relPose.y = m_odoPose.y;
/*m_count表示这个函数被调用的次数 如果是第0次调用,则所有的位姿都是一样的*/
if (!m_count)
{
m_lastPartPose=m_odoPose=relPose;
}
//write the state of the reading and update all the particles using the motion model
/*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置 这一步对应于 里程计的更新 */
int tmp_size = m_particles.size();
//这个for循环显然可以用OpenMP进行并行化
//#pragma omp parallel for
for(int i = 0; i < tmp_size;i++)
{
OrientedPoint& pose(m_particles[i].pose);
pose = m_motionModel.drawFromMotion(m_particles[i],relPose,m_odoPose);
}
//invoke the callback
/*回调函数 实际上什么都没做*/
onOdometryUpdate();
// accumulate the robot translation and rotation
/*根据两次里程计的数据 计算出来机器人的线性位移和角度位移的累积值 m_odoPose表示上一次的里程计位姿 relPose表示新的里程计的位姿*/
OrientedPoint move=relPose-m_odoPose;
move.theta=atan2(sin(move.theta), cos(move.theta));
//统计机器人在进行激光雷达更新之前 走了多远的距离 以及 平移了多少的角度
m_linearDistance+=sqrt(move*move);
m_angularDistance+=fabs(move.theta);
// cerr <<"linear Distance:"<
// cerr <<"angular Distance:"<
// if the robot jumps throw a warning
/*
* 如果机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新
* 则需要进行报警。这个误差很可能是里程计或者激光雷达的BUG造成的。
* 例如里程计数据出错 或者 激光雷达很久没有数据等等
* 每次进行激光雷达的更新之后 m_linearDistance这个参数就会清零
*/
if (m_linearDistance>m_distanceThresholdCheck)
{
cerr << "***********************************************************************" << endl;
cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y
<< " " <<m_odoPose.theta << endl;
cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y
<< " " <<relPose.theta << endl;
cerr << "***********************************************************************" << endl;
cerr << "** The Odometry has a big jump here. This is probably a bug in the **" << endl;
cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
cerr << "***********************************************************************" << endl;
}
//更新 把当前的位置赋值给旧的位置
m_odoPose=relPose;
bool processed=false;
// process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
/*只有当机器人走过一定的距离 或者 旋转过一定的角度 或者过一段指定的时间才处理激光数据*/
if (! m_count
|| m_linearDistance>=m_linearThresholdDistance
|| m_angularDistance>=m_angularThresholdDistance
|| (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_))
{
last_update_time_ = reading.getTime();
std::cout <<std::endl<<"Start to Process Scan##################"<<std::endl;
if (m_outputStream.is_open())
{
m_outputStream << setiosflags(ios::fixed) << setprecision(6);
m_outputStream << "FRAME " << m_readingCount;
m_outputStream << " " << m_linearDistance;
m_outputStream << " " << m_angularDistance << endl;
}
// if (m_infoStream)
// {
// m_infoStream << "update frame " << m_readingCount << endl
// << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
// m_infoStream << "FRAME " << m_readingCount<
// m_infoStream <<"Scan-Match Number: "<
// }
// cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y
// << " " << reading.getPose().theta << endl;
//this is for converting the reading in a scan-matcher feedable form
/*复制一帧数据 把激光数据转换为scan-match需要的格式*/
// if(reading.getSize() != m_beams)
// {
// cerr<<"********************************************"<
// cerr<<"********************************************"<
// cerr<<"reading_size:"<
// cerr<<"********************************************"<
// cerr<<"********************************************"<
// }
int beam_number = reading.getSize();
double * plainReading = new double[beam_number];
for(unsigned int i=0; i<beam_number; i++)
{
plainReading[i]=reading.m_dists[i];
}
//这个备份主要是用来储存的。
RangeReading* reading_copy;
//champion_nav_msgs激光数据
if(reading.m_angles.size() == reading.m_dists.size())
{
reading_copy = new RangeReading(beam_number,
&(reading.m_dists[0]),
&(reading.m_angles[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
}
//ros的激光数据
else
{
reading_copy = new RangeReading(beam_number,
&(reading.m_dists[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
}
/*如果不是第一帧数据*/
if (m_count>0)
{
/*
为每个粒子进行scanMatch，计算出来每个粒子的最优位姿，同时计算改最优位姿的得分和似然 对应于gmapping论文中的用最近的一次测量计算proposal的算法
这里面除了进行scanMatch之外，还对粒子进行了权重的计算，并计算了粒子的有效区域 但不进行内存分配 内存分配在resample()函数中
这个函数在gridslamprocessor.hxx里面。
*/
scanMatch(plainReading);
//至此 关于proposal的更新完毕了，接下来是计算权重
onScanmatchUpdate();
/*
由于scanMatch中对粒子的权重进行了更新，那么这个时候各个粒子的轨迹上的累计权重都需要重新计算
这个函数即更新各个粒子的轨迹上的累计权重是更新
GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
*/
updateTreeWeights(false);
/*
* 粒子重采样 根据neff的大小来进行重采样 不但进行了重采样，也对地图进行更新
* GridSlamProcessor::resample 函数在gridslamprocessor.hxx里面实现
*/
std::cerr<<"plainReading:"<<m_beams<<std::endl;
resample(plainReading, adaptParticles, reading_copy);
}
/*如果是第一帧激光数据*/
else
{
//如果是第一帧数据，则可以直接计算activeArea。因为这个时候，对机器人的位置是非常确定的，就是(0,0,0)
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
m_matcher.registerScan(it->map, it->pose, plainReading);
//m_matcher.registerScan(it->lowResolutionMap,it->pose,plainReading);
//为每个粒子创建路径的第一个节点。该节点的权重为0,父节点为it->node(这个时候为NULL)。
//因为第一个节点就是轨迹的根，所以没有父节点
TNode* node=new TNode(it->pose, 0., it->node, 0);
node->reading = reading_copy;
it->node=node;
}
}
// cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
//进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重
//GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) 函数在gridslamprocessor_tree.cpp里面实现
updateTreeWeights(false);
// cerr << ".done!" <
delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration
//机器人累计行走的多远的路程没有进行里程计的更新 每次更新完毕之后都要把这个数值清零
m_linearDistance=0;
m_angularDistance=0;
m_count++;
processed=true;
//keep ready for the next step
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
it->previousPose=it->pose;
}
}
m_readingCount++;
return processed;
}
drawFromMotion运动模型数据更新函数
/*
@desc 里程计运动模型
@p 表示粒子估计的最优位置(机器人上一个时刻的最优位置)
@pnew 表示里程计算出来的新的位置
@pold 表示里程计算出来的旧的位置(即上一个里程计的位置)
*/
OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
double sxy=0.3*srr;
/*
* 计算出pnew 相对于 pold走了多少距离
* 这里的距离表达是相对于车身坐标系来说的。
*/
OrientedPoint delta=absoluteDifference(pnew, pold);
/*初始化一个点*/
OrientedPoint noisypoint(delta);
/*走过的X轴方向的距离加入噪声*/
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
/*走过的Y轴方向的距离加入噪声*/
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
/*走过的Z轴方向的距离加入噪声*/
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
/*限制角度的范围*/
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
if (noisypoint.theta>M_PI)
noisypoint.theta-=2*M_PI;
/*把加入了噪声的值 加到粒子估计的最优的位置上 即得到新的位置(根据运动模型推算出来的位置)*/
return absoluteSum(p,noisypoint);
}
scanMatch扫描匹配函数
/**Just scan match every single particle.
If the scan matching fails, the particle gets a default likelihood.
@desc 为每一个粒子都计算扫描匹配。扫描匹配即为在里程计的基础上，通过优化求得位姿
这个函数是最终被使用的函数
这个函数不但对每个位姿进行scan-match来优化，再优化之后，还会计算每个粒子的权重
这里的权重用似然表示。
*/
inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
// sample a new pose from each scan in the reference
/*每个粒子都要进行scan-match*/
double sumScore=0;
int particle_number = m_particles.size();
//用openMP的方式来进行并行化，因此这里不能用迭代器 只能用下标的方式进行访问
//并行话之后会把里面的循环均匀的分到各个不同的核里面去。
/* 计算最优的粒子
optimize 调用了 score 这个函数 （计算粒子得分）
在score 函数里，首先计算障碍物的坐标phit，然后将phit转换成网格坐标iPhit
计算光束上与障碍物相邻的非障碍物网格坐标pfree,pfrree由phit沿激光束方向移动一个网格的距离得到，将pfree转换成网格坐标ipfree（增量，并不是实际值）
在iphit 及其附近8个（m_kernelSize:default=1）栅格（pr,对应自由栅格为pf）搜索最优可能是障碍物的栅格。
最优准则： pr 大于某一阈值，pf小于该阈值，且pr栅格的phit的平均坐标与phit的距离bestMu最小。
得分计算： s +=exp(-1.0/m_gaussianSigma*bestMu*besMu) 参考NDT算法,距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型
至此 score 函数结束并返回粒子（currentPose）得分，然后回到optimize函数
optimize 干的事就是 currentPose 的位姿进行微调，前、后、左、右、左转、右转 共6次，然后选取得分最高的位姿，返回最终的得分
*/
//#pragma omp parallel for
for (int i = 0; i < particle_number;i++)
{
OrientedPoint corrected;
double score, l, s;
/*进行scan-match 计算粒子的最优位姿 调用scanmatcher.cpp里面的函数 --这是gmapping本来的做法*/
score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);
/*矫正成功则更新位姿*/
if (score>m_minimumScore)
{
m_particles[i].pose = corrected;
}
/*扫描匹配不上 则使用里程计的数据 使用里程计数据不进行更新 因为在进行扫描匹配之前 里程计已经更新过了*/
else
{
//输出信息 这个在并行模式下可以会出现错位
if (m_infoStream)
{
m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
m_infoStream << "lp:" << m_lastPartPose.x << " " << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
}
}
//粒子的最优位姿计算了之后，重新计算粒子的权重(相当于粒子滤波器中的观测步骤，计算p(z|x,m))，粒子的权重由粒子的似然来表示。
/*
* 计算粒子的得分和权重(似然) 注意粒子的权重经过ScanMatch之后已经更新了
* 在论文中 例子的权重不是用最有位姿的似然值来表示的。
* 是用所有的似然值的和来表示的。
*/
m_matcher.likelihoodAndScore(s, l, m_particles[i].map, m_particles[i].pose, plainReading);
sumScore+=score;
m_particles[i].weight+=l;
m_particles[i].weightSum+=l;
//set up the selective copy of the active area
//by detaching the areas that will be updated
/*计算出来最优的位姿之后，进行地图的扩充 这里不会进行内存分配
*不进行内存分配的原因是这些粒子进行重采样之后有可能会消失掉，因此在后面进行冲采样的时候统一进行内存分配。
*理论上来说，这里的操作是没有必要的，因为后面的重采样的时候还会进行一遍
*/
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading);
}
if (m_infoStream)
m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
}
optimize位姿优化函数
/*
/ zq commit
@desc 根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来
这个函数是真正被调用来进行scan-match的函数
@param pnew 新的最优位置
@param map 地图
@param init 初始位置
@param readings 激光数据
*/
double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const
{
double bestScore=-1;
/*计算当前位置的得分*/
OrientedPoint currentPose=init;
double currentScore=score(map, currentPose, readings);
/*所有时的步进增量*/
double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
/*精确搜索的次数*/
unsigned int refinement=0;
/*搜索的方向*/
enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
//enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
int c_iterations=0;
do
{
/*如果这一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长 这个策略跟LM有点像*/
if (bestScore>=currentScore)
{
refinement++;
adelta*=.5;
ldelta*=.5;
}
bestScore=currentScore;
OrientedPoint bestLocalPose=currentPose;
OrientedPoint localPose=currentPose;
/*把8个方向都搜索一次 得到这8个方向里面最好的一个位姿和对应的得分*/
Move move=Front;
do
{
localPose=currentPose;
switch(move)
{
case Front:
localPose.x+=ldelta;
move=Back;
break;
case Back:
localPose.x-=ldelta;
move=Left;
break;
case Left:
localPose.y-=ldelta;
move=Right;
break;
case Right:
localPose.y+=ldelta;
move=TurnLeft;
break;
case TurnLeft:
localPose.theta+=adelta;
move=TurnRight;
break;
case TurnRight:
localPose.theta-=adelta;
move=Done;
break;
default:;
}
//计算当前的位姿和初始位姿的区别 区别越大增益越小
double odo_gain=1;
//计算当前位姿的角度和初始角度的区别 如果里程计比较可靠的话
//那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚
if (m_angularOdometryReliability>0.)
{
double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
odo_gain*=exp(-m_angularOdometryReliability*dth);
}
//计算线性距离的区别 线性距离也是一样
if (m_linearOdometryReliability>0.)
{
double dx=init.x-localPose.x;
double dy=init.y-localPose.y;
double drho=dx*dx+dy*dy;
odo_gain*=exp(-m_linearOdometryReliability*drho);
}
/*计算得分=增益*score*/
double localScore=odo_gain*score(map, localPose, readings);
/*如果得分更好，则更新*/
if (localScore>currentScore)
{
currentScore=localScore;
bestLocalPose=localPose;
}
c_iterations++;
} while(move!=Done);
/* 把当前位置设置为目前最优的位置 如果8个值都被差了的话，那么这个值不会更新*/
currentPose=bestLocalPose;
}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
/*返回最优位置和得分*/
pnew=currentPose;
return bestScore;
}
score函数
/*
@desc 根据地图、机器人位置、激光雷达数据，计算出一个得分：原理为likelihood_field_range_finder_model
这个函数被scanmatcher.cpp里面的optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings)
调用
@map 对应的地图
@p 机器人对应的初始位置
@readings 激光雷达数据
*/
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const
{
double s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;
OrientedPoint lp=p;
/*
把激光雷达的坐标转换到世界坐标系
先旋转到机器人坐标系，然后再转换到世界坐标系
p表示base_link在map中的坐标
m_laserPose 表示base_laser在base_link坐标系中的坐标
*/
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
/*
* map.getDelta表示地图分辨率 m_freeCellRatio = sqrt(2)
* 意思是如果激光hit了某个点 那么沿着激光方向的freeDelta距离的地方要是空闲才可以
*/
unsigned int skip=0;
double freeDelta=map.getDelta()*m_freeCellRatio;
//枚举所有的激光束
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
skip++;
skip=skip>m_likelihoodSkip?0:skip;
if (skip||*r>m_usableRange||*r==0.0) continue;
/*被激光雷达击中的点 在地图坐标系中的坐标*/
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint iphit=map.world2map(phit);
/*该束激光的最后一个空闲坐标，即紧贴hitCell的freeCell 原理为：假设phit是被激光击中的点，这样的话沿着激光方向的前面一个点必定的空闲的*/
Point pfree=lp;
//理论上来说 这个应该是一个bug。修改成下面的之后，改善不大。
// pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
// pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
pfree.x+=(*r - freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r - freeDelta)*sin(lp.theta+*angle);
/*phit 和 freeCell的距离*/
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
/*在kernelSize大小的窗口中搜索出最优最可能被这个激光束击中的点 这个kernelSize在大小为1*/
bool found=false;
Point bestMu(0.,0.);
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)
{
/*根据已开始的关系 搜索phit的时候，也计算出来pfree*/
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
/*得到各自对应的Cell*/
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
/*
(double)cell返回的是该cell被占用的概率
这束激光要合法必须要满足cell是被占用的，而fcell是空闲的
原理为：假设phit是被激光击中的点，这样的话沿着激光方向的前面一个点必定的空闲的
*/
if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
{
/*计算出被击中的点与对应的cell的currentScore距离*/
Point mu=phit-cell.mean();
if (!found)
{
bestMu=mu;
found=true;
}
else
{
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
}
}
/*socre的计算公式exp(-d^2 / sigma)) 这里的sigma表示方差 不是标准差*/
if (found)
{
double tmp_score = exp(-1.0/m_gaussianSigma*bestMu*bestMu);
s += tmp_score;
//只在周围的9个栅格里面寻找，因此平方的意义不大。
//s += tmp_score*tmp_score;
}
}
return s;
}
likelihoodAndScore函数
/*
@desc 根据地图、机器人位置、激光雷达数据，同时计算出一个得分和似然：原理为likelihood_field_range_finder_model
计算出来的似然即为粒子的权重
这个函数被scanmatcher.cpp里面的optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings)
调用
这个函数跟score是非常像的，不同的是这个函数除了计算的得分之外，还计算的似然likelihood
这个函数的计算出来的似然在gmapping中用来表示粒子的权重
@param s 得分
@param l 似然 粒子的权重
@param map 对应的地图
@param p 机器人对应的初始位置
@param readings 激光雷达数据
*/
inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const
{
using namespace std;
l=0;
s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;
/*
把激光雷达的坐标转换到世界坐标系
先旋转到机器人坐标系，然后再转换到世界坐标系
*/
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
//如果没有击中的时候的似然值 nullLikehood = -0.5
double noHit=nullLikelihood/(m_likelihoodSigma);
unsigned int skip=0;
unsigned int c=0;
double freeDelta=map.getDelta()*m_freeCellRatio;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
/*每隔m_likelihoodSkip个激光束 就跳过一个激光束*/
skip++;
skip=skip>m_likelihoodSkip?0:skip;
if (*r>m_usableRange) continue;
if (skip) continue;
/*被激光击中的点*/
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint iphit=map.world2map(phit);
Point pfree=lp;
pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
/*在对应的kernerSize中搜索*/
bool found=false;
Point bestMu(0.,0.);
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)
{
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
/*如果cell(pr)被占用 而cell(pf)没有被占用 则说明找到了一个合法的点*/
if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
{
Point mu=phit-cell.mean();
if (!found)
{
bestMu=mu;
found=true;
}
else
{
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
}
}
/*计算得分 得分是只计有用的激光束*/
if (found)
{
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
c++;
}
/*计算似然 似然是计算所有的激光束 如果某一个激光束打中了空地 那也需要计算进去*/
if (!skip)
{
//似然不是指数 似然只是指数的上标
double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
l+=(found)?f:noHit;
}
}
return c;
}
invalidateActiveArea函数
/**
* @brief ScanMatcher::invalidateActiveArea
* 每次调用computeActiveArea()之前，都必须要调用这个函数
*/
void ScanMatcher::invalidateActiveArea()
{
m_activeAreaComputed=false;
}
computeActiveArea计算有效区域函数
/*
@desc 计算有效区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。(这里只是计算出来栅格的位置，然后插入地图中,并不对数据进行更新)
这里计算的有效区域的坐标都是patch坐标，不是cell坐标
注意！！！！！：：
这个函数在正常的进行SLAM算法的过程中，使用了m_generateMap = false。这个时候不会为了空闲区域分配内存。
当要生成可视化的地图的时候,m_generateMap = true。这个时候就会为空闲区域分配内存
@param map 地图
@param p 机器人位置
@param readings 激光雷达数据
*/
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
{
/*已经计算过了，则没必要计算了*/
if (m_activeAreaComputed)
return;
/*把激光雷达的位置转换到地图坐标系*/
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
/*地图的范围*/
Point min(map.map2world(0,0));
Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
/*扩展地图的范围*/
if (lp.x<min.x) min.x=lp.x;
if (lp.y<min.y) min.y=lp.y;
if (lp.x>max.x) max.x=lp.x;
if (lp.y>max.y) max.y=lp.y;
/*determine the size of the area*/
/*根据激光数据扩展地图的范围*/
const double * angle=m_laserAngles+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
/*去除不合理的值*/
if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
//根据设置截断gmapping的值
double d=*r>m_usableRange?m_usableRange:*r;
/*被激光击中的位置*/
Point phit=lp;
phit.x+=d*cos(lp.theta+*angle);
phit.y+=d*sin(lp.theta+*angle);
/*扩充范围*/
if (phit.x<min.x) min.x=phit.x;
if (phit.y<min.y) min.y=phit.y;
if (phit.x>max.x) max.x=phit.x;
if (phit.y>max.y) max.y=phit.y;
}
//min=min-Point(map.getDelta(),map.getDelta());
//max=max+Point(map.getDelta(),map.getDelta());
/*如果地图需要扩展*/
if ( !map.isInside(min) || !map.isInside(max))
{
/*得到目前地图的大小*/
Point lmin(map.map2world(0,0));
Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
//cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
//cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
/*如果需要扩充，则把对应的维度扩展m_enlargeStep的大小*/
min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
map.resize(min.x, min.y, max.x, max.y);
//cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
}
/*地图的有效区域(地图坐标系)*/
HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
/*allocate the active area*/
angle=m_laserAngles+m_initialBeamsSkip;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
/*如果需要生成地图*/
if (m_generateMap)
{
//排除错误的激光点
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
/*激光束的起点和终点*/
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p0=map.world2map(lp);
IntPoint p1=map.world2map(phit);
/*bresenham算法来计算激光起点到终点要经过的路径*/
GridLineTraversalLine line;
line.points=m_linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
/*更新地图 把画线算法计算出来的值都算进去*/
for (int i=0; i<line.num_points-1; i++)
{
assert(map.isInside(m_linePoints[i]));
activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
}
//如果激光距离小于使用的值 则需要把定点也算进去 说明这个值是好的。
//同时如果d>=m_usableRange 那么说明这个值只用来进行标记空闲区域 不用来进行标记障碍物
if (d<m_usableRange)
{
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
}
else
{
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
IntPoint cp=map.storage().patchIndexes(p1);
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
}
}
map.storage().setActiveArea(activeArea, true);
m_activeAreaComputed=true;
}
updateTreeWeights更新粒子权重函数
/*
更新权重
调用这个函数的目的是因此
*/
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
if (!weightsAlreadyNormalized)
{
normalize();
}
resetTree();
propagateWeights();
}
normalize归一化函数
/*
@desc 把粒子的权重归一化
主要功能为归一化粒子的权重，同时计算出neff
*/
inline void GridSlamProcessor::normalize()
{
//normalize the log m_weights
double gain=1./(m_obsSigmaGain*m_particles.size());
/*求所有粒子中的最大的权重*/
double lmax= -std::numeric_limits<double>::max();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
lmax=it->weight>lmax?it->weight:lmax;
}
//cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl;
/*权重以最大权重为中心的高斯分布*/
m_weights.clear();
double wcum=0;
m_neff=0;
for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
m_weights.push_back(exp(gain*(it->weight-lmax)));
wcum+=m_weights.back();
//cout << "l=" << it->weight<< endl;
}
/*
计算有效粒子数 和 归一化权重
权重=wi/w
neff = 1/w*w
*/
m_neff=0;
for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++)
{
*it=*it/wcum;
double w=*it;
m_neff+=w*w;
}
m_neff=1./m_neff;
}
resetTree重置粒子树函数
/*把所有粒子的所有的轨迹中各个节点的accWeight清零*/
void GridSlamProcessor::resetTree()
{
// don't calls this function directly, use updateTreeWeights(..) !
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
TNode* n=it->node;
while (n)
{
n->accWeight=0;
n->visitCounter=0;
n=n->parent;
}
}
}
propagateWeights函数
/*
这个函数被updateTreeWeights()调用
*/
double GridSlamProcessor::propagateWeights()
{
// don't calls this function directly, use updateTreeWeights(..) !
// all nodes must be resetted to zero and weights normalized
// the accumulated weight of the root
// 求所有根节点的累计权重之和
double lastNodeWeight=0;
// sum of the weights in the leafs
// 所有叶子节点的权重 也就是m_weights数组里面所有元素的和
double aw=0;
std::vector<double>::iterator w=m_weights.begin();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
{
//求解所有叶子节点的累计权重
double weight=*w;
aw+=weight;
//叶子节点的子节点累计权重就等于自己的权重 因为它没有子节点
//每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径
TNode * n=it->node;
n->accWeight=weight;
/*
*
* 递归函数，
* accWeight)表示节点n的所有子节点的累计权重
* 迭代求一条路径上所有点的累计权重
* 节点的累计权重表示该节点的所有子节点的权重的和
*/
lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
w++;
}
if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001)
{
cerr << "ERROR: ";
cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl;
assert(0);
}
return lastNodeWeight;
}
registerScan函数
/*
@desc 已知位置的覆盖栅格地图算法(使用的模型为Counting Model)
Counting Model：某一个点被覆盖的概率为被占用的次数与被访问的次数的比值
@map 对应的地图
@p 机器人的位姿 机器人的位姿 不是激光雷达的位姿
@reading 激光雷达的数据
*/
double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
{
if (!m_activeAreaComputed)
computeActiveArea(map, p, readings);
//每次registerScan的时候都进行计算
computeActiveArea(map, p, readings);
//this operation replicates the cells that will be changed in the registration operation
//为activeArea里面的没有分配内存的区域分配内存
map.storage().allocActiveArea();
/*把激光雷达的位置转换到地图坐标系*/
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
IntPoint p0=map.world2map(lp);
const double * angle=m_laserAngles+m_initialBeamsSkip;
/*esum表示总体的熵的增加或者减少*/
double esum=0;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
{
if (m_generateMap)
{
/*去除非法的激光束*/
double d=*r;
if (d>m_laserMaxRange||d==0.0||isnan(d))
continue;
if (d>m_usableRange)
d=m_usableRange;
/*被该激光束击中的点的地图坐标*/
Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
IntPoint p1=map.world2map(phit);
//IntPoint linePoints[20000] ;
/*bresenham画线算法来计算 激光位置和被激光击中的位置之间的空闲位置*/
GridLineTraversalLine line;
line.points=m_linePoints;
GridLineTraversal::gridLine(p0, p1, &line);
/*更新空闲位置*/
for (int i=0; i<line.num_points-1; i++)
{
PointAccumulator& cell=map.cell(line.points[i]);
/*更新前的熵的负数*/
double e=-cell.entropy();
cell.update(false, Point(0,0));
/*加上更新后的熵，即更新后的熵减去更新前的熵，求得这次更新对于熵的变化*/
e+=cell.entropy();
esum+=e;
}
/*更新被击中的位置 只有小于m_usableRange的栅格来用来标记障碍物*/
if (d<m_usableRange)
{
double e=-map.cell(p1).entropy();
map.cell(p1).update(true, phit);
e+=map.cell(p1).entropy();
esum+=e;
}
}
else
{
if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
/*被击中的点的地图坐标*/
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint p1=map.world2map(phit);
assert(p1.x>=0 && p1.y>=0);
/*更新对应的cell的值*/
map.cell(p1).update(true,phit);
}
}
return esum;
}
resample粒子重采样函数
/*
@desc 粒子滤波器重采样。
分为两步：
1.需要重采样，则所有保留下来的粒子的轨迹都加上一个新的节点，然后进行地图更新。
2.不需要冲采样，则所有的粒子的轨迹都加上一个新的节点，然后进行地图的更新
在重采样完毕之后，会调用registerScan函数来更新地图
*/
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading)
{
bool hasResampled = false;
/*备份老的粒子的轨迹 即保留叶子节点 在增加新节点的时候使用*/
TNodeVector oldGeneration;
for (unsigned int i=0; i<m_particles.size(); i++)
{
oldGeneration.push_back(m_particles[i].node);
}
/*如果需要进行重采样*/
if (m_neff<m_resampleThreshold*m_particles.size())
{
if (m_infoStream)
m_infoStream << "*************RESAMPLE***************" << std::endl;
//采取重采样方法决定，哪些粒子会保留 保留的粒子会返回下标.里面的下标可能会重复，因为有些粒子会重复采样
//而另外的一些粒子会消失掉
uniform_resampler<double, double> resampler;
m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
if (m_outputStream.is_open())
{
m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++)
{
m_outputStream << *it << " ";
}
m_outputStream << std::endl;
}
onResampleUpdate();
//BEGIN: BUILDING TREE
//重采样之后的粒子
ParticleVector temp;
unsigned int j=0;
//要删除的粒子下标
std::vector<unsigned int> deletedParticles; //this is for deleteing the particles which have been resampled away.
//枚举每一个要被保留的粒子
for (unsigned int i=0; i<m_indexes.size(); i++)
{
//统计要被删除的粒子
while(j<m_indexes[i])
{
deletedParticles.push_back(j);
j++;
}
if (j==m_indexes[i])
j++;
//得到当前的保留的粒子
Particle & p=m_particles[m_indexes[i]];
//每一个需要保留下来的粒子都需要在路径中增加一个新的节点
TNode* node=0;
TNode* oldNode=oldGeneration[m_indexes[i]];
//创建一个新的节点 改节点的父节点为oldNode
node=new TNode(p.pose, 0, oldNode, 0);
node->reading=reading;
//这个要保留下来的粒子，要保留的粒子的下标为m_indexs
temp.push_back(p);
temp.back().node=node;
temp.back().previousIndex=m_indexes[i];
}
while(j<m_indexes.size())
{
deletedParticles.push_back(j);
j++;
}
//把要删除的粒子的Node都删除掉，Node表示轨迹的起点(最新的点)
std::cerr << "Deleting Nodes:";
for (unsigned int i=0; i<deletedParticles.size(); i++)
{
std::cerr <<" " << deletedParticles[i];
delete m_particles[deletedParticles[i]].node;
m_particles[deletedParticles[i]].node=0;
}
std::cerr << " Done" <<std::endl;
std::cerr << "Deleting old particles..." ;
std::cerr << "Done" << std::endl;
//清楚全部的粒子 然后从tmp中读取保留下来的粒子
m_particles.clear();
//枚举要保留下来的所有的粒子 每个粒子都需要更新地图
std::cerr << "Copying Particles and Registering scans...";
//对于保留下来的粒子进行更新 这里是可以并行化操作的。
//在并行化操作里面 m_particles.push_back()会报错 因此并行化 需要把push_back()提出来。
//在外面的的for循环进行
int tmp_size = temp.size();
//#pragma omp parallel for
for(int i = 0; i<tmp_size;i++)
{
//对保留下来的粒子数据进行更新
//每个粒子的权重都设置为相同的值
temp[i].setWeight(0);
//为每个粒子更新running_scans
//增加了一帧激光数据 因此需要更新地图
m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);
//m_matcher.registerScan(temp[i].lowResolutionMap,temp[i].pose,plainReading);
}
//提取出来 防止并行优化时报错
for(int i = 0; i< tmp_size;i++)
m_particles.push_back(temp[i]);
std::cerr << " Done" <<std::endl;
hasResampled = true;
}
/*否则的话，进行扫描匹配*/
else
{
//不进行重采样的话，权值不变。只为轨迹创建一个新的节点
//为每个粒子更新地图 同样可以并行化
int particle_size = m_particles.size();
//#pragma omp parallel for
for(int i = 0; i < particle_size;i++)
{
//创建一个新的树节点
TNode* node = 0;
node = new TNode(m_particles[i].pose,0.0,oldGeneration[i],0);
//把这个节点接入到树中
node->reading = reading;
m_particles[i].node = node;
//更新各个例子的地图
m_matcher.invalidateActiveArea();
m_matcher.registerScan(m_particles[i].map, m_particles[i].pose, plainReading);
m_particles[i].previousIndex = i;
}
std::cerr<<std::endl;
// int index=0;
// TNodeVector::iterator node_it=oldGeneration.begin();
// for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
// {
// //create a new node in the particle tree and add it to the old tree
// //新建一个树的节点。
// //BEGIN: BUILDING TREE
// TNode* node=0;
// node=new TNode(it->pose, 0.0, *node_it, 0);
// //node->reading=0;
// node->reading=reading;
// it->node=node;
// //END: BUILDING TREE
// //粒子的轨迹更新了之后，对应的地图也需要更新
// m_matcher.invalidateActiveArea();
// m_matcher.registerScan(it->map, it->pose, plainReading);
// it->previousIndex=index;
// index++;
// node_it++;
// }
std::cerr << "Done" <<std::endl;
}
updateMap地图更新函数
/*
更新地图:这里的更新地图，只是为了可视化。因为真正的地图都存储在粒子里面。
这里会拿一个权值最大的粒子的地图发布出来.
得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图
然后把得到的地图发布出去
这个函数被laserCallback()调用，每次addScan()成功了，就会调用这个函数来生成地图，并发布出去
*/
void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{
ROS_DEBUG("Update map");
boost::mutex::scoped_lock map_lock (map_mutex_);
GMapping::ScanMatcher matcher;
/*设置scanmatcher的各个参数*/
matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
gsp_laser_->getPose());
matcher.setlaserMaxRange(maxRange_);
matcher.setusableRange(maxUrange_);
matcher.setgenerateMap(true);
/*得到权值最高的粒子*/
GMapping::GridSlamProcessor::Particle best =
gsp_->getParticles()[gsp_->getBestParticleIndex()];
//发布位姿的熵
std_msgs::Float64 entropy;
entropy.data = computePoseEntropy();
if(entropy.data > 0.0)
entropy_publisher_.publish(entropy);
//如果没有地图 则初始化一个地图
if(!got_map_)
{
map_.map.info.resolution = delta_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
/*地图的中点*/
GMapping::Point center;
center.x=(xmin_ + xmax_) / 2.0;
center.y=(ymin_ + ymax_) / 2.0;
/*初始化一个scanmatcherMap 创建一个地图*/
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
delta_);
/*更新地图*/
//遍历粒子的整条轨迹 按照轨迹上各个节点存储的信息来重新计算一个地图
ROS_DEBUG("Trajectory tree:");
for(GMapping::GridSlamProcessor::TNode* n = best.node;n;n = n->parent)
{
ROS_DEBUG(" %.3f %.3f %.3f",
n->pose.x,
n->pose.y,
n->pose.theta);
if(!n->reading)
{
ROS_DEBUG("Reading is NULL");
continue;
}
//进行地图更新
//matcher.invalidateActiveArea();
//matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
//matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
matcher.registerScan(smap, n->pose, &(n->reading->m_dists[0]));
}
// the map may have expanded, so resize ros message as well
// 扩充地图的大小
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY())
{
// NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
// so we must obtain the bounding box in a different way
GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
xmin_ = wmin.x; ymin_ = wmin.y;
xmax_ = wmax.x; ymax_ = wmax.y;
ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
xmin_, ymin_, xmax_, ymax_);
map_.map.info.width = smap.getMapSizeX();
map_.map.info.height = smap.getMapSizeY();
map_.map.info.origin.position.x = xmin_;
map_.map.info.origin.position.y = ymin_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
}
//根据地图的信息计算出来各个点的情况:occ、free、noinformation
//这样对地图进行标记主要是方便用RVIZ显示出来
for(int x=0; x < smap.getMapSizeX(); x++)
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
/// 得到.xy被占用的概率
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
//unknown
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_UNKNOWN;
//占用
else if(occ > occ_thresh_)
{
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_OCC;
}
//freespace
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = GMAPPING_FREE;
}
}
//到了这一步，肯定是有地图了。
got_map_ = true;
//make sure to set the header information on the map
//把计算出来的地图发布出去
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
}