1. Particles
stay SLAM GMapping (4) SLAM processor Each particle updated by particle filter independently records a possible robot trajectory and environment map
In order to improve efficiency, GMapping designs a data structure of trajectory tree
The trajectory of the robot can be obtained by tracing from the leaf node to the root node
In openslam_ gmapping/include/gmapping/gridfastslam/gridslamprocessor. Particle structure in H
/* This class defines the particles of the filter. Each particle has a mapping, a pose, and a weight, and retains the current node in the track tree */
struct Particle
{
/* Construct a particle, give a map, parameter map: particle map */
Particle(const ScanMatcherMap &map);
/* Returns the weight of the particle */
inline operator double() const { return weight; }
/* Returns the pose of particles */
inline operator OrientedPoint() const { return pose; }
/* Set particle weights, parameter w: weights */
inline void setWeight(double w) { weight = w; }
/* Map */
ScanMatcherMap map;
/* Posture */
OrientedPoint pose;
/* Position and attitude of the robot in the previous time period (used to calculate odometer displacement) */
OrientedPoint previousPose;
/* weight */
double weight;
/* Cumulative weight */
double weightSum;
double gweight;
/* The index of the previous particle in the track tree */
int previousIndex;
/* Pointer entry of track tree */
TNode *node;
};
Constructor initializes various member variables and gives initial values
/**
* @brief Particle construction function
*
* @param m Laser map object
*/
GridSlamProcessor::Particle::Particle(const ScanMatcherMap &m) : map(m), pose(0, 0, 0), weight(0), weightSum(0), gweight(0), previousIndex(0)
{
node = 0;
}
The TNode object indicated by the pointer can be traced to the root node of the track tree
The node experienced during this period is a possible trajectory recorded by the current particle
2. Track
The definition of TNode type of track tree node, like Particle, is also the definition of structure form
struct TNode
{
/* Node for constructing track tree */
TNode(const OrientedPoint &pose, double weight, TNode *parent = 0, unsigned int childs = 0);
/* Destroy tree nodes and continuously update the tree. If only one child of the parent node is deleted, the parent node will also be deleted. This is because the parent node no longer exists in the track tree. */
~TNode();
/* Posture */
OrientedPoint pose;
/* weight */
double weight;
/* The sum of all particle weights is the weight accumulation of the previous node of the track */
/* The sum of particle weights at a node of the track */
double accWeight;
double gweight;
/* Parent node */
TNode *parent;
/* The reading range associated with this node, i.e. recording laser sensor data */
const RangeReading *reading;
/* Number of child nodes */
unsigned int childs;
/* Counters for accessing nodes (internal use) */
mutable unsigned int visitCounter;
/* Access flag (internal use) */
mutable bool flag;
};
Constructor initializes various member variables and gives initial values
/**
* @brief Particle trajectory node construction function
*
* @param p Particle pose
* @param w Particle weight
* @param n The parent node of the node to be constructed
* @param c The number of child nodes of this node
*/
GridSlamProcessor::TNode::TNode(const OrientedPoint &p, double w, TNode *n, unsigned int c)
{
pose = p; /* Particle pose settings */
weight = w; /* Particle weight settings */
childs = c; /* Setting the number of child nodes */
parent = n; /* Parent node settings */
reading = 0; /* Record laser sensor data initialization */
gweight = 0;
/* Increase the number of child nodes of the parent node */
if (n)
{
n->childs++;
}
flag = 0; /* Access flag initialization */
accWeight = 0; /* Weight sum initialization */
}
During deconstruction, you need to pay attention to releasing resources
/**
* @brief Particle trajectory node destructor
*/
GridSlamProcessor::TNode::~TNode()
{
/* Determine whether the parent node exists, reduce the number of child nodes of the parent node, and release resources */
if (parent && (--parent->childs) <= 0)
delete parent;
assert(!childs);
}
3. Update track weight
In particle filter, the weight of particles has been preliminarily updated in the second step of scan matching
Step 3: update the particle track weights updateTreeWeights
The parameter weightsAlreadyNormalized describes whether the particle weights have been normalized
Function calls three functions to complete three tasks: normalizing particle weight, resetting trajectory tree and updating trajectory tree weight
/**
* @brief Update track weight function
*
* @param weightsAlreadyNormalized Weight normalized
*/
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
/* Ensure ownership re unification */
if (!weightsAlreadyNormalized)
{
normalize();
}
/* Reset all track trees */
resetTree();
/* Update all track tree weights */
propagateWeights();
}
3.1. Particle weight normalization
In openslam_ gmapping/include/gmapping/gridfastslam/gridslamprocessor. normalize function defined in hxx
In addition to normalizing all particle weights, the weight similarity neff is also calculated
/**
* @brief Particle weight renormalization function
*
* Normalize all particle weights and calculate weight similarity
*/
inline void GridSlamProcessor::normalize()
{
/* Calculate the weight gain of particles = 1 / (smoothing factor of likelihood * size) */
double gain = 1. / (m_obsSigmaGain * m_particles.size());
/* Calculates the largest weight in the particle collection */
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;
}
m_weights.clear(); /* Particle weight initialization clear */
double wcum = 0; /* Cumulative weight initialization */
m_neff = 0; /* Similarity initialization */
/* Traverse the particle swarm and update all particle weight ranges */
for (std::vector<Particle>::iterator it = m_particles.begin(); it != m_particles.end(); it++)
{
m_weights.push_back(exp(gain * (it->weight - lmax))); /* Update weight in range [0,1] */
wcum += m_weights.back(); /* Cumulative summation weight */
}
m_neff = 0; /* Similarity initialization */
/* Traverse particle weights, update all particle weights, and normalize them */
for (std::vector<double>::iterator it = m_weights.begin(); it != m_weights.end(); it++)
{
*it = *it / wcum; /* The normalized particle weight is obtained by dividing the weight by the weight sum */
/* Reciprocal of cumulative weight similarity */
double w = *it;
m_neff += w * w; /* m_neff Will always be less than 1. If all w are close, then m_neff will be smaller, and it will be larger after the countdown, indicating that the weight gap is small */
}
m_neff = 1. / m_neff; /* Calculate weight similarity */
}
3.2. Reset track tree
In openslam_ gmapping/gridfastslam/gridslamprocessor_ tree. resetTree function defined in CPP
That is, clear the cumulative weight and access count of each node
/**
* @brief Reset all trace tree functions
*
* It traverses all particles, the cumulative weight and access count of each track node
*/
void GridSlamProcessor::resetTree()
{
// don't calls this function directly, use updateTreeWeights(..) !
/* Instead of calling this function directly, use updateTreeWeights */
/* Traversed all the particles */
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++)
{
/* Trace up to the root node along the node node of the particle, and clear the cumulative weight and access count of each node in the traversal process */
TNode *n = it->node;
while (n)
{
n->accWeight = 0;
n->visitCounter = 0;
n = n->parent;
}
}
}
3.3. Update track tree weights
In openslam_ gmapping/gridfastslam/gridslamprocessor_ tree. The propagateWeights function is defined in CPP
/**
* @brief Update all track tree weight functions
*
* @return lastNodeWeight The cumulative weight of the root node of the particle swarm's trajectory
*/
double GridSlamProcessor::propagateWeights()
{
double lastNodeWeight = 0; /* The cumulative weight of the root node of the particle swarm's trajectory */
double aw = 0; /* Sum of particle swarm trajectory and leaf node weight */
/* A leaf node that defines the weights of all (the latest pose of the particle swarm) */
std::vector<double>::iterator w = m_weights.begin();
/* Ergodic particle swarm optimization */
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++)
{
/* Get leaf node weights and accumulate */
double weight = *w;
aw += weight;
TNode *n = it->node; /* Gets the leaf node of the current particle's new track */
n->accWeight = weight; /* Sets the initial cumulative weight of the leaf node */
lastNodeWeight += propagateWeight(n->parent, n->accWeight); /* Find the root node of the track from the leaf node, update the access count and cumulative weight of the nodes on the track, and obtain the weight of the root node */
w++; /* The weight entry of leaf node (the latest pose of particle swarm) changes with the traversal of particle swarm */
}
/* The weight sum of leaf and root nodes of all particle trajectories should be 1, because they belong to the weight sum of a data update */
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);
}
/* Returns the cumulative weight of the particle swarm's trajectory root node */
return lastNodeWeight;
}
The propagateWeight function is called.
It is used to find the root node of the track from the leaf node, update the access count and cumulative weight of the nodes on the track, and obtain the weight of the root node
/**
* @brief Get the weight function of root node of single track tree
*
* Accumulate the access count and weight of the node of the current track tree, and return the weight of the root node of the current track tree
*
* @param n Node of track tree
* @param weight Cumulative weight of the current track tree
* @return w The weight of the root node of the current track tree
*/
double propagateWeight(GridSlamProcessor::TNode *n, double weight)
{
if (!n)
return weight; /* Returns the node weight of the current track tree */
double w = 0;
n->visitCounter++; /* Cumulative count of accesses to the current node */
n->accWeight += weight; /* Accumulate the weight of the current node */
/* Recursive query root node weight */
if (n->visitCounter == n->childs)
{
w = propagateWeight(n->parent, n->accWeight);
}
assert(n->visitCounter <= n->childs);
return w;
}
thank you