这里只截取其中的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:"<<m_linearDistance<<endl;
// cerr <<"angular Distance:"<<m_angularDistance<<endl;
// 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); // setiosflags 意思就是设置输入输出的标志
// iso::fixed 是操作符setiosflags 的参数之一,该参数指定的动作是以带小数点的形式表示浮点数,并且在允许的精度范围内尽可能的把数字移向小数点右侧;
// ios::right是 右对齐
// setprecision(6)是设置数字的精度为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<<endl;
// m_infoStream <<"Scan-Match Number: "<<m_count<<endl;
// }
// 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<<"********************************************"<<endl;
// cerr<<"********************************************"<<endl;
// cerr<<"reading_size:"<<reading.getSize()<<" m_beams:"<<m_beams<<endl;
// cerr<<"********************************************"<<endl;
// cerr<<"********************************************"<<endl;
// }
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!" <<endl;
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;
}
std::ofstream& GridSlamProcessor::outputStream(){
return m_outputStream;
}
std::ostream& GridSlamProcessor::infoStream(){
return m_infoStream;
}