Commit 7cd01402 authored by Daniel Müller's avatar Daniel Müller
Browse files

clangformat file added and applied

parent 985a835f
---
Language: Cpp
# BasedOnStyle: WebKit
AccessModifierOffset: -4
AlignAfterOpenBracket: false
AlignEscapedNewlinesLeft: false
AlignOperands: false
AlignTrailingComments: false
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: Inline
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AlwaysBreakAfterDefinitionReturnType: false
AlwaysBreakBeforeMultilineStrings: false
AlwaysBreakTemplateDeclarations: false
BinPackArguments: true
BinPackParameters: true
BreakBeforeBinaryOperators: All
BreakBeforeBraces: Allman
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: true
ColumnLimit: 100
CommentPragmas: '^ IWYU pragma:'
ConstructorInitializerAllOnOneLineOrOnePerLine: false
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: false
DerivePointerAlignment: false
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
IndentCaseLabels: false
IndentWidth: 4
IndentWrappedFunctionNames: false
KeepEmptyLinesAtTheStartOfBlocks: true
MaxEmptyLinesToKeep: 2
NamespaceIndentation: Inner
ObjCBlockIndentWidth: 4
ObjCSpaceAfterProperty: true
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 60
PointerAlignment: Left
SpaceAfterCStyleCast: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 1
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Cpp11
TabWidth: 4
UseTab: Never
...
......@@ -34,131 +34,130 @@ class OccupancyMap;
class HyperSlamFilter
{
public:
/**
* This constructor initializes the random number generator and sets the
* member variables to the given values.
* @param particleNum Number of particleFilters to use.
*/
HyperSlamFilter(int particleFilterNum, int particleNum);
/**
* The destructor releases the OccupancyMap and the particles.
*/
~HyperSlamFilter();
/**
* This method runs the filter routine.
* The given odometry measurement is used as movement hypothesis, the
* laserData-vector is used
* as measurement and is used to weight the particles.
* @param currentPoseOdometry Odometry data of time t.
* @param laserData msg containing the laser measurement.
*/
void filter(Transformation2D trans, sensor_msgs::LaserScanConstPtr laserData);
/**
* Computes and sets the new value for m_Alpha1.
* @param percent Rotation error while rotating (see class constructor for
* details)
*/
void setRotationErrorRotating(float percent);
/**
* Computes and sets the new value for m_Alpha2.
* @param degreesPerMeter Rotation error while translating (see class
* constructor for details)
*/
void setRotationErrorTranslating(float degreesPerMeter);
/**
* Computes and sets the new value for m_Alpha3.
* @param percent Translation error while translating (see class constructor
* for details)
*/
void setTranslationErrorTranslating(float percent);
/**
* Computes and sets the new value for m_Alpha4.
* @param mPerDegree Translation error while rotating (see class constructor
* for details)
*/
void setTranslationErrorRotating(float mPerDegree);
/**
* Computes and sets the new value for m_Alpha5.
* @param mPerDegree Move jitter while turning (see class constructor for
* details)
*/
void setMoveJitterWhileTurning(float mPerDegree);
/**
* Sets whether the map is updated or just used for self-localization.
* @param doMapping True if robot shall do mapping, false otherwise.
*/
void setMapping(bool doMapping);
/**
* Deletes the current occupancy map and copies a new one into the system.
* @param occupancyMap The occupancy map to load into the system (copies are
* being made)
*/
void setOccupancyMap(OccupancyMap* occupancyMap);
/**
* Sets the robot pose in the current occupancy map.
* @param Robot pose.
* @param scatterVariance if not equal to 0 the poses are equally scattered
* around the pose
*/
void setRobotPose(Pose pose, double scatterVarXY = 0.0,
double scatterVarTheta = 0.0);
/**
*Returns the best SlamFilter
*/
SlamFilter* getBestSlamFilter();
void resetHigh();
/**
* Factor (default 0.98) of the contrast of the best particle.
* If the contrast of the worst particle is below this threshold
* it will be replaced by the best particle
* @param deletionThreshold see above
*/
void setDeletionThreshold(double deletionThreshold);
/**
* applies masking to map of slam filter set in GUI
* @param msg masking message received from GUI
*/
void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
for (unsigned i = 0; i < m_ParticleFilterNum; ++i)
/**
* This constructor initializes the random number generator and sets the
* member variables to the given values.
* @param particleNum Number of particleFilters to use.
*/
HyperSlamFilter(int particleFilterNum, int particleNum);
/**
* The destructor releases the OccupancyMap and the particles.
*/
~HyperSlamFilter();
/**
* This method runs the filter routine.
* The given odometry measurement is used as movement hypothesis, the
* laserData-vector is used
* as measurement and is used to weight the particles.
* @param currentPoseOdometry Odometry data of time t.
* @param laserData msg containing the laser measurement.
*/
void filter(Transformation2D trans, sensor_msgs::LaserScanConstPtr laserData);
/**
* Computes and sets the new value for m_Alpha1.
* @param percent Rotation error while rotating (see class constructor for
* details)
*/
void setRotationErrorRotating(float percent);
/**
* Computes and sets the new value for m_Alpha2.
* @param degreesPerMeter Rotation error while translating (see class
* constructor for details)
*/
void setRotationErrorTranslating(float degreesPerMeter);
/**
* Computes and sets the new value for m_Alpha3.
* @param percent Translation error while translating (see class constructor
* for details)
*/
void setTranslationErrorTranslating(float percent);
/**
* Computes and sets the new value for m_Alpha4.
* @param mPerDegree Translation error while rotating (see class constructor
* for details)
*/
void setTranslationErrorRotating(float mPerDegree);
/**
* Computes and sets the new value for m_Alpha5.
* @param mPerDegree Move jitter while turning (see class constructor for
* details)
*/
void setMoveJitterWhileTurning(float mPerDegree);
/**
* Sets whether the map is updated or just used for self-localization.
* @param doMapping True if robot shall do mapping, false otherwise.
*/
void setMapping(bool doMapping);
/**
* Deletes the current occupancy map and copies a new one into the system.
* @param occupancyMap The occupancy map to load into the system (copies are
* being made)
*/
void setOccupancyMap(OccupancyMap* occupancyMap);
/**
* Sets the robot pose in the current occupancy map.
* @param Robot pose.
* @param scatterVariance if not equal to 0 the poses are equally scattered
* around the pose
*/
void setRobotPose(Pose pose, double scatterVarXY = 0.0, double scatterVarTheta = 0.0);
/**
*Returns the best SlamFilter
*/
SlamFilter* getBestSlamFilter();
void resetHigh();
/**
* Factor (default 0.98) of the contrast of the best particle.
* If the contrast of the worst particle is below this threshold
* it will be replaced by the best particle
* @param deletionThreshold see above
*/
void setDeletionThreshold(double deletionThreshold);
/**
* applies masking to map of slam filter set in GUI
* @param msg masking message received from GUI
*/
void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
m_SlamFilters[i]->applyMasking(msg);
for (unsigned i = 0; i < m_ParticleFilterNum; ++i)
{
m_SlamFilters[i]->applyMasking(msg);
}
}
}
private:
/** Used SlamFilters */
std::vector<SlamFilter*> m_SlamFilters;
/** Used SlamFilters */
std::vector<SlamFilter*> m_SlamFilters;
/** Number of SlamFilters */
unsigned m_ParticleFilterNum;
/** Number of SlamFilters */
unsigned m_ParticleFilterNum;
/** Number of Particles of SlamFilter */
unsigned m_ParticleNum;
/** Number of Particles of SlamFilter */
unsigned m_ParticleNum;
/** */
double m_DeletionThreshold;
/** */
double m_DeletionThreshold;
/** Best SLAM Filter */
SlamFilter* m_BestSlamFilter;
/** Best SLAM Filter */
SlamFilter* m_BestSlamFilter;
/** Worst SlamFilter */
SlamFilter* m_WorstSlamFilter;
/** Worst SlamFilter */
SlamFilter* m_WorstSlamFilter;
bool m_DoMapping;
bool m_DoMapping;
};
#endif
......@@ -21,54 +21,45 @@
class Particle
{
public:
/**
* This constructor assigns the given weight to the member m_Weight.
* @param weight The weight of the particle.
*/
Particle(float weight = 0.0, int id = 0);
/**
* This constructor assigns the given weight to the member m_Weight.
* @param weight The weight of the particle.
*/
Particle(float weight = 0.0, int id = 0);
/**
* The destructor does nothing so far.
*/
virtual ~Particle();
/**
* The destructor does nothing so far.
*/
virtual ~Particle();
/**
* This method returns the importance factor of the particle.
* @return The importance factor (=weight) of the particle.
*/
inline float getWeight() const
{
return m_Weight;
}
/**
* This method returns the importance factor of the particle.
* @return The importance factor (=weight) of the particle.
*/
inline float getWeight() const { return m_Weight; }
/**
* Method to set the weight of the particle.
* @param newWeight New weight for the particle.
*/
inline void setWeight(float newWeight)
{
m_Weight = newWeight;
}
/**
* Method to set the weight of the particle.
* @param newWeight New weight for the particle.
*/
inline void setWeight(float newWeight) { m_Weight = newWeight; }
/**
* @return id of the particle that is stored in m_Id
*/
inline int getId()
{
return m_Id;
}
/**
* @return id of the particle that is stored in m_Id
*/
inline int getId() { return m_Id; }
private:
/**
* Stores the importance factor (=weight) of the particle. This should be a
* value between 0 and 1.
*/
float m_Weight;
/**
* Stores the importance factor (=weight) of the particle. This should be a
* value between 0 and 1.
*/
float m_Weight;
/**
* Stores the id of the particle (for testing purpose)
*/
int m_Id;
/**
* Stores the id of the particle (for testing purpose)
*/
int m_Id;
};
#endif
#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
#include <cmath>
#include <homer_nav_libs/Math/Transformation2D.h>
#include <iostream>
#include <limits.h>
#include <omp.h>
#include <sensor_msgs/LaserScan.h>
#include <cmath>
#include <iostream>
#include <ros/ros.h>
......@@ -31,329 +31,319 @@ const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
*
* @see Particle
*/
template <class ParticleType>
class ParticleFilter
template <class ParticleType> class ParticleFilter
{
public:
/**
* The constructor initializes the random number generator and allocates the
* memory for the particle lists.
* The lists will have particleNum elements.
* @param particleNum Number of particles for the filter.
*/
ParticleFilter<ParticleType>(int particleNum);
/**
* The destructor releases the particle lists.
*/
virtual ~ParticleFilter();
/**
* @return Number of particles used in this filter
*/
int getParticleNum();
/**
* @return The number of effective particles (according to "Improving
* Grid-based SLAM with Rao-Blackwellized Particle
* Filters by Adaptive Proposals and Selective Resampling (2005)" by Giorgio
* Grisetti, Cyrill Stachniss, Wolfram Burgard
*/
int getEffectiveParticleNum() const;
int getEffectiveParticleNum2() const;
/**
* @return Pointer to the particle that has the highest weight.
*/
ParticleType* getBestParticle() const;
/**
* The constructor initializes the random number generator and allocates the
* memory for the particle lists.
* The lists will have particleNum elements.
* @param particleNum Number of particles for the filter.
*/
ParticleFilter<ParticleType>(int particleNum);
/**
* The destructor releases the particle lists.
*/
virtual ~ParticleFilter();
/**
* @return Number of particles used in this filter
*/
int getParticleNum();
/**
* @return The number of effective particles (according to "Improving
* Grid-based SLAM with Rao-Blackwellized Particle
* Filters by Adaptive Proposals and Selective Resampling (2005)" by Giorgio
* Grisetti, Cyrill Stachniss, Wolfram Burgard
*/
int getEffectiveParticleNum() const;
int getEffectiveParticleNum2() const;
/**
* @return Pointer to the particle that has the highest weight.
*/
ParticleType* getBestParticle() const;
protected:
/**
* This method generates a random variable in the interval [0,1].
* @param init The initial value for the static random base number. When
* running the constructor of this
* class, this method is run once with the C-function time() as parameter to
* initialize it.
* Then you should use it without parameter.
* @return Random value between 0 and 1
*/
double random01(unsigned long init = 0) const;
/**
* This method sorts the particles in m_CurrentList from leftIndex to
* rightIndex according to their weight.
* The particle with the highest weight is at position 0 after calling this
* function. The algorithm used here is
* known as quicksort and works recursively.
* @param leftIndex Left index of area to sort
* @param rightIndex Right index of area to sort
*/
void sort(int leftIndex, int rightIndex);
/**
* This method normalizes the weights of the particles. After calling this
* function, the sum of the weights of
* all particles in m_CurrentList equals 1.0.
* In this function the sum of all weights of the particles of m_CurrentList
* is computed and each weight of each
* particle is devided through this sum.
*/
void normalize();
/**
* This method selects a new set of particles out of an old set according to
* their weight
* (importance resampling). The particles from the list m_CurrentList points
* to are used as source,
* m_LastList points to the destination list. The pointers m_CurrentList and
* m_LastList are switched.
* The higher the weight of a particle, the more particles are drawn
* (copied) from this particle.
* The weight remains untouched, because measure() will be called
* afterwards.
* This method only works on a sorted m_CurrentList, therefore sort() is
* called first.
*/
void resample();
/**
* This method drifts the particles (second step of a filter process).
* Has to be implemented in sub-classes (pure virtual function).
*/
virtual void drift(Transformation2D odoTrans) = 0;
/**
* This method has to be implemented in sub-classes. It is used to determine
* the weight of each particle.
*/
virtual void measure(sensor_msgs::LaserScanPtr laserData) = 0;
/**
* These two pointers point to m_ParticleListOne and to m_ParticleListTwo.
* The particles are drawn from m_LastList to m_CurrentList to avoid new and
* delete commands.
* In each run, the pointers are switched in resample().
*/
ParticleType** m_CurrentList;
ParticleType** m_LastList;
/**
* Stores the number of particles.
*/
int m_ParticleNum;
/**
* Stores the number of effective particles.
*/
int m_EffectiveParticleNum;
/**
* This method generates a random variable in the interval [0,1].
* @param init The initial value for the static random base number. When
* running the constructor of this
* class, this method is run once with the C-function time() as parameter to
* initialize it.
* Then you should use it without parameter.
* @return Random value between 0 and 1
*/
double random01(unsigned long init = 0) const;
/**
* This method sorts the particles in m_CurrentList from leftIndex to
* rightIndex according to their weight.
* The particle with the highest weight is at position 0 after calling this
* function. The algorithm used here is
* known as quicksort and works recursively.
* @param leftIndex Left index of area to sort
* @param rightIndex Right index of area to sort
*/
void sort(int leftIndex, int rightIndex);
/**
* This method normalizes the weights of the particles. After calling this
* function, the sum of the weights of
* all particles in m_CurrentList equals 1.0.
* In this function the sum of all weights of the particles of m_CurrentList
* is computed and each weight of each
* particle is devided through this sum.
*/
void normalize();
/**
* This method selects a new set of particles out of an old set according to
* their weight
* (importance resampling). The particles from the list m_CurrentList points
* to are used as source,
* m_LastList points to the destination list. The pointers m_CurrentList and
* m_LastList are switched.
* The higher the weight of a particle, the more particles are drawn
* (copied) from this particle.
* The weight remains untouched, because measure() will be called
* afterwards.
* This method only works on a sorted m_CurrentList, therefore sort() is
* called first.