Commit d566259b authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

wip

parent 7c9813c2
......@@ -69,9 +69,7 @@ target_link_libraries(
)
set(
ParticleFilter_SRC
src/ParticleFilter/SlamParticle.cpp
src/ParticleFilter/SlamFilter.cpp
src/ParticleFilter/Particle.cpp
)
add_library(
......
/homer_mapping/size: 4 # size of one edge of the map in m
/homer_mapping/resolution: 0.03 # m meter per cell
#map config values
/homer_mapping/backside_checking: true # Enable checking to avoid matching front- and backside of obstacles, e.g. walls. Useful when creating high resolution maps
/homer_mapping/obstacle_borders: true # Leaves a small border around obstacles unchanged when inserting a laser scan. Improves stability of generated map
/homer_mapping/measure_sampling_step: 0.1 # m Minimum distance in m between two samples for probability calculation
/homer_mapping/laser_scanner/free_reading_distance: 0.1 # Minimum distance in m to be classified as free in case of errorneous measurement
/homer_mapping/base_frame: "base_link"
/particlefilter/error_values/rotation_error_rotating: 20.0 # percent
/particlefilter/error_values/rotation_error_rotating: 50.0 # percent
/particlefilter/error_values/rotation_error_translating: 0.05 # degrees per meter
/particlefilter/error_values/translation_error_translating: 6.0 # percent
/particlefilter/error_values/translation_error_rotating: 0.02 # m per degree
/particlefilter/error_values/move_jitter_while_turning: 0.02 # m per degree
/particlefilter/error_values/translation_error_translating: 10.0 # percent
/particlefilter/error_values/move_jitter_while_turning: 0.05 # m per degree
/particlefilter/particle_num: 1000
/particlefilter/max_rotation_per_second: 0.1 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
......
#ifndef OCCUPANCYMAP_H
#define OCCUPANCYMAP_H
#pragma once
#include <Eigen/Geometry>
#include <cmath>
#include <fstream>
#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_nav_libs/Math/Box2D.h>
#include <homer_nav_libs/Math/Math.h>
#include <homer_nav_libs/Math/Point2D.h>
#include <homer_nav_libs/Math/Pose.h>
#include <iostream>
#include <list>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <sstream>
#include <string>
#include <tf/transform_listener.h>
#include <string>
#include <vector>
using namespace std;
/**
* Structure to store the start and end point of each laser range in the current
* scan
* @param sensorPos position of the laser in the current scan (in base_link
* frame)
* @param endPos position of the end point of the laser frame in the current
* scan (in base_link frame)
* @param free indicates if the laser range hit an obstacle (false) or not
* (true)
*/
struct RangeMeasurement
{
geometry_msgs::Point sensorPos;
geometry_msgs::Point endPos;
float range;
bool free;
};
/**
* Used in struct MeasurePoint to specify if a measurement point is at the
* border of a scan segment
*/
enum BorderType
{
NoBorder,
LeftBorder,
RightBorder
};
const float UNKNOWN_LIKELIHOOD = 0.3;
// Flags of current changes //
const char NO_CHANGE = 0;
const char OCCUPIED = 1;
const char FREE = 2;
// the safety border around occupied pixels which is left unchanged
const char SAFETY_BORDER = 3;
const char CONTRAST = 4;
///////////////////////////////
// assumed laser measure count for loaded maps
const int LOADED_MEASURECOUNT = 10;
/**
* Structure to store a measurement point for computeLaserScanProbability()
* @param hitPos Position of measured obstacle (robot coordinates)
* @param frontPos Position to check for NOT_KNOWN terrain
* This is needed to assure that front- and backside of
* obstacles can be distinguished
* @param border specifies if the measurement point is at the border of a scan
* segment
*/
struct MeasurePoint
{
Point2D hitPos;
Point2D frontPos;
BorderType borderType;
CVec2 normal;
};
#include <homer_mapping/ParticleFilter/SlamParticle.h>
/**
* @class OccupancyMap
......@@ -90,9 +18,8 @@ struct MeasurePoint
* @brief This class holds and manages an occupancy map.
*
* An occupancy map is a map where free space and occupied space are marked.
* This map stores values
* for free and occupied space in an (2D-)unsigned char array. This array can be
* seen as a graylevel image.
* This map stores values for free and occupied space in an (2D-)unsigned char array.
* This array can be seen as a graylevel image.
* The darker a cell, the higher the probability that this cell is occupied by
* an obstacle.
* The size of the map and the size of one cell can be defined in the setup file
......@@ -106,299 +33,34 @@ struct MeasurePoint
class OccupancyMap
{
public:
static const int8_t INACCESSIBLE = 100;
static const int8_t OBSTACLE = 99;
static const int8_t OCCUPIED = 98;
static const int8_t UNKNOWN = 50;
static const int8_t NOT_SEEN_YET = -1;
static const int8_t FREE = 0;
/**
* The default constructor calls initMembers().
*/
OccupancyMap();
/**
* Constructor for a loaded map.
*/
OccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
/**
* Copy constructor, copies all members inclusive the arrays that lie behind
* the pointers.
* @param occupancyMap Source for copying
*/
OccupancyMap(const OccupancyMap& occupancyMap);
/**
* Method to init all members with default values from the configuration
* file. All arrays are initialized.
*/
void initMembers();
/**
* Assignment operator, copies all members (deep copy!)
* @param source Source to copy from
* @return Reference to copied OccupancyMap
*/
OccupancyMap& operator=(const OccupancyMap& source);
/**
* Deletes all dynamically allocated memory.
*/
~OccupancyMap();
/**
* @return The resolution of the map in m.
*/
// int resolution() const;
geometry_msgs::Pose origin() const;
/**
* @return Width of the map.
*/
int width() const;
/**
* @return Height of the map.
*/
int height() const;
/**
* @return Probability of pixel p being occupied.
*/
float getOccupancyProbability(const Eigen::Vector2i& p) const;
/**
* @brief This function inserts the data of a laserscan into the map.
*
* With the given data, start and end cells of a laser beam are computed and
* given to the
* method markLineFree().
* If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is
* called for the endpoint.
* @param laserData The laser data msg.
*/
void insertLaserData(
sensor_msgs::LaserScan::ConstPtr laserData, const tf::Transform& transform);
void insertRanges(const vector<RangeMeasurement>& ranges, ros::Time stamp = ros::Time::now());
/**
* @brief gives a list specially processed coordinates to be used for
* computeLaserScanProbability
*/
std::vector<MeasurePoint> getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData) const;
/**
* This method computes a score that describes how good the given hypothesis
* matches with the map
* @param robotPose The pose of the robot
* @return The "fitting factor". The higher the factor, the better the
* fitting.
* This factor is NOT normalized, it is a positive float between 0
* and FLOAT_MAX
*/
float computeScore(const Pose& robotPose, const std::vector<MeasurePoint>& measurePoints) const;
/**
* Returns an "image" of occupancy probability image.
* @param[out] data vector containing occupancy probabilities. 0 = free, 100
* = occupied, -1 = NOT_KNOWN
* @param[out] width Width of data array
* @param[out] height Height of data array
* @param[out] resolution Resolution of the map (m_metaData.resolution)
*/
void getOccupancyProbabilityImage(vector<int8_t>& data, nav_msgs::MapMetaData& metaData) const;
/**
* This method marks free the position of the robot (according to its
* dimensions).
*/
void markRobotPositionFree();
/**
* @brief Computes the contrast of a single pixel.
* @param prob probability value (100=occupied, 50=NOT_KNOWN, 0=free) of a
* pixel.
* @return Contrast value from 0 (no contrast) to 1 (max. contrast) of this
* pixel
*/
double contrastFromProbability(int8_t prob);
/// GETTERS
Box2D<int> getExploredRegion() { return m_ExploredRegion; }
Box2D<int> getChangedRegion() { return m_ChangedRegion; }
void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down);
tf::StampedTransform getLaserTransform(std::string frame_id) const;
void insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData, const tf::Pose& pose);
void computeScores(std::vector<std::shared_ptr<SlamParticle>>& robotPoses, sensor_msgs::LaserScanConstPtr laserData) const;
nav_msgs::OccupancyGrid getMap() const;
protected:
/**
* This method increments m_MeasurementCount for pixel p.
* @param p Pixel that has been measured.
*/
void incrementMeasurementCount(const Eigen::Vector2i& p);
/**
* This method increments the occupancy count int m_OccupancyCount for pixel
* p.
* @param p Occupied pixel.
*/
void incrementOccupancyCount(const Eigen::Vector2i& p);
void changeMapSize(const tf::Point& min, const tf::Point& max);
/**
* This method increments m_MeasurementCount and if neccessary
* m_OccupancyCount for all pixels.
*/
void applyChanges();
tf::StampedTransform getLaserTransform(const std_msgs::Header& header) const;
void clearChanges();
void markFree(const Eigen::Vector2i& startPixel, const Eigen::Vector2i& endPixel);
/**
* This method scales the counts of all pixels down to the given value.
* @param maxCount Maximum value to which all counts are set.
*/
void scaleDownCounts(int maxCount);
/**
* This function paints a line from a start pixel to an end pixel.
* The computation is made with the Bresenham algorithm.
* @param data array on which the line shall be painted
* @param startPixel starting coordinates of the beam
* @param endPixel ending coordinates of the beam
* @param value The value with which the lines are marked.
*/
void drawLine(Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel, char value);
/**
* This method computes the values for m_OccupancyProbabilities from
* m_MeasurementCount and m_OccupancyCount.
*/
void computeOccupancyProbabilities();
/**
* This method sets all values of m_CurrentChanges to NO_CHANGE.
*/
void clearCurrentChanges();
/**
* This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
* and m_MaxChangeY.
* This means that no current changes are assumed.
*/
void resetChangedRegion();
/**
* This method updates the values of m_MinChangeX, m_MaxChangeX,
* m_MinChangeY and m_MaxChangeY to current changes.
* The area around the current robot pose will be included to the changed
* region.
* @param robotPose The current pose of the robot.
*/
void updateChangedRegion(Pose robotPose);
Eigen::Vector2i toMapCoords(const tf::Point& p) const;
/**
* This method sets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
* and m_MaxChangeY
* to initial values so that the complete map will be processed.
* Stores the map
*/
void maximizeChangedRegion();
/**
* This method resets all values of m_ExploredX, m_MaxExploredX,
* m_MinExploredY and m_MaxExploredY.
*/
void resetExploredRegion();
/**
* Stores the metadata of the map
*/
nav_msgs::MapMetaData m_metaData;
/**
* Stores the size of the map arrays, i.e. m_metaData.width *
* m_metaData.height
* for faster computation.
*/
unsigned m_ByteSize;
/**
* Array to store occupancy probability values.
* Values between 0 and 1.
*/
float* m_OccupancyProbability;
// Counts how often a pixel is hit by a measurement.
unsigned short* m_MeasurementCount;
// Counts how often a pixel is hit by a measurement that says the pixel is
// occupied.
unsigned short* m_OccupancyCount;
// Used for setting flags for cells, that have been modified during the
// current update.
unsigned char* m_CurrentChanges;
// Used for high Sensitive areas
unsigned short* m_HighSensitive;
struct PixelValue
{
float OccupancyProbability;
unsigned short MeasurementCount;
unsigned short OccupancyCount;
unsigned char CurrentChange;
PixelValue()
{
OccupancyProbability = UNKNOWN_LIKELIHOOD;
OccupancyCount = 0;
MeasurementCount = 0;
CurrentChange = NO_CHANGE;
}
};
std::vector<PixelValue> m_MapPoints;
/**
* Store values from config files.
*/
// minimum range classified as free in case of errorneous laser measurement
float m_FreeReadingDistance;
// enables checking to avoid matching front- and backside of an obstacle,
// e.g. wall
bool m_BacksideChecking;
// leaves a small border around obstacles unchanged when inserting a laser
// scan
bool m_ObstacleBorders;
// minimum distance in m between two samples for probability calculation
float m_MeasureSamplingStep;
/**
* Defines a bounding box around the changes in the current map.
*/
Box2D<int> m_ChangedRegion;
/**
* Defines a bounding box around the area in the map, which is already
* explored.
*/
Box2D<int> m_ExploredRegion;
nav_msgs::OccupancyGrid m_map;
/**
* ros transform listener
*/
tf::TransformListener m_tfListener;
/**
* ros transformation laser to base_link
*/
std::map<std::string, tf::StampedTransform> m_savedTransforms;
tf::Transform m_latestMapTransform;
std::map<std::string, tf::StampedTransform> m_transformCache;
};
#endif
#ifndef PARTICLE_H
#define PARTICLE_H
#include <fstream>
#include <iostream>
/**
* @class Particle
*
* @author Malte Knauf, Stephan Wirth
*
* @brief This class is an implementation of a "particle".
*
* A particle as it is used in particle filters is a set of one state and one
* importance factor (=weight).
* A set of Particles is a discrete representation of a probability
* distribution.
*
* @see ParticleFilter
*/
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);
/**
* 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; }
/**
* 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; }
private:
/**
* 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;
};
#endif
......@@ -80,15 +80,11 @@ protected:
double random01(unsigned long init = 0) const;
/**
* This method sorts the particles in m_CurrentList from leftIndex to
* rightIndex according to their weight.
* This method sorts the particles in m_CurrentList 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
* function.
*/
void sort(int leftIndex, int rightIndex);
void sort();
/**
* This method normalizes the weights of the particles. After calling this
......@@ -120,7 +116,7 @@ protected:
* 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;
virtual void drift(tf::Transform odoTrans) = 0;
/**
* This method has to be implemented in sub-classes. It is used to determine
......@@ -135,7 +131,6 @@ protected:
* In each run, the pointers are switched in resample().
*/
std::vector<std::shared_ptr<ParticleType>> m_CurrentList;
std::vector<std::shared_ptr<ParticleType>> m_LastList;
/**
* Stores the number of particles.
......@@ -152,7 +147,6 @@ template <class ParticleType> ParticleFilter<ParticleType>::ParticleFilter(int p
{
// initialize particle lists
m_CurrentList.resize(particleNum);
m_LastList.resize(particleNum);
// initialize random number generator
random01(time(0));
......@@ -182,76 +176,29 @@ double ParticleFilter<ParticleType>::random01(unsigned long init) const
return (double)(n / 2) / (double)LONG_MAX;
}
template <class ParticleType> void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight)
template <class ParticleType> void ParticleFilter<ParticleType>::sort()
{
// SOMETHING LEFT TO SORT?
if (indexLeft >= indexRight)
{
// ready!
return;
}
// CREATE PARTITION
int le = indexLeft;
int ri = indexRight;
int first = le;
int pivot = ri--;
while (le <= ri)
{
// skip from left
while (m_CurrentList[le]->getWeight() > m_CurrentList[pivot]->getWeight())
{
le++;
}
// skip from right
while (
(ri >= first) && (m_CurrentList[ri]->getWeight() <= m_CurrentList[pivot]->getWeight()))
{
ri--;
}
// now we have two elements to swap
if (le < ri)
{
// swap
std::shared_ptr<ParticleType> temp = m_CurrentList[le];
m_CurrentList[le] = m_CurrentList[ri];
m_CurrentList[ri] = temp;
le++;
}
}
if (le != pivot)
{
// swap
std::shared_ptr<ParticleType> temp = m_CurrentList[le];
m_CurrentList[le] = m_CurrentList[pivot];
m_CurrentList[pivot] = temp;
}
// sort left side
sort(indexLeft, le - 1);
// sort right side
sort(le + 1, indexRight);
std::sort(m_CurrentList.begin(), m_CurrentList.end(),
[](const std::shared_ptr<ParticleType> a, const std::shared_ptr<ParticleType> b) {